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

AP_InertialSensor: cope with 2 IMUs in SITL

This commit is contained in:
Andrew Tridgell 2015-01-20 19:47:45 +11:00
parent a37f3680e4
commit 8b59c72eb9
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1133,6 +1133,10 @@ check_sample:
*/
void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
{
if (_accel_count == 0) {
// we haven't initialised yet
return;
}
if (instance < INS_MAX_INSTANCES) {
_accel[instance] = accel;
_accel_healthy[instance] = true;
@ -1144,11 +1148,16 @@ void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
{
if (_gyro_count == 0) {
// we haven't initialised yet
return;
}
if (instance < INS_MAX_INSTANCES) {
_gyro[instance] = gyro;
_gyro_healthy[instance] = true;
if (_gyro_count <= instance) {
_gyro_count = instance+1;
_gyro_cal_ok[instance] = true;
}
}
}