fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 18:47:34 +08: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
src/modules/simulator/simulator.h
+5
View File
@@ -259,9 +259,14 @@ private:
bool _baro_blocked{false};
bool _baro_stuck{false};
bool _mag_blocked{false};
bool _mag_stuck{false};
bool _gps_blocked{false};
bool _airspeed_blocked{false};
float _last_magx{0.0f};
float _last_magy{0.0f};
float _last_magz{0.0f};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
#endif
src/modules/simulator/simulator_mavlink.cpp
+16 -2
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;