Global: change Device::PeriodicCb signature
Remove bool return as it's never being used and not supported on PX4.
This commit is contained in:
parent
5aa4bc4368
commit
5472bc4de1
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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();
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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,
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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()
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -50,7 +50,7 @@ private:
|
||||
bool _check_id();
|
||||
bool _calibrate();
|
||||
|
||||
bool _update();
|
||||
void _update();
|
||||
void _update_timer();
|
||||
|
||||
AP_AK8963_BusDriver *_bus;
|
||||
|
@ -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()
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -39,8 +39,8 @@ private:
|
||||
bool _calibrate();
|
||||
bool _setup_sampling_mode();
|
||||
|
||||
bool _timer();
|
||||
|
||||
void _timer();
|
||||
|
||||
/* Read a single sample */
|
||||
bool _read_sample();
|
||||
|
||||
|
@ -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()
|
||||
|
@ -41,7 +41,7 @@ private:
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation);
|
||||
|
||||
bool timer();
|
||||
void timer();
|
||||
bool init();
|
||||
void start_conversion();
|
||||
|
||||
|
@ -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()
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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()
|
||||
|
@ -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);
|
||||
|
@ -38,7 +38,7 @@ public:
|
||||
SPEED_LOW,
|
||||
};
|
||||
|
||||
FUNCTOR_TYPEDEF(PeriodicCb, bool);
|
||||
FUNCTOR_TYPEDEF(PeriodicCb, void);
|
||||
typedef void* PeriodicHandle;
|
||||
|
||||
Device(enum BusType type)
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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];
|
||||
|
@ -41,9 +41,7 @@ void TimerPollable::on_can_read()
|
||||
_wrapper->start_cb();
|
||||
}
|
||||
|
||||
if (!_cb()) {
|
||||
_removeme = true;
|
||||
}
|
||||
_cb();
|
||||
|
||||
if (_wrapper) {
|
||||
_wrapper->end_cb();
|
||||
|
@ -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
|
||||
|
@ -15,7 +15,7 @@ public:
|
||||
private:
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
|
||||
|
||||
bool _poll_data(void);
|
||||
void _poll_data(void);
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -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)
|
||||
|
@ -20,7 +20,7 @@ public:
|
||||
|
||||
private:
|
||||
void reset();
|
||||
bool _update(void);
|
||||
void _update(void);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -26,7 +26,7 @@ protected:
|
||||
private:
|
||||
bool _in_timer;
|
||||
|
||||
bool _bus_timer(void);
|
||||
void _bus_timer(void);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
|
||||
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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()
|
||||
|
@ -98,7 +98,7 @@ private:
|
||||
/**
|
||||
* Device periodic callback to read data from the sensors.
|
||||
*/
|
||||
bool _poll_data();
|
||||
void _poll_data();
|
||||
|
||||
/**
|
||||
* Read samples from fifo.
|
||||
|
@ -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)
|
||||
|
@ -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 */
|
||||
|
@ -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
|
||||
|
@ -28,7 +28,7 @@ public:
|
||||
|
||||
private:
|
||||
bool _init_sensor();
|
||||
bool _accumulate();
|
||||
void _accumulate();
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
|
||||
|
||||
|
@ -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()
|
||||
|
@ -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();
|
||||
|
@ -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)
|
||||
|
@ -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);
|
||||
};
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -44,5 +44,5 @@ private:
|
||||
// setup sensor
|
||||
bool setup_sensor(void);
|
||||
|
||||
bool timer(void);
|
||||
void timer(void);
|
||||
};
|
||||
|
@ -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.
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user