Skip to content

Commit

Permalink
Global: change Device::PeriodicCb signature
Browse files Browse the repository at this point in the history
Remove bool return as it's never being used and not supported on PX4.
  • Loading branch information
lucasdemarchi authored and tridge committed Jan 13, 2017
1 parent 5aa4bc4 commit 5472bc4
Show file tree
Hide file tree
Showing 64 changed files with 135 additions and 185 deletions.
12 changes: 5 additions & 7 deletions libraries/AP_ADC/AP_ADC_ADS1115.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ bool AP_ADC_ADS1115::init()

_gain = ADS1115_PGA_4P096;

_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, bool));
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, void));

return true;
}
Expand Down Expand Up @@ -200,23 +200,23 @@ float AP_ADC_ADS1115::_convert_register_data_to_mv(int16_t word) const
return (float) word * pga;
}

bool AP_ADC_ADS1115::_update()
void AP_ADC_ADS1115::_update()
{
uint8_t config[2];
be16_t val;

if (!_dev->read_registers(ADS1115_RA_CONFIG, config, sizeof(config))) {
error("_dev->read_registers failed in ADS1115");
return true;
return;
}

/* check rdy bit */
if ((config[1] & 0x80) != 0x80 ) {
return true;
return;
}

if (!_dev->read_registers(ADS1115_RA_CONVERSION, (uint8_t *)&val, sizeof(val))) {
return true;
return;
}

float sample = _convert_register_data_to_mv(be16toh(val));
Expand All @@ -227,6 +227,4 @@ bool AP_ADC_ADS1115::_update()
/* select next channel */
_channel_to_read = (_channel_to_read + 1) % _channels_number;
_start_conversion(_channel_to_read);

return true;
}
2 changes: 1 addition & 1 deletion libraries/AP_ADC/AP_ADC_ADS1115.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class AP_ADC_ADS1115
int _channel_to_read;
adc_report_s *_samples;

bool _update();
void _update();
bool _start_conversion(uint8_t channel);

float _convert_register_data_to_mv(int16_t word) const;
Expand Down
7 changes: 3 additions & 4 deletions libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ bool AP_Airspeed_MS4525::init()
_dev->set_retries(2);

_dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool));
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, void));
return true;
}

Expand Down Expand Up @@ -173,18 +173,17 @@ void AP_Airspeed_MS4525::_voltage_correction(float &diff_press_pa, float &temper
}

// 50Hz timer
bool AP_Airspeed_MS4525::_timer()
void AP_Airspeed_MS4525::_timer()
{
if (_measurement_started_ms == 0) {
_measure();
return true;
return;
}
if ((AP_HAL::millis() - _measurement_started_ms) > 10) {
_collect();
// start a new measurement
_measure();
}
return true;
}

// return the current differential_pressure in Pascal
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Airspeed/AP_Airspeed_MS4525.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ class AP_Airspeed_MS4525 : public AP_Airspeed_Backend
private:
void _measure();
void _collect();
bool _timer();
void _timer();
void _voltage_correction(float &diff_press_pa, float &temperature);

float _temp_sum;
Expand Down
8 changes: 3 additions & 5 deletions libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ bool AP_Airspeed_MS5525::init()

// read at 80Hz
dev->register_periodic_callback(1000000UL/80U,
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS5525::timer, bool));
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS5525::timer, void));
return true;
}

Expand Down Expand Up @@ -219,7 +219,7 @@ void AP_Airspeed_MS5525::calculate(void)
}

// 50Hz timer
bool AP_Airspeed_MS5525::timer()
void AP_Airspeed_MS5525::timer()
{
uint32_t adc_val = read_adc();

Expand All @@ -241,12 +241,10 @@ bool AP_Airspeed_MS5525::timer()

uint8_t next_cmd = next_state == 0 ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
if (!dev->transfer(&next_cmd, 1, nullptr, 0)) {
return true;
return;
}

state = next_state;

return true;
}

// return the current differential_pressure in Pascal
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Airspeed/AP_Airspeed_MS5525.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ class AP_Airspeed_MS5525 : public AP_Airspeed_Backend
private:
void measure();
void collect();
bool timer();
void timer();
bool read_prom(void);
uint16_t crc4_prom(void);
int32_t read_adc();
Expand Down
7 changes: 3 additions & 4 deletions libraries/AP_Baro/AP_Baro_BMP085.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,16 +81,16 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice>

sem->give();

_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, bool));
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void));
}

/*
This is a state machine. Acumulate a new sensor reading.
*/
bool AP_Baro_BMP085::_timer(void)
void AP_Baro_BMP085::_timer(void)
{
if (!_data_ready()) {
return true;
return;
}

if (_state == 0) {
Expand All @@ -106,7 +106,6 @@ bool AP_Baro_BMP085::_timer(void)
} else {
_cmd_read_pressure();
}
return true;
}

/*
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_Baro/AP_Baro_BMP085.h
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@ class AP_Baro_BMP085 : public AP_Baro_Backend
void _calculate();
bool _data_ready();

bool _timer(void);
void _timer(void);

AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
AP_HAL::DigitalSource *_eoc;

Expand Down
12 changes: 5 additions & 7 deletions libraries/AP_Baro/AP_Baro_MS5611.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ bool AP_Baro_MS56XX::_init()

/* Request 100Hz update */
_dev->register_periodic_callback(10 * USEC_PER_MSEC,
FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, bool));
FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, void));
return true;
}

