fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add terrain/dist_bottom reset deltas (vehicle_local_position/vehicle_global_position)

Browse Source
This commit is contained in:
Daniel Agar
2024-07-15 10:25:42 -04:00
committed by GitHub
parent a5a67315fd
commit e03aef616c
11 changed files with 104 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+6 -1
View File
@@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters()
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
}
if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) {
_ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom);
_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
}
if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
@@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
// distance to bottom
if (_sub_vehicle_local_position.get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
}
// global frame reference coordinates to enable conversions
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+2
View File
@@ -61,6 +61,7 @@ struct ekf_reset_counters_s {
uint8_t z;
uint8_t vz;
uint8_t heading;
uint8_t hagl;
};
class FlightTask : public ModuleParams
@@ -191,6 +192,7 @@ protected:
virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
virtual void _ekfResetHandlerPositionZ(float delta_z) {};
virtual void _ekfResetHandlerHagl(float delta_hagl) {};
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
virtual void _ekfResetHandlerHeading(float delta_psi) {};