Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: allow simulation of compass sensor failure

Browse Source
This commit is contained in:
Andrew Tridgell 2020-08-28 12:26:58 +10:00
parent bf9c76a5c3
commit c2feebea13
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -128,12 +128,22 @@ void AP_Compass_SITL::_timer()
} else { } else {
f.rotate(get_board_orientation()); f.rotate(get_board_orientation());
} }
if (i == 0) { // scale the compass to simulate sensor scale factor errors
// scale the first compass to simulate sensor scale factor errors f *= _sitl->mag_scaling[i];
f *= _sitl->mag_scaling;
switch (_sitl->mag_fail[i]) {
case 0:
accumulate_sample(f, _compass_instance[i], 10);
_last_data[i] = f;
break;
case 1:
// no data
break;
case 2:
// frozen compass
accumulate_sample(_last_data[i], _compass_instance[i], 10);
break;
} }
accumulate_sample(f, _compass_instance[i], 10);
} }
} }

1
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -40,5 +40,6 @@ private:
Matrix3f _eliptical_corr; Matrix3f _eliptical_corr;
Vector3f _last_dia; Vector3f _last_dia;
Vector3f _last_odi; Vector3f _last_odi;
Vector3f _last_data[MAX_SITL_COMPASSES];
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD