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

HAL_SITL: use SIM_MAG_OFS in SITL compass

This commit is contained in:
Andrew Tridgell 2015-04-21 08:29:51 +10:00
parent 1ce9cb693a
commit a4bad1c703
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -154,8 +154,6 @@ private:
VectorN<readings_mag,mag_buffer_length> buffer_mag;
uint32_t time_delta_mag;
uint32_t delayed_time_mag;
Vector3f mag_data;
Vector3f new_mag_data;
// airspeed sensor delay buffer variables
struct readings_wind {

9
libraries/AP_HAL_AVR_SITL/sitl_compass.cpp
View File

@ -43,8 +43,7 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
Vector3f motor = _sitl->mag_mot.get() * _current;
new_mag_data = _compass->getHIL() + noise + motor;
Vector3f new_mag_data = _compass->getHIL() + noise + motor;
uint32_t now = hal.scheduler->millis();
// add delay
@ -74,10 +73,12 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
}
}
if (best_time_delta_mag < 1000) { // only output stored state if < 1 sec retrieval error
mag_data = buffer_mag[best_index_mag].data;
new_mag_data = buffer_mag[best_index_mag].data;
}
_compass->setHIL(mag_data);
new_mag_data -= _sitl->mag_ofs.get();
_compass->setHIL(new_mag_data);
}
#endif