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

navigator: set 1 second hysteresis time before triggering navigator_fail_max_hagl

Browse Source 
- this is needed to avoid a race condition with the estimator in cases where
we transition from hover to forward flight and expect airspeed aiding to
activate after the transition.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2024-12-19 14:07:57 +03:00
parent 367a8a7fc4
commit ba6f3f1548
2 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/navigator/mission_block.cpp
View File

@ -1023,10 +1023,19 @@ void MissionBlock::updateFailsafeChecks()
void MissionBlock::updateMaxHaglFailsafe()
{
const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
if (_navigator->get_global_position()->terrain_alt_valid
&& ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
if (!_navigator->get_position_setpoint_triplet()->current.valid ||
!_navigator->get_global_position()->terrain_alt_valid ||
!PX4_ISFINITE(_navigator->get_local_position()->hagl_max)) {
_navigator->update_and_get_terrain_alt_exceeded_state(false, hrt_absolute_time());
return;
}
const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
const bool exceeded = (target_alt - _navigator->get_global_position()->terrain_alt) >
_navigator->get_local_position()->hagl_max;
if (_navigator->update_and_get_terrain_alt_exceeded_state(exceeded, hrt_absolute_time())) {
// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");

8
src/modules/navigator/navigator.h
View File

@ -59,6 +59,7 @@
#include <lib/adsb/AdsbConflict.h>
#include <lib/perf/perf_counter.h>
#include <lib/hysteresis/hysteresis.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
@ -240,6 +241,12 @@ public:
*/
void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
bool update_and_get_terrain_alt_exceeded_state(bool exceeded, hrt_abstime now)
{
_max_terrain_alt_exceeded_hyst.set_state_and_update(exceeded, now);
return _max_terrain_alt_exceeded_hyst.get_state();
}
/**
* Get if the yaw acceptance is required at the current mission item
*
@ -385,6 +392,7 @@ private:
bool _is_capturing_images{false}; // keep track if we need to stop capturing images
systemlib::Hysteresis _max_terrain_alt_exceeded_hyst{false};
// update subscriptions
void params_update();