diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index 4bc5d8af5f..b098c889a5 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -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; } @@ -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)); @@ -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; } diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.h b/libraries/AP_ADC/AP_ADC_ADS1115.h index 2f49a9dc8a..98e6544875 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.h +++ b/libraries/AP_ADC/AP_ADC_ADS1115.h @@ -36,7 +36,7 @@ private: 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; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp index 4905945814..9f227fc35f 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp @@ -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; } @@ -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 diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h index b89edcc69e..cb3208cf21 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h @@ -45,7 +45,7 @@ public: private: void _measure(); void _collect(); - bool _timer(); + void _timer(); void _voltage_correction(float &diff_press_pa, float &temperature); float _temp_sum; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp index 1c8f8e71b2..75f0315c39 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp @@ -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; } @@ -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(); @@ -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 diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h index c4cdd5c213..e5e227a4f7 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h @@ -45,7 +45,7 @@ public: private: void measure(); void collect(); - bool timer(); + void timer(); bool read_prom(void); uint16_t crc4_prom(void); int32_t read_adc(); diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 4873e0ebec..95ec3c92dc 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -81,16 +81,16 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr 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) { @@ -106,7 +106,6 @@ bool AP_Baro_BMP085::_timer(void) } else { _cmd_read_pressure(); } - return true; } /* diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index cdc3de3cdf..7bf345cdec 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -23,8 +23,8 @@ private: void _calculate(); bool _data_ready(); - bool _timer(void); - + void _timer(void); + AP_HAL::OwnPtr _dev; AP_HAL::DigitalSource *_eoc; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 90cbb262ef..5088bc5e06 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -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; } @@ -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; @@ -278,7 +278,7 @@ 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 */ @@ -286,13 +286,13 @@ bool AP_Baro_MS56XX::_timer(void) // 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)) { @@ -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, diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index e230f4591a..d74b136003 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -46,7 +46,7 @@ private: uint16_t _read_prom_word(uint8_t word); uint32_t _read_adc(); - bool _timer(); + void _timer(); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp index 0b0ae6da88..25e57f0c2f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.cpp @@ -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 @@ -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]; @@ -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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h index a84e8aabf8..dc92e3259f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_I2C.h @@ -23,7 +23,7 @@ public: private: - bool timer(void); + void timer(void); // read word from register // returns true if read was successful, false if failed diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index d11ea802db..439a04d068 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -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; @@ -112,7 +112,7 @@ fail: return false; } -bool AP_Compass_AK09916::timer() +void AP_Compass_AK09916::timer() { struct PACKED { int16_t magx; @@ -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; } @@ -155,7 +155,6 @@ bool AP_Compass_AK09916::timer() check_registers: dev->check_next_register(); - return true; } void AP_Compass_AK09916::read() diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index a9a7e4d235..45541ed00b 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -49,7 +49,7 @@ private: * Device periodic callback to read data from the sensor. */ bool init(); - bool timer(); + void timer(); uint8_t compass_instance; Vector3f accum; diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 87089122f4..e865bfac9f 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -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); } @@ -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 *) ®s, 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); @@ -263,9 +263,6 @@ bool AP_Compass_AK8963::_update() } _sem->give(); } - -fail: - return true; } /* diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 5a75a56b89..518351ee42 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -50,7 +50,7 @@ private: bool _check_id(); bool _calibrate(); - bool _update(); + void _update(); void _update_timer(); AP_AK8963_BusDriver *_bus; diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index 48194ea2a5..cd919be70b 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -190,7 +190,7 @@ bool AP_Compass_BMM150::init() set_dev_id(_compass_instance, _dev->get_bus_id()); _dev->register_periodic_callback(MEASURE_TIME_USEC, - FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, bool)); + FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void)); return true; @@ -237,7 +237,7 @@ int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) return constrain_int32(dividend / divisor, -0x8000, 0x8000); } -bool AP_Compass_BMM150::_update() +void AP_Compass_BMM150::_update() { const uint32_t time_usec = AP_HAL::micros(); @@ -246,7 +246,7 @@ bool AP_Compass_BMM150::_update() /* Checking data ready status */ if (!ret || !(data[3] & 0x1)) { - return true; + return; } const uint16_t rhall = le16toh(data[3] >> 2); @@ -273,7 +273,7 @@ bool AP_Compass_BMM150::_update() correct_field(raw_field, _compass_instance); if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return true; + return; } _mag_accum += raw_field; _accum_count++; @@ -282,7 +282,6 @@ bool AP_Compass_BMM150::_update() _accum_count = 5; } _sem->give(); - return true; } void AP_Compass_BMM150::read() diff --git a/libraries/AP_Compass/AP_Compass_BMM150.h b/libraries/AP_Compass/AP_Compass_BMM150.h index de24c6d4da..83b95fad9d 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.h +++ b/libraries/AP_Compass/AP_Compass_BMM150.h @@ -41,7 +41,7 @@ private: * Device periodic callback to read data from the sensor. */ bool init(); - bool _update(); + void _update(); bool _load_trim_values(); int16_t _compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2); int16_t _compensate_z(int16_t z, uint32_t rhall); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 5cff49b5c6..a151e98728 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -209,7 +209,7 @@ bool AP_Compass_HMC5843::init() // read from sensor at 75Hz _bus->register_periodic_callback(13333, - FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, void)); hal.console->printf("HMC5843 found on bus 0x%x\n", _bus->get_bus_id()); @@ -225,7 +225,7 @@ errout: * * bus semaphore has been taken already by HAL */ -bool AP_Compass_HMC5843::_timer() +void AP_Compass_HMC5843::_timer() { bool result = _read_sample(); @@ -233,7 +233,7 @@ bool AP_Compass_HMC5843::_timer() _take_sample(); if (!result) { - return true; + return; } uint32_t tnow = AP_HAL::micros(); @@ -274,8 +274,6 @@ bool AP_Compass_HMC5843::_timer() } _sem->give(); } - - return true; } /* diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 3197bb0b43..11c4cca793 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -39,8 +39,8 @@ private: bool _calibrate(); bool _setup_sampling_mode(); - bool _timer(); - + void _timer(); + /* Read a single sample */ bool _read_sample(); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index 264d95e51b..2bd63b567b 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -111,7 +111,7 @@ bool AP_Compass_IST8310::init() set_dev_id(_instance, _dev->get_bus_id()); _dev->register_periodic_callback(10 * USEC_PER_MSEC, - FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void)); return true; @@ -126,9 +126,8 @@ void AP_Compass_IST8310::start_conversion() SINGLE_MEASUREMENT_MODE); } -bool AP_Compass_IST8310::timer() +void AP_Compass_IST8310::timer() { - bool ret = false; struct PACKED { @@ -142,14 +141,14 @@ bool AP_Compass_IST8310::timer() ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer)); if (!ret) { /* We're going to be back on the next iteration either way */ - return true; + return; } auto status = buffer.status; if (!(status & 0x01)) { /* We're not ready yet */ - return true; + return; } auto x = static_cast(le16toh(buffer.rx)); @@ -175,8 +174,6 @@ bool AP_Compass_IST8310::timer() } start_conversion(); - - return true; } void AP_Compass_IST8310::read() diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h index 7927096475..2ee0cf0c5c 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.h +++ b/libraries/AP_Compass/AP_Compass_IST8310.h @@ -41,7 +41,7 @@ private: AP_HAL::OwnPtr dev, enum Rotation rotation); - bool timer(); + void timer(); bool init(); void start_conversion(); diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 12c7381364..15d6ef6ef6 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -125,7 +125,7 @@ bool AP_Compass_LIS3MDL::init() // call timer() at 155Hz dev->register_periodic_callback(1000000U/155U, - FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void)); return true; @@ -134,7 +134,7 @@ fail: return false; } -bool AP_Compass_LIS3MDL::timer() +void AP_Compass_LIS3MDL::timer() { struct PACKED { int16_t magx; @@ -177,7 +177,6 @@ bool AP_Compass_LIS3MDL::timer() check_registers: dev->check_next_register(); - return true; } void AP_Compass_LIS3MDL::read() diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.h b/libraries/AP_Compass/AP_Compass_LIS3MDL.h index 18e5027704..dc3255e41a 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.h +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.h @@ -50,7 +50,7 @@ private: * Device periodic callback to read data from the sensor. */ bool init(); - bool timer(); + void timer(); uint8_t compass_instance; Vector3f accum; diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index a5c306a00f..a3e6e10420 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -284,7 +284,7 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation) #endif // read at 100Hz - _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, bool)); + _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void)); return true; } @@ -343,10 +343,10 @@ fail_whoami: return false; } -bool AP_Compass_LSM303D::_update() +void AP_Compass_LSM303D::_update() { if (!_read_sample()) { - return true; + return; } Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale; @@ -373,7 +373,6 @@ bool AP_Compass_LSM303D::_update() } _sem->give(); } - return true; } // Read Sensor data diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.h b/libraries/AP_Compass/AP_Compass_LSM303D.h index beb6934175..dcc263ba76 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.h +++ b/libraries/AP_Compass/AP_Compass_LSM303D.h @@ -34,7 +34,7 @@ private: bool _data_ready(); bool _hardware_init(); - bool _update(); + void _update(); void _disable_i2c(); bool _mag_set_range(uint8_t max_ga); bool _mag_set_samplerate(uint16_t frequency); diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index fbc5e2e915..be03506aae 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -113,7 +113,7 @@ bool AP_Compass_LSM9DS1::init() _dev->set_device_type(DEVTYPE_LSM9DS1); set_dev_id(_compass_instance, _dev->get_bus_id()); - _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, bool)); + _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void)); bus_sem->give(); @@ -137,24 +137,24 @@ void AP_Compass_LSM9DS1::_dump_registers() hal.console->println(); } -bool AP_Compass_LSM9DS1::_update(void) +void AP_Compass_LSM9DS1::_update(void) { struct sample_regs regs; Vector3f raw_field; uint32_t time_us = AP_HAL::micros(); if (!_block_read(LSM9DS1M_STATUS_REG_M, (uint8_t *) ®s, sizeof(regs))) { - goto fail; + return; } if (regs.status & 0x80) { - 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; } raw_field *= _scaling; @@ -181,9 +181,6 @@ bool AP_Compass_LSM9DS1::_update(void) } _sem->give(); } - -fail: - return true; } void AP_Compass_LSM9DS1::read() diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index 4a68b06445..db860252f5 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h @@ -28,7 +28,7 @@ private: bool _check_id(void); bool _configure(void); bool _set_scale(void); - bool _update(void); + void _update(void); uint8_t _register_read(uint8_t reg); void _register_write(uint8_t reg, uint8_t val); diff --git a/libraries/AP_HAL/Device.h b/libraries/AP_HAL/Device.h index 8121be0f2d..8a512849bf 100644 --- a/libraries/AP_HAL/Device.h +++ b/libraries/AP_HAL/Device.h @@ -38,7 +38,7 @@ public: SPEED_LOW, }; - FUNCTOR_TYPEDEF(PeriodicCb, bool); + FUNCTOR_TYPEDEF(PeriodicCb, void); typedef void* PeriodicHandle; Device(enum BusType type) diff --git a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp index bf50bb3a36..a24701a6eb 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp @@ -81,10 +81,10 @@ void AnalogIn_Raspilot::init() return; } - _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, bool)); + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, void)); } -bool AnalogIn_Raspilot::_update() +void AnalogIn_Raspilot::_update() { struct IOPacket tx = { }, rx = { }; uint16_t count = RASPILOT_ADC_MAX_CHANNELS; @@ -120,5 +120,4 @@ bool AnalogIn_Raspilot::_update() } } } - return true; } diff --git a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h index db48ff469e..5ee582fdf7 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h +++ b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h @@ -35,7 +35,7 @@ public: float board_voltage(void); protected: - bool _update(); + void _update(); AP_HAL::AnalogSource *_vcc_pin_analog_source; AnalogSource_Raspilot *_channels[RASPILOT_ADC_MAX_CHANNELS]; diff --git a/libraries/AP_HAL_Linux/PollerThread.cpp b/libraries/AP_HAL_Linux/PollerThread.cpp index 8b8302b626..ce505f1e6a 100644 --- a/libraries/AP_HAL_Linux/PollerThread.cpp +++ b/libraries/AP_HAL_Linux/PollerThread.cpp @@ -41,9 +41,7 @@ void TimerPollable::on_can_read() _wrapper->start_cb(); } - if (!_cb()) { - _removeme = true; - } + _cb(); if (_wrapper) { _wrapper->end_cb(); diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp index edffcab341..1d5ab68f0b 100644 --- a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp +++ b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp @@ -24,10 +24,10 @@ void RCInput_Raspilot::init() _dev = hal.spi->get_device("raspio"); // start the timer process to read samples - _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCInput_Raspilot::_poll_data, bool)); + _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCInput_Raspilot::_poll_data, void)); } -bool RCInput_Raspilot::_poll_data(void) +void RCInput_Raspilot::_poll_data(void) { struct IOPacket _dma_packet_tx, _dma_packet_rx; uint16_t count = LINUX_RC_INPUT_NUM_CHANNELS; @@ -52,8 +52,6 @@ bool RCInput_Raspilot::_poll_data(void) if ( rc_ok && (rx_crc == crc_packet(&_dma_packet_rx)) ) { _update_periods(&_dma_packet_rx.regs[6], (uint8_t)num_values); } - - return true; } #endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.h b/libraries/AP_HAL_Linux/RCInput_Raspilot.h index 16fb077f56..fc9a8670ea 100644 --- a/libraries/AP_HAL_Linux/RCInput_Raspilot.h +++ b/libraries/AP_HAL_Linux/RCInput_Raspilot.h @@ -15,7 +15,7 @@ public: private: AP_HAL::OwnPtr _dev; - bool _poll_data(void); + void _poll_data(void); }; } diff --git a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp index dea9c68b54..223217ba78 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp @@ -27,7 +27,7 @@ void RCOutput_Raspilot::init() { _dev = hal.spi->get_device("raspio"); - _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, bool)); + _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, void)); } void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz) @@ -74,12 +74,12 @@ void RCOutput_Raspilot::read(uint16_t* period_us, uint8_t len) period_us[i] = read(0 + i); } -bool RCOutput_Raspilot::_update(void) +void RCOutput_Raspilot::_update(void) { int i; if (_corked) { - return true; + return; } if (_new_frequency) { @@ -129,7 +129,6 @@ bool RCOutput_Raspilot::_update(void) _dma_packet_tx.crc = crc_packet(&_dma_packet_tx); _dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx), (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx)); - return true; } void RCOutput_Raspilot::cork(void) diff --git a/libraries/AP_HAL_Linux/RCOutput_Raspilot.h b/libraries/AP_HAL_Linux/RCOutput_Raspilot.h index 179ae62199..e8529ea918 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Raspilot.h +++ b/libraries/AP_HAL_Linux/RCOutput_Raspilot.h @@ -20,7 +20,7 @@ public: private: void reset(); - bool _update(void); + void _update(void); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp index f44fceca42..d80b566558 100644 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp @@ -66,7 +66,7 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) if (!_registered_callback) { _dev = hal.spi->get_device("raspio"); _registered_callback = true; - _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&RPIOUARTDriver::_bus_timer, bool)); + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&RPIOUARTDriver::_bus_timer, void)); } /* set baudrate */ @@ -105,7 +105,7 @@ void RPIOUARTDriver::_timer_tick(void) } } -bool RPIOUARTDriver::_bus_timer(void) +void RPIOUARTDriver::_bus_timer(void) { /* set the baudrate of raspilotio */ if (_need_set_baud) { @@ -129,7 +129,7 @@ bool RPIOUARTDriver::_bus_timer(void) /* finish set */ if (!_initialised) { - return true; + return; } _in_timer = true; @@ -172,6 +172,4 @@ bool RPIOUARTDriver::_bus_timer(void) } _in_timer = false; - - return true; } diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.h b/libraries/AP_HAL_Linux/RPIOUARTDriver.h index 9a4df0f4d2..e1b119bf7b 100644 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.h +++ b/libraries/AP_HAL_Linux/RPIOUARTDriver.h @@ -26,7 +26,7 @@ protected: private: bool _in_timer; - bool _bus_timer(void); + void _bus_timer(void); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp index eea4776b6d..46f1ab1d33 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.cpp +++ b/libraries/AP_IRLock/AP_IRLock_I2C.cpp @@ -44,7 +44,7 @@ void AP_IRLock_I2C::init() // read at 50Hz printf("Starting IRLock on I2C\n"); - dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_IRLock_I2C::read_frames, bool)); + dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_IRLock_I2C::read_frames, void)); } /* @@ -108,15 +108,15 @@ bool AP_IRLock_I2C::read_block(struct frame &irframe) return true; } -bool AP_IRLock_I2C::read_frames(void) +void AP_IRLock_I2C::read_frames(void) { if (!sync_frame_start()) { - return true; + return; } struct frame irframe; if (!read_block(irframe)) { - return true; + return; } int16_t corner1_pix_x = irframe.pixel_x - irframe.pixel_size_x/2; @@ -148,8 +148,6 @@ bool AP_IRLock_I2C::read_frames(void) _target_info.size_x, _target_info.size_y); } #endif - - return true; } // retrieve latest sensor data - returns true if new data is available diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.h b/libraries/AP_IRLock/AP_IRLock_I2C.h index dcb17004ac..80238a41df 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.h +++ b/libraries/AP_IRLock/AP_IRLock_I2C.h @@ -31,7 +31,7 @@ private: bool sync_frame_start(void); bool read_block(struct frame &irframe); - bool read_frames(void); + void read_frames(void); void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index f5305c0cc2..94ee571b11 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -185,7 +185,7 @@ void AP_InertialSensor_BMI160::start() /* Call _poll_data() at 1kHz */ _dev->register_periodic_callback(1000, - FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, bool)); + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, void)); } bool AP_InertialSensor_BMI160::update() @@ -417,10 +417,9 @@ read_fifo_end: } } -bool AP_InertialSensor_BMI160::_poll_data() +void AP_InertialSensor_BMI160::_poll_data() { _read_fifo(); - return true; } bool AP_InertialSensor_BMI160::_hardware_init() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h index 240731b72a..332910f39f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h @@ -98,7 +98,7 @@ private: /** * Device periodic callback to read data from the sensors. */ - bool _poll_data(); + void _poll_data(); /** * Read samples from fifo. diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 1e4ff3a818..2f8376d31c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -463,7 +463,7 @@ void AP_InertialSensor_Invensense::start() } // start the timer process to read samples - _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, bool)); + _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void)); } @@ -519,10 +519,9 @@ bool AP_InertialSensor_Invensense::_data_ready() /* * Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held */ -bool AP_InertialSensor_Invensense::_poll_data() +void AP_InertialSensor_Invensense::_poll_data() { _read_fifo(); - return true; } bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 4615946050..54b36342dd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -83,7 +83,7 @@ private: bool _data_ready(); /* Poll for new data (non-blocking) */ - bool _poll_data(); + void _poll_data(); /* Read and write functions taking the differences between buses into * account */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 6dfe709ca0..31f316139d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -211,7 +211,7 @@ void AP_InertialSensor_L3G4200D::start(void) _accel_instance = _imu.register_accel(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D)); // start the timer process to read samples - _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool)); + _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void)); } /* @@ -226,7 +226,7 @@ bool AP_InertialSensor_L3G4200D::update(void) } // Accumulate values from accels and gyros -bool AP_InertialSensor_L3G4200D::_accumulate(void) +void AP_InertialSensor_L3G4200D::_accumulate(void) { uint8_t num_samples_available; uint8_t fifo_status = 0; @@ -279,8 +279,7 @@ bool AP_InertialSensor_L3G4200D::_accumulate(void) _notify_new_accel_raw_sample(_accel_instance, accel); } } - } - return true; + } } #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index f3728e0288..02da026176 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -28,7 +28,7 @@ public: private: bool _init_sensor(); - bool _accumulate(); + void _accumulate(); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 5a17fb4d61..700acd8881 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -528,7 +528,7 @@ void AP_InertialSensor_LSM9DS0::start(void) _set_accel_max_abs_offset(_accel_instance, 5.0f); /* start the timer process to read samples */ - _dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, bool)); + _dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void)); } @@ -685,7 +685,7 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale) /** * Timer process to poll for new data from the LSM9DS0. */ -bool AP_InertialSensor_LSM9DS0::_poll_data() +void AP_InertialSensor_LSM9DS0::_poll_data() { if (_gyro_data_ready()) { _read_data_transaction_g(); @@ -701,8 +701,6 @@ bool AP_InertialSensor_LSM9DS0::_poll_data() if (!_dev_accel->check_next_register()) { _inc_accel_error_count(_accel_instance); } - - return true; } bool AP_InertialSensor_LSM9DS0::_accel_data_ready() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h index 49373e1d93..d0f0e758c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -52,7 +52,7 @@ private: bool _accel_data_ready(); bool _gyro_data_ready(); - bool _poll_data(); + void _poll_data(); bool _init_sensor(); bool _hardware_init(); diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp index 3b283496aa..7c417a2f08 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.cpp +++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp @@ -51,7 +51,7 @@ bool Display_SSD1306_I2C::hw_init() _need_hw_update = true; - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_timer, bool)); + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_timer, void)); return true; } @@ -62,10 +62,10 @@ bool Display_SSD1306_I2C::hw_update() return true; } -bool Display_SSD1306_I2C::_timer() +void Display_SSD1306_I2C::_timer() { if (!_need_hw_update) { - return true; + return; } _need_hw_update = false; @@ -88,8 +88,6 @@ bool Display_SSD1306_I2C::_timer() memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_ROWS], SSD1306_ROWS); _dev->transfer((uint8_t *)&display_buffer, sizeof(display_buffer), nullptr, 0); } - - return true; } bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y) diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h index 94b8770833..2568af931b 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.h +++ b/libraries/AP_Notify/Display_SSD1306_I2C.h @@ -18,5 +18,5 @@ private: AP_HAL::OwnPtr _dev; uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE]; bool _need_hw_update; - bool _timer(void); + void _timer(void); }; diff --git a/libraries/AP_Notify/NavioLED_I2C.cpp b/libraries/AP_Notify/NavioLED_I2C.cpp index 3863f855e9..0c4770eae2 100644 --- a/libraries/AP_Notify/NavioLED_I2C.cpp +++ b/libraries/AP_Notify/NavioLED_I2C.cpp @@ -32,7 +32,7 @@ bool NavioLED_I2C::hw_init() return false; } - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NavioLED_I2C::_timer, bool)); + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NavioLED_I2C::_timer, void)); return true; } @@ -47,10 +47,10 @@ bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) return true; } -bool NavioLED_I2C::_timer(void) +void NavioLED_I2C::_timer(void) { if (!_need_update) { - return true; + return; } _need_update = false; @@ -73,6 +73,4 @@ bool NavioLED_I2C::_timer(void) 0x00, 0x00, red_channel_lsb, red_channel_msb}; _dev->transfer(transaction, sizeof(transaction), nullptr, 0); - - return true; } diff --git a/libraries/AP_Notify/NavioLED_I2C.h b/libraries/AP_Notify/NavioLED_I2C.h index ab57895e77..ca3f9662e6 100644 --- a/libraries/AP_Notify/NavioLED_I2C.h +++ b/libraries/AP_Notify/NavioLED_I2C.h @@ -27,7 +27,7 @@ protected: private: AP_HAL::OwnPtr _dev; - bool _timer(void); + void _timer(void); struct { uint8_t r, g, b; } rgb; diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 6558178d86..0a20a6fca1 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp @@ -53,7 +53,7 @@ bool ToshibaLED_I2C::hw_init() // give back i2c semaphore _dev->get_semaphore()->give(); - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, bool)); + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void)); return ret; } @@ -65,17 +65,16 @@ bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) return true; } -bool ToshibaLED_I2C::_timer(void) +void ToshibaLED_I2C::_timer(void) { if (!_need_update) { - return true; + return; } _need_update = false; - + /* 4-bit for each color */ uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(rgb.b >> 4), (uint8_t)(rgb.g / 16), (uint8_t)(rgb.r / 16) }; _dev->transfer(val, sizeof(val), nullptr, 0); - return true; } diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h index aa62f95387..ecfe3dd911 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.h +++ b/libraries/AP_Notify/ToshibaLED_I2C.h @@ -27,7 +27,7 @@ public: private: AP_HAL::OwnPtr _dev; - bool _timer(void); + void _timer(void); bool _need_update; struct { uint8_t r, g, b; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp index 2eeb1b0381..1773d21a28 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp @@ -88,7 +88,7 @@ bool AP_OpticalFlow_PX4Flow::setup_sensor(void) return false; } // read at 10Hz - dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, bool)); + dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, void)); return true; } @@ -99,11 +99,11 @@ void AP_OpticalFlow_PX4Flow::update(void) } // timer to read sensor -bool AP_OpticalFlow_PX4Flow::timer(void) +void AP_OpticalFlow_PX4Flow::timer(void) { struct i2c_integral_frame frame; if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) { - return true; + return; } struct OpticalFlow::OpticalFlow_state state {}; state.device_id = get_bus_id(); @@ -124,6 +124,4 @@ bool AP_OpticalFlow_PX4Flow::timer(void) } _update_frontend(state); - - return true; } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h index f3a03eeea5..9ab7657dff 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.h @@ -44,5 +44,5 @@ private: // setup sensor bool setup_sensor(void); - bool timer(void); + void timer(void); }; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index fc520a863d..6d9e14284a 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -149,8 +149,8 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void) _dev->get_semaphore()->give(); integral.last_frame_us = AP_HAL::micros(); - - _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, bool)); + + _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void)); return true; failed: @@ -266,10 +266,10 @@ void AP_OpticalFlow_Pixart::motion_burst(void) _dev->set_chip_select(false); } -bool AP_OpticalFlow_Pixart::timer(void) +void AP_OpticalFlow_Pixart::timer(void) { if (AP_HAL::micros() - last_burst_us < 500) { - return true; + return; } motion_burst(); last_burst_us = AP_HAL::micros(); @@ -301,7 +301,6 @@ bool AP_OpticalFlow_Pixart::timer(void) sum_x = sum_y = 0; } #endif - return true; } // update - read latest values from sensor and fill in x,y and totals. diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h index 8468a2551e..dd28b60ea4 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.h @@ -62,7 +62,7 @@ private: void srom_download(void); void load_configuration(void); - bool timer(void); + void timer(void); void motion_burst(void); int32_t sum_x; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index b339ae6f93..df614d8e42 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -64,7 +64,7 @@ void AP_RangeFinder_LightWareI2C::init() { // call timer() at 20Hz _dev->register_periodic_callback(50000, - FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::timer, void)); } // read - return last value measured by sensor @@ -94,13 +94,12 @@ void AP_RangeFinder_LightWareI2C::update(void) // nothing to do - its all done in the timer() } -bool AP_RangeFinder_LightWareI2C::timer(void) +void AP_RangeFinder_LightWareI2C::timer(void) { if (get_reading(state.distance_cm)) { // update range_valid state based on distance measured update_status(); } else { set_status(RangeFinder::RangeFinder_NoData); - } - return true; + } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index 13abe64f20..04b941cf6e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -19,8 +19,8 @@ private: AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr dev); void init(); - bool timer(); - + void timer(); + // get a reading bool get_reading(uint16_t &reading_cm); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index 0540e7f3e0..aa71da045c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -90,7 +90,7 @@ bool AP_RangeFinder_MaxsonarI2CXL::_init(void) _dev->get_semaphore()->give(); _dev->register_periodic_callback(50000, - FUNCTOR_BIND_MEMBER(&AP_RangeFinder_MaxsonarI2CXL::_timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_RangeFinder_MaxsonarI2CXL::_timer, void)); return true; } @@ -126,7 +126,7 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm) /* timer called at 20Hz */ -bool AP_RangeFinder_MaxsonarI2CXL::_timer(void) +void AP_RangeFinder_MaxsonarI2CXL::_timer(void) { uint16_t d; if (get_reading(d)) { @@ -136,9 +136,8 @@ bool AP_RangeFinder_MaxsonarI2CXL::_timer(void) _sem->give(); } } - return true; } - + /* update the state of the sensor diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index f65a0e38c9..304560e821 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -28,8 +28,8 @@ private: AP_RangeFinder_MaxsonarI2CXL(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); - bool _init(void); - bool _timer(void); + bool _init(void); + void _timer(void); uint16_t distance; bool new_distance; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 646ad50ec0..e5bb46fb42 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -71,7 +71,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, Range /* called at 50Hz */ -bool AP_RangeFinder_PulsedLightLRF::timer(void) +void AP_RangeFinder_PulsedLightLRF::timer(void) { if (check_reg_counter++ == 10) { check_reg_counter = 0; @@ -110,7 +110,6 @@ bool AP_RangeFinder_PulsedLightLRF::timer(void) break; } } - return true; } @@ -181,7 +180,7 @@ bool AP_RangeFinder_PulsedLightLRF::init(void) } _dev->register_periodic_callback(20000, - FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, bool)); + FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, void)); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index a79fbf09fc..f9437d3958 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -33,7 +33,7 @@ private: // start a reading bool init(void); - bool timer(void); + void timer(void); bool lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); AP_HAL::OwnPtr _dev;