fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_mode_manager: pass deltas on estimator reset

Browse Source
This commit is contained in:
Daniel Agar
2021-07-19 09:34:07 -04:00
parent d79eea0c41
commit 87610957a4
10 changed files with 32 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+4 -4
View File
@@ -49,22 +49,22 @@ void FlightTask::_checkEkfResetCounters()
{
// Check if a reset event has happened
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
_ekfResetHandlerPositionXY();
_ekfResetHandlerPositionXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_xy});
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
}
if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
_ekfResetHandlerVelocityXY();
_ekfResetHandlerVelocityXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_vxy});
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
}
if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
_ekfResetHandlerPositionZ();
_ekfResetHandlerPositionZ(_sub_vehicle_local_position.get().delta_z);
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
}
if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
_ekfResetHandlerVelocityZ();
_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
}
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+4 -4
View File
@@ -208,10 +208,10 @@ protected:
* TODO: add the delta values to all the handlers
*/
void _checkEkfResetCounters();
virtual void _ekfResetHandlerPositionXY() {};
virtual void _ekfResetHandlerVelocityXY() {};
virtual void _ekfResetHandlerPositionZ() {};
virtual void _ekfResetHandlerVelocityZ() {};
virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
virtual void _ekfResetHandlerPositionZ(float delta_z) {};
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
virtual void _ekfResetHandlerHeading(float delta_psi) {};
map_projection_reference_s _global_local_proj_ref{};