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; @@ -28,6 +28,8 @@ using namespace HALSITL;
*/
void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
{
static uint32_t last_update;
if (_compass == NULL) {
// no compass in this sketch
return;
@ -45,7 +47,13 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) @@ -45,7 +47,13 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
Vector3f motor = _sitl->mag_mot.get() * _current;
Vector3f new_mag_data = _compass->getHIL(0) + noise + motor;
// 100Hz, to match the real APM2 compass
uint32_t now = hal.scheduler->millis();
if ((now - last_update) < 10) {
return;
}
last_update = now;
// add 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.

Loading…
Cancel
Save