Expand Down Expand Up @@ -259,7 +259,7 @@ bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
* as fast as pressure. Hence we reuse the same temperature for 4 samples of
* pressure.
*/
bool AP_Baro_MS56XX::_timer(void)
void AP_Baro_MS56XX::_timer(void)
{
uint8_t next_cmd;
uint8_t next_state;
Expand All @@ -278,21 +278,21 @@ bool AP_Baro_MS56XX::_timer(void)
next_cmd = next_state == 0 ? ADDR_CMD_CONVERT_TEMPERATURE
: ADDR_CMD_CONVERT_PRESSURE;
if (!_dev->transfer(&next_cmd, 1, nullptr, 0)) {
return true;
return;
}

/* if we had a failed read we are all done */
if (adc_val == 0) {
// a failed read can mean the next returned value will be
// corrupt, we must discard it
_discard_next = true;
return true;
return;
}

if (_discard_next) {
_discard_next = false;
_state = next_state;
return true;
return;
}

if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
Expand All @@ -306,8 +306,6 @@ bool AP_Baro_MS56XX::_timer(void)
_sem->give();
_state = next_state;
}

return true;
}

void AP_Baro_MS56XX::_update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Baro/AP_Baro_MS5611.h
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class AP_Baro_MS56XX : public AP_Baro_Backend
uint16_t _read_prom_word(uint8_t word);
uint32_t _read_adc();

bool _timer();
void _timer();

AP_HAL::OwnPtr<AP_HAL::Device> _dev;

Expand Down
5 changes: 2 additions & 3 deletions libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ AP_BattMonitor_SMBus_I2C::AP_BattMonitor_SMBus_I2C(AP_BattMonitor &mon, uint8_t
: AP_BattMonitor_SMBus(mon, instance, mon_state)
, _dev(std::move(dev))
{
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_I2C::timer, bool));
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_SMBus_I2C::timer, void));
}

/// Read the battery voltage and current. Should be called at 10hz
Expand All @@ -39,7 +39,7 @@ void AP_BattMonitor_SMBus_I2C::read()
// nothing to do - all done in timer()
}

bool AP_BattMonitor_SMBus_I2C::timer()
void AP_BattMonitor_SMBus_I2C::timer()
{
uint16_t data;
uint8_t buff[4];
Expand All @@ -62,7 +62,6 @@ bool AP_BattMonitor_SMBus_I2C::timer()
if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
_state.healthy = false;
}
return true;
}

// read word from register
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class AP_BattMonitor_SMBus_I2C : public AP_BattMonitor_SMBus

private:

bool timer(void);
void timer(void);

// read word from register
// returns true if read was successful, false if failed
Expand Down
7 changes: 3 additions & 4 deletions libraries/AP_Compass/AP_Compass_AK09916.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ bool AP_Compass_AK09916::init()

// call timer() at 100Hz
dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::timer, bool));
FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::timer, void));

return true;

Expand All @@ -112,7 +112,7 @@ bool AP_Compass_AK09916::init()
return false;
}

bool AP_Compass_AK09916::timer()
void AP_Compass_AK09916::timer()
{
struct PACKED {
int16_t magx;
Expand All @@ -131,7 +131,7 @@ bool AP_Compass_AK09916::timer()
if (!dev->read_registers(REG_ST1, &st1, 1) || !(st1 & 1)) {
goto check_registers;
}

if (!dev->read_registers(REG_HXL, (uint8_t *)&data, sizeof(data))) {
goto check_registers;
}
Expand All @@ -155,7 +155,6 @@ bool AP_Compass_AK09916::timer()

check_registers:
dev->check_next_register();
return true;
}

void AP_Compass_AK09916::read()
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Compass/AP_Compass_AK09916.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class AP_Compass_AK09916 : public AP_Compass_Backend
* Device periodic callback to read data from the sensor.
*/
bool init();
bool timer();
void timer();

uint8_t compass_instance;
Vector3f accum;
Expand Down
13 changes: 5 additions & 8 deletions libraries/AP_Compass/AP_Compass_AK8963.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,7 @@ bool AP_Compass_AK8963::init()
bus_sem->give();

/* timer needs to be called every 10ms so set the freq_div to 10 */
if (!_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, bool))) {
if (!_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void))) {
// fallback to timer
_timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void), 10);
}
Expand Down Expand Up @@ -215,26 +215,26 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co
field.z *= _magnetometer_ASA[2];
}

bool AP_Compass_AK8963::_update()
void AP_Compass_AK8963::_update()
{
struct sample_regs regs;
Vector3f raw_field;
uint32_t time_us = AP_HAL::micros();

if (!_bus->block_read(AK8963_HXL, (uint8_t *) &regs, sizeof(regs))) {
goto fail;
return;
}

/* Check for overflow. See AK8963's datasheet, section
* 6.4.3.6 - Magnetic Sensor Overflow. */
if ((regs.st2 & 0x08)) {
goto fail;
return;
}

raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);

if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
goto fail;
return;
}

_make_factory_sensitivity_adjustment(raw_field);
Expand Down Expand Up @@ -263,9 +263,6 @@ bool AP_Compass_AK8963::_update()
}
_sem->give();
}

fail:
return true;
}

/*
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Compass/AP_Compass_AK8963.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ class AP_Compass_AK8963 : public AP_Compass_Backend
bool _check_id();
bool _calibrate();

bool _update();
void _update();
void _update_timer();

AP_AK8963_BusDriver *_bus;
Expand Down
Loading

0 comments on commit 5472bc4

Please sign in to comment.