yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavsdk_tests: add test for stuck mag

Browse Source
This commit is contained in:
Julian Oes
2020-06-26 16:43:51 +02:00
committed by Daniel Agar
parent afcfa2fbe0
commit c31246e13a
5 changed files with 65 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -247,8 +247,17 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {
_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
if (_mag_stuck) {
_px4_mag_0.update(time, _last_magx, _last_magy, _last_magz);
_px4_mag_1.update(time, _last_magx, _last_magy, _last_magz);
} else {
_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_last_magx = sensors.xmag;
_last_magy = sensors.ymag;
_last_magz = sensors.zmag;
}
}
// baro
@@ -989,6 +998,11 @@ void Simulator::check_failure_injections()
supported = true;
_mag_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
supported = true;
_mag_stuck = true;
_mag_blocked = false;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_mag_blocked = false;