mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-27 02:04:00 -04:00
AP_Compass: remove user of timesliced logic
This commit is contained in:
parent
0a158f6d3f
commit
6bab28397a
@ -170,7 +170,7 @@ bool AP_Compass_AK8963::init()
|
|||||||
/* timer needs to be called every 10ms so set the freq_div to 10 */
|
/* 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, void))) {
|
if (!_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void))) {
|
||||||
// fallback to timer
|
// fallback to timer
|
||||||
_timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void), 10);
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void));
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
@ -272,7 +272,7 @@ void AP_Compass_AK8963::_update_timer()
|
|||||||
{
|
{
|
||||||
uint32_t now = AP_HAL::micros();
|
uint32_t now = AP_HAL::micros();
|
||||||
|
|
||||||
if (!_timesliced && now - _last_update_timestamp < 10000) {
|
if (now - _last_update_timestamp < 10000) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -281,7 +281,6 @@ void AP_Compass_AK8963::_update_timer()
|
|||||||
}
|
}
|
||||||
|
|
||||||
_update();
|
_update();
|
||||||
|
|
||||||
_last_update_timestamp = now;
|
_last_update_timestamp = now;
|
||||||
|
|
||||||
_bus->get_semaphore()->give();
|
_bus->get_semaphore()->give();
|
||||||
|
@ -64,7 +64,6 @@ private:
|
|||||||
|
|
||||||
uint8_t _compass_instance;
|
uint8_t _compass_instance;
|
||||||
bool _initialized;
|
bool _initialized;
|
||||||
bool _timesliced;
|
|
||||||
enum Rotation _rotation;
|
enum Rotation _rotation;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user