Browse Source

AP_HAL_AVR_SITL: Limit SITL compass reports to 100Hz

Similarly to how the SITL baro currently works, setHIL is only called at 100Hz.
mission-4.1.18
wsilva32 9 years ago committed by Andrew Tridgell
parent
commit
ab7148386c
  1. 8
      libraries/AP_HAL_SITL/sitl_compass.cpp

8
libraries/AP_HAL_SITL/sitl_compass.cpp

@ -28,6 +28,8 @@ using namespace HALSITL;
*/ */
void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
{ {
static uint32_t last_update;
if (_compass == NULL) { if (_compass == NULL) {
// no compass in this sketch // no compass in this sketch
return; return;
@ -45,7 +47,13 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
Vector3f motor = _sitl->mag_mot.get() * _current; Vector3f motor = _sitl->mag_mot.get() * _current;
Vector3f new_mag_data = _compass->getHIL(0) + noise + motor; Vector3f new_mag_data = _compass->getHIL(0) + noise + motor;
// 100Hz, to match the real APM2 compass
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
if ((now - last_update) < 10) {
return;
}
last_update = now;
// add delay // add delay
uint32_t best_time_delta_mag = 1000; // initialise large time representing buffer entry closest to current time - delay. uint32_t best_time_delta_mag = 1000; // initialise large time representing buffer entry closest to current time - delay.
uint8_t best_index_mag = 0; // initialise number representing the index of the entry in buffer closest to delay. uint8_t best_index_mag = 0; // initialise number representing the index of the entry in buffer closest to delay.

Loading…
Cancel
Save