Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: BMM150: convert it to use Device periodic callback

This commit is contained in:
Gustavo Jose de Sousa 2016-08-29 11:35:07 -03:00 committed by Lucas De Marchi
parent 667fca60f5
commit b259356703
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -193,9 +193,8 @@ bool AP_Compass_BMM150::init()
_compass_instance = register_compass();
set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150);
hal.scheduler->suspend_timer_procs();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
hal.scheduler->resume_timer_procs();
_dev->register_periodic_callback(MEASURE_TIME_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, bool));
return true;
@ -242,25 +241,20 @@ int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
return constrain_int32(dividend / divisor, -0x8000, 0x8000);
}
void AP_Compass_BMM150::_update()
bool AP_Compass_BMM150::_update()
{
const uint32_t time_usec = AP_HAL::micros();
if (time_usec - _last_update_timestamp < MEASURE_TIME_USEC) {
return;
}
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
return true;
}
le16_t data[4];
bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
_dev->get_semaphore()->give();
/* Checking data ready status */
if (!ret || !(data[3] & 0x1)) {
return;
return true;
}
const uint16_t rhall = le16toh(data[3] >> 2);
@ -287,7 +281,7 @@ void AP_Compass_BMM150::_update()
correct_field(raw_field, _compass_instance);
if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
return true;
}
_mag_accum += raw_field;
_accum_count++;
@ -297,6 +291,7 @@ void AP_Compass_BMM150::_update()
}
_last_update_timestamp = time_usec;
_accum_sem->give();
return true;
}
void AP_Compass_BMM150::read()

5
libraries/AP_Compass/AP_Compass_BMM150.h
View File

@ -39,7 +39,10 @@ public:
private:
AP_Compass_BMM150(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
void _update();
/**
* Device periodic callback to read data from the sensor.
*/
bool _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);