Global: change Device::PeriodicCb signature

Remove bool return as it's never being used and not supported on PX4.
This commit is contained in:
Lucas De Marchi 2017-01-13 11:26:14 -08:00 committed by Andrew Tridgell
parent 5aa4bc4368
commit 5472bc4de1
64 changed files with 135 additions and 185 deletions

View File

@ -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;
}

View File

@ -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;

View File

@ -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

View File

@ -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;

View File

@ -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

View File

@ -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();

View File

@ -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) {
@ -106,7 +106,6 @@ bool AP_Baro_BMP085::_timer(void)
} else {
_cmd_read_pressure();
}
return true;
}
/*

View File

@ -23,8 +23,8 @@ private:
void _calculate();
bool _data_ready();
bool _timer(void);
void _timer(void);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
AP_HAL::DigitalSource *_eoc;

View File

@ -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,

View File

@ -46,7 +46,7 @@ private:
uint16_t _read_prom_word(uint8_t word);
uint32_t _read_adc();
bool _timer();
void _timer();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;

View File

@ -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

View File

@ -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

View File

@ -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()

View File

@ -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;

View File

@ -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 *) &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);
@ -263,9 +263,6 @@ bool AP_Compass_AK8963::_update()
}
_sem->give();
}
fail:
return true;
}
/*

View File

@ -50,7 +50,7 @@ private:
bool _check_id();
bool _calibrate();
bool _update();
void _update();
void _update_timer();
AP_AK8963_BusDriver *_bus;

View File

@ -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()

View File

@ -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);

View File

@ -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;
}
/*

View File

@ -39,8 +39,8 @@ private:
bool _calibrate();
bool _setup_sampling_mode();
bool _timer();
void _timer();
/* Read a single sample */
bool _read_sample();

View File

@ -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<int16_t>(le16toh(buffer.rx));
@ -175,8 +174,6 @@ bool AP_Compass_IST8310::timer()
}
start_conversion();
return true;
}
void AP_Compass_IST8310::read()

View File

@ -41,7 +41,7 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
bool timer();
void timer();
bool init();
void start_conversion();

View File

@ -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()

View File

@ -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;

View File

@ -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

View File

@ -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);

View File

@ -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 *) &regs, 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()

View File

@ -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);

View File

@ -38,7 +38,7 @@ public:
SPEED_LOW,
};
FUNCTOR_TYPEDEF(PeriodicCb, bool);
FUNCTOR_TYPEDEF(PeriodicCb, void);
typedef void* PeriodicHandle;
Device(enum BusType type)

View File

@ -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;
}

View File

@ -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];

View File

@ -41,9 +41,7 @@ void TimerPollable::on_can_read()
_wrapper->start_cb();
}
if (!_cb()) {
_removeme = true;
}
_cb();
if (_wrapper) {
_wrapper->end_cb();

View File

@ -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

View File

@ -15,7 +15,7 @@ public:
private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
bool _poll_data(void);
void _poll_data(void);
};
}

View File

@ -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)

View File

@ -20,7 +20,7 @@ public:
private:
void reset();
bool _update(void);
void _update(void);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;

View File

@ -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;
}

View File

@ -26,7 +26,7 @@ protected:
private:
bool _in_timer;
bool _bus_timer(void);
void _bus_timer(void);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;

View File

@ -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

View File

@ -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);

View File

@ -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()

View File

@ -98,7 +98,7 @@ private:
/**
* Device periodic callback to read data from the sensors.
*/
bool _poll_data();
void _poll_data();
/**
* Read samples from fifo.

View File

@ -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)

View File

@ -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 */

View File

@ -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

View File

@ -28,7 +28,7 @@ public:
private:
bool _init_sensor();
bool _accumulate();
void _accumulate();
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;

View File

@ -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()

View File

@ -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();

View File

@ -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)

View File

@ -18,5 +18,5 @@ private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE];
bool _need_hw_update;
bool _timer(void);
void _timer(void);
};

View File

@ -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;
}

View File

@ -27,7 +27,7 @@ protected:
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
bool _timer(void);
void _timer(void);
struct {
uint8_t r, g, b;
} rgb;

View File

@ -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;
}

View File

@ -27,7 +27,7 @@ public:
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
bool _timer(void);
void _timer(void);
bool _need_update;
struct {
uint8_t r, g, b;

View File

@ -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;
}

View File

@ -44,5 +44,5 @@ private:
// setup sensor
bool setup_sensor(void);
bool timer(void);
void timer(void);
};

View File

@ -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.

View File

@ -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;

View File

@ -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;
}
}

View File

@ -19,8 +19,8 @@ private:
AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
void init();
bool timer();
void timer();
// get a reading
bool get_reading(uint16_t &reading_cm);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;

View File

@ -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

View File

@ -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;

View File

@ -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;
}

View File

@ -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<AP_HAL::I2CDevice> _dev;