mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 19:32:36 +08:00
Commander: Introduce global_position_relaxed (#24280)
To separate accuracy requirements for VTOL hover and cruise. - global_position_relaxed refers to having a valid horizontal velocity aid source in the estimator and a set global reference position, but poses no requirements on the accuracy of the provided position estimate. - Auto flight modes Mission, Loiter and RTL, while in fixed-wing mode, only require the relaxed global position going forward - COM_POS_FS_EPH is thus no longer used on fixed-wing vehicles (resp. VTOL in FW) - rename failsafe_flags.local_position_accuracy_low to failsafe_flags.position_accuracy_low --------- Signed-off-by: RomanBapst <bapstroman@gmail.com> Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: Silvan <silvan@auterion.com>
This commit is contained in:
@@ -13,8 +13,6 @@ param set-default MAV_TYPE 1
|
|||||||
#
|
#
|
||||||
# Default parameters for fixed wing UAVs.
|
# Default parameters for fixed wing UAVs.
|
||||||
#
|
#
|
||||||
# there is a 2.5 factor applied on the _FS thresholds if for invalidation
|
|
||||||
param set-default COM_POS_FS_EPH 50
|
|
||||||
param set-default COM_VEL_FS_EVH 3
|
param set-default COM_VEL_FS_EVH 3
|
||||||
|
|
||||||
param set-default COM_POS_LOW_EPH 50
|
param set-default COM_POS_LOW_EPH 50
|
||||||
|
|||||||
@@ -10,9 +10,6 @@ set VEHICLE_TYPE vtol
|
|||||||
# MAV_TYPE_VTOL_FIXEDROTOR 22
|
# MAV_TYPE_VTOL_FIXEDROTOR 22
|
||||||
param set-default MAV_TYPE 22
|
param set-default MAV_TYPE 22
|
||||||
|
|
||||||
# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
|
|
||||||
param set-default COM_POS_FS_EPH 50
|
|
||||||
|
|
||||||
param set-default COM_POS_LOW_EPH 50
|
param set-default COM_POS_LOW_EPH 50
|
||||||
|
|
||||||
param set-default MIS_TAKEOFF_ALT 20
|
param set-default MIS_TAKEOFF_ALT 20
|
||||||
|
|||||||
@@ -187,7 +187,23 @@ The following settings also apply, but are not displayed in the QGC UI.
|
|||||||
|
|
||||||
## Position (GNSS) Loss Failsafe
|
## Position (GNSS) Loss Failsafe
|
||||||
|
|
||||||
The _Position Loss Failsafe_ is triggered if the quality of the PX4 global position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate.
|
The _Position Loss Failsafe_ is triggered if the quality of the PX4 position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate.
|
||||||
|
The sections below cover first the trigger and then the failsafe action taken by the controller.
|
||||||
|
|
||||||
|
### Position Loss Failsafe Trigger
|
||||||
|
|
||||||
|
There are basically two mechanisms in PX4 to trigger position failsafes:
|
||||||
|
- A timeout since the last sensor data was fused that provides direct speed or horizontal position measurements. Sensors that fall into that category are: GNSS, optical flow, airspeed, VIO, auxiliary global position.
|
||||||
|
- The estimated horizontal position accuracy exceeds a certain threshold. This check is only done on hovering systems (rotary wing vehicles or VTOLs in hover phase).
|
||||||
|
|
||||||
|
The relevant parameters shown below.
|
||||||
|
|
||||||
|
| Parameter | Description |
|
||||||
|
| -------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------- |
|
||||||
|
| <a id="EKF2_NOAID_TOUT"></a>[EKF2_NOAID_TOUT](../advanced_config/parameter_reference.md#EKF2_NOAID_TOUT) | Maximum inertial dead-reckoning time, so the time after the last data sample was received of any sensor that constrains the velocity drift [microseconds]. |
|
||||||
|
| <a id="COM_POS_FS_EPH"></a>[COM_POS_FS_EPH](../advanced_config/parameter_reference.md#COM_POS_FS_EPH) | Horizontal position error threshold for hovering vehicles (Multicopters and VTOLs in hover). Fixed-wing vehicles have this value set to infinity.|
|
||||||
|
|
||||||
|
### Position Loss Failsafe Action
|
||||||
|
|
||||||
The failure action is controlled by [COM_POSCTL_NAVL](../advanced_config/parameter_reference.md#COM_POSCTL_NAVL), based on whether RC control is assumed to be available (and altitude information):
|
The failure action is controlled by [COM_POSCTL_NAVL](../advanced_config/parameter_reference.md#COM_POSCTL_NAVL), based on whether RC control is assumed to be available (and altitude information):
|
||||||
|
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ bool mission_failure # Mission failure
|
|||||||
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
||||||
bool wind_limit_exceeded # Wind limit exceeded
|
bool wind_limit_exceeded # Wind limit exceeded
|
||||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||||
bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
|
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
|
||||||
bool navigator_failure # Navigator failed to execute a mode
|
bool navigator_failure # Navigator failed to execute a mode
|
||||||
|
|
||||||
# Failure detector
|
# Failure detector
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ bool mission_failure # Mission failure
|
|||||||
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
||||||
bool wind_limit_exceeded # Wind limit exceeded
|
bool wind_limit_exceeded # Wind limit exceeded
|
||||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||||
bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
|
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
|
||||||
bool navigator_failure # Navigator failed to execute a mode
|
bool navigator_failure # Navigator failed to execute a mode
|
||||||
|
|
||||||
# Failure detector
|
# Failure detector
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ bool mission_failure # Mission failure
|
|||||||
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
||||||
bool wind_limit_exceeded # Wind limit exceeded
|
bool wind_limit_exceeded # Wind limit exceeded
|
||||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||||
bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
|
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
|
||||||
bool navigator_failure # Navigator failed to execute a mode
|
bool navigator_failure # Navigator failed to execute a mode
|
||||||
|
|
||||||
# Failure detector
|
# Failure detector
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ bool mission_failure # Mission failure
|
|||||||
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
||||||
bool wind_limit_exceeded # Wind limit exceeded
|
bool wind_limit_exceeded # Wind limit exceeded
|
||||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||||
bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
|
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
|
||||||
bool navigator_failure # Navigator failed to execute a mode
|
bool navigator_failure # Navigator failed to execute a mode
|
||||||
|
|
||||||
# Failure detector
|
# Failure detector
|
||||||
|
|||||||
+1
-1
@@ -81,7 +81,7 @@ set(msg_files
|
|||||||
EstimatorStates.msg
|
EstimatorStates.msg
|
||||||
EstimatorStatus.msg
|
EstimatorStatus.msg
|
||||||
EstimatorStatusFlags.msg
|
EstimatorStatusFlags.msg
|
||||||
Event.msg
|
versioned/Event.msg
|
||||||
FigureEightStatus.msg
|
FigureEightStatus.msg
|
||||||
FailsafeFlags.msg
|
FailsafeFlags.msg
|
||||||
FailureDetectorStatus.msg
|
FailureDetectorStatus.msg
|
||||||
|
|||||||
@@ -12,6 +12,7 @@ uint32 mode_req_local_alt
|
|||||||
uint32 mode_req_local_position
|
uint32 mode_req_local_position
|
||||||
uint32 mode_req_local_position_relaxed
|
uint32 mode_req_local_position_relaxed
|
||||||
uint32 mode_req_global_position
|
uint32 mode_req_global_position
|
||||||
|
uint32 mode_req_global_position_relaxed
|
||||||
uint32 mode_req_mission
|
uint32 mode_req_mission
|
||||||
uint32 mode_req_offboard_signal
|
uint32 mode_req_offboard_signal
|
||||||
uint32 mode_req_home_position
|
uint32 mode_req_home_position
|
||||||
@@ -29,6 +30,7 @@ bool local_position_invalid # Local position estimate invalid
|
|||||||
bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
|
bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
|
||||||
bool local_velocity_invalid # Local velocity estimate invalid
|
bool local_velocity_invalid # Local velocity estimate invalid
|
||||||
bool global_position_invalid # Global position estimate invalid
|
bool global_position_invalid # Global position estimate invalid
|
||||||
|
bool global_position_invalid_relaxed # Global position estimate invalid with relaxed accuracy requirements
|
||||||
bool auto_mission_missing # No mission available
|
bool auto_mission_missing # No mission available
|
||||||
bool offboard_control_signal_lost # Offboard signal lost
|
bool offboard_control_signal_lost # Offboard signal lost
|
||||||
bool home_position_invalid # No home position available
|
bool home_position_invalid # No home position available
|
||||||
@@ -48,7 +50,7 @@ bool mission_failure # Mission failure
|
|||||||
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
|
||||||
bool wind_limit_exceeded # Wind limit exceeded
|
bool wind_limit_exceeded # Wind limit exceeded
|
||||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||||
bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid
|
bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid
|
||||||
bool navigator_failure # Navigator failed to execute a mode
|
bool navigator_failure # Navigator failed to execute a mode
|
||||||
|
|
||||||
# Failure detector
|
# Failure detector
|
||||||
|
|||||||
@@ -0,0 +1,34 @@
|
|||||||
|
uint32 MESSAGE_VERSION = 0
|
||||||
|
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
uint8 request_id
|
||||||
|
uint8 registration_id
|
||||||
|
|
||||||
|
uint8 HEALTH_COMPONENT_INDEX_NONE = 0
|
||||||
|
|
||||||
|
uint8 health_component_index # HEALTH_COMPONENT_INDEX_*
|
||||||
|
bool health_component_is_present
|
||||||
|
bool health_component_warning
|
||||||
|
bool health_component_error
|
||||||
|
|
||||||
|
bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run
|
||||||
|
|
||||||
|
uint8 num_events
|
||||||
|
|
||||||
|
EventV0[5] events
|
||||||
|
|
||||||
|
# Mode requirements
|
||||||
|
bool mode_req_angular_velocity
|
||||||
|
bool mode_req_attitude
|
||||||
|
bool mode_req_local_alt
|
||||||
|
bool mode_req_local_position
|
||||||
|
bool mode_req_local_position_relaxed
|
||||||
|
bool mode_req_global_position
|
||||||
|
bool mode_req_mission
|
||||||
|
bool mode_req_home_position
|
||||||
|
bool mode_req_prevent_arming
|
||||||
|
bool mode_req_manual_control
|
||||||
|
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 4
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
# this message is required here in the msg_old folder because other msg are depending on it
|
||||||
|
# Events interface
|
||||||
|
|
||||||
|
uint32 MESSAGE_VERSION = 0
|
||||||
|
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
uint32 id # Event ID
|
||||||
|
uint16 event_sequence # Event sequence number
|
||||||
|
uint8[25] arguments # (optional) arguments, depend on event id
|
||||||
|
|
||||||
|
uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 16
|
||||||
@@ -13,3 +13,5 @@
|
|||||||
#include "translation_vehicle_status_v1.h"
|
#include "translation_vehicle_status_v1.h"
|
||||||
#include "translation_airspeed_validated_v1.h"
|
#include "translation_airspeed_validated_v1.h"
|
||||||
#include "translation_vehicle_attitude_setpoint_v1.h"
|
#include "translation_vehicle_attitude_setpoint_v1.h"
|
||||||
|
#include "translation_arming_check_reply_v1.h"
|
||||||
|
#include "translation_event_v1.h"
|
||||||
|
|||||||
@@ -0,0 +1,84 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* Copyright (c) 2025 PX4 Development Team.
|
||||||
|
* SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
****************************************************************************/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// Translate ArmingCheckReply v0 <--> v1
|
||||||
|
#include <px4_msgs_old/msg/arming_check_reply_v0.hpp>
|
||||||
|
#include <px4_msgs/msg/arming_check_reply.hpp>
|
||||||
|
#include "translation_event_v1.h"
|
||||||
|
|
||||||
|
class ArmingCheckReplyV1Translation {
|
||||||
|
public:
|
||||||
|
using MessageOlder = px4_msgs_old::msg::ArmingCheckReplyV0;
|
||||||
|
static_assert(MessageOlder::MESSAGE_VERSION == 0);
|
||||||
|
|
||||||
|
using MessageNewer = px4_msgs::msg::ArmingCheckReply;
|
||||||
|
static_assert(MessageNewer::MESSAGE_VERSION == 1);
|
||||||
|
|
||||||
|
static constexpr const char* kTopic = "/fmu/in/arming_check_reply";
|
||||||
|
|
||||||
|
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||||
|
msg_newer.timestamp = msg_older.timestamp;
|
||||||
|
|
||||||
|
msg_newer.request_id = msg_older.request_id;
|
||||||
|
msg_newer.registration_id = msg_older.registration_id;
|
||||||
|
|
||||||
|
msg_newer.health_component_index = msg_older.health_component_index;
|
||||||
|
msg_newer.health_component_is_present = msg_older.health_component_is_present;
|
||||||
|
msg_newer.health_component_warning = msg_older.health_component_warning;
|
||||||
|
msg_newer.health_component_error = msg_older.health_component_error;
|
||||||
|
|
||||||
|
msg_newer.can_arm_and_run = msg_older.can_arm_and_run;
|
||||||
|
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < msg_newer.events.size();i++) {
|
||||||
|
EventV1Translation::fromOlder(msg_older.events.at(i), msg_newer.events.at(i));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
msg_newer.mode_req_angular_velocity = msg_older.mode_req_angular_velocity;
|
||||||
|
msg_newer.mode_req_attitude = msg_older.mode_req_attitude;
|
||||||
|
msg_newer.mode_req_local_alt = msg_older.mode_req_local_alt;
|
||||||
|
msg_newer.mode_req_local_position = msg_older.mode_req_local_position;
|
||||||
|
msg_newer.mode_req_local_position_relaxed = msg_older.mode_req_local_position_relaxed;
|
||||||
|
msg_newer.mode_req_global_position = msg_older.mode_req_global_position;
|
||||||
|
msg_newer.mode_req_global_position_relaxed = false;
|
||||||
|
msg_newer.mode_req_mission = msg_older.mode_req_mission;
|
||||||
|
msg_newer.mode_req_home_position = msg_older.mode_req_home_position;
|
||||||
|
msg_newer.mode_req_prevent_arming = msg_older.mode_req_prevent_arming;
|
||||||
|
msg_newer.mode_req_manual_control = msg_older.mode_req_manual_control;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||||
|
msg_older.timestamp = msg_newer.timestamp;
|
||||||
|
|
||||||
|
msg_older.request_id = msg_newer.request_id;
|
||||||
|
msg_older.registration_id = msg_newer.registration_id;
|
||||||
|
|
||||||
|
msg_older.health_component_index = msg_newer.health_component_index;
|
||||||
|
msg_older.health_component_is_present = msg_newer.health_component_is_present;
|
||||||
|
msg_older.health_component_warning = msg_newer.health_component_warning;
|
||||||
|
msg_older.health_component_error = msg_newer.health_component_error;
|
||||||
|
|
||||||
|
msg_older.can_arm_and_run = msg_newer.can_arm_and_run;
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < msg_older.events.size();i++) {
|
||||||
|
EventV1Translation::toOlder(msg_newer.events.at(i), msg_older.events.at(i));
|
||||||
|
}
|
||||||
|
|
||||||
|
msg_older.mode_req_angular_velocity = msg_newer.mode_req_angular_velocity;
|
||||||
|
msg_older.mode_req_attitude = msg_newer.mode_req_attitude;
|
||||||
|
msg_older.mode_req_local_alt = msg_newer.mode_req_local_alt;
|
||||||
|
msg_older.mode_req_local_position = msg_newer.mode_req_local_position;
|
||||||
|
msg_older.mode_req_global_position = msg_newer.mode_req_global_position;
|
||||||
|
msg_older.mode_req_mission = msg_newer.mode_req_mission;
|
||||||
|
msg_older.mode_req_home_position = msg_newer.mode_req_home_position;
|
||||||
|
msg_older.mode_req_prevent_arming = msg_newer.mode_req_prevent_arming;
|
||||||
|
msg_older.mode_req_manual_control = msg_newer.mode_req_manual_control;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
REGISTER_TOPIC_TRANSLATION_DIRECT(ArmingCheckReplyV1Translation);
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* Copyright (c) 2025 PX4 Development Team.
|
||||||
|
* SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
****************************************************************************/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
// Translate Event v0 <--> v1
|
||||||
|
#include <px4_msgs_old/msg/event_v0.hpp>
|
||||||
|
#include <px4_msgs/msg/event.hpp>
|
||||||
|
|
||||||
|
class EventV1Translation {
|
||||||
|
public:
|
||||||
|
using MessageOlder = px4_msgs_old::msg::EventV0;
|
||||||
|
static_assert(MessageOlder::MESSAGE_VERSION == 0);
|
||||||
|
|
||||||
|
using MessageNewer = px4_msgs::msg::Event;
|
||||||
|
static_assert(MessageNewer::MESSAGE_VERSION == 1);
|
||||||
|
|
||||||
|
static constexpr const char* kTopic = "fmu/out/event";
|
||||||
|
|
||||||
|
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||||
|
msg_newer.timestamp = msg_older.timestamp;
|
||||||
|
msg_newer.id = msg_older.id;
|
||||||
|
msg_newer.event_sequence = msg_older.event_sequence;
|
||||||
|
msg_newer.arguments = msg_older.arguments;
|
||||||
|
msg_newer.log_levels = msg_older.log_levels;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||||
|
msg_older.timestamp = msg_newer.timestamp;
|
||||||
|
msg_older.id = msg_newer.id;
|
||||||
|
msg_older.event_sequence = msg_newer.event_sequence;
|
||||||
|
msg_older.arguments = msg_newer.arguments;
|
||||||
|
msg_older.log_levels = msg_newer.log_levels;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
REGISTER_TOPIC_TRANSLATION_DIRECT(EventV1Translation);
|
||||||
@@ -1,4 +1,4 @@
|
|||||||
uint32 MESSAGE_VERSION = 0
|
uint32 MESSAGE_VERSION = 1
|
||||||
|
|
||||||
uint64 timestamp # time since system start (microseconds)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
@@ -25,6 +25,7 @@ bool mode_req_local_alt
|
|||||||
bool mode_req_local_position
|
bool mode_req_local_position
|
||||||
bool mode_req_local_position_relaxed
|
bool mode_req_local_position_relaxed
|
||||||
bool mode_req_global_position
|
bool mode_req_global_position
|
||||||
|
bool mode_req_global_position_relaxed
|
||||||
bool mode_req_mission
|
bool mode_req_mission
|
||||||
bool mode_req_home_position
|
bool mode_req_home_position
|
||||||
bool mode_req_prevent_arming
|
bool mode_req_prevent_arming
|
||||||
|
|||||||
@@ -1,4 +1,6 @@
|
|||||||
# Events interface
|
# Events interface
|
||||||
|
uint32 MESSAGE_VERSION = 1
|
||||||
|
|
||||||
uint64 timestamp # time since system start (microseconds)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
uint32 id # Event ID
|
uint32 id # Event ID
|
||||||
@@ -45,6 +45,7 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
|
|||||||
_failsafe_flags.local_position_invalid_relaxed = true;
|
_failsafe_flags.local_position_invalid_relaxed = true;
|
||||||
_failsafe_flags.local_velocity_invalid = true;
|
_failsafe_flags.local_velocity_invalid = true;
|
||||||
_failsafe_flags.global_position_invalid = true;
|
_failsafe_flags.global_position_invalid = true;
|
||||||
|
_failsafe_flags.global_position_invalid_relaxed = true;
|
||||||
_failsafe_flags.auto_mission_missing = true;
|
_failsafe_flags.auto_mission_missing = true;
|
||||||
_failsafe_flags.offboard_control_signal_lost = true;
|
_failsafe_flags.offboard_control_signal_lost = true;
|
||||||
_failsafe_flags.home_position_invalid = true;
|
_failsafe_flags.home_position_invalid = true;
|
||||||
@@ -54,7 +55,10 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
|
|||||||
{
|
{
|
||||||
_reporter.reset();
|
_reporter.reset();
|
||||||
|
|
||||||
_reporter.prepare(_context.status().vehicle_type);
|
// treat VTOLs in transition mode as fixed-wing - this is not in line with what's published as vehicle_status_s::vehicle_type
|
||||||
|
const uint8_t vehicle_type = _context.status().in_transition_mode ? vehicle_status_s::VEHICLE_TYPE_FIXED_WING :
|
||||||
|
_context.status().vehicle_type;
|
||||||
|
_reporter.prepare(vehicle_type);
|
||||||
|
|
||||||
_context.setIsArmingRequest(is_arming_request);
|
_context.setIsArmingRequest(is_arming_request);
|
||||||
|
|
||||||
@@ -78,7 +82,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
|
|||||||
_reporter._mavlink_log_pub = &_mavlink_log_pub;
|
_reporter._mavlink_log_pub = &_mavlink_log_pub;
|
||||||
_reporter.reset();
|
_reporter.reset();
|
||||||
|
|
||||||
_reporter.prepare(_context.status().vehicle_type);
|
_reporter.prepare(vehicle_type);
|
||||||
|
|
||||||
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
||||||
if (!_checks[i]) {
|
if (!_checks[i]) {
|
||||||
|
|||||||
@@ -633,10 +633,18 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s
|
|||||||
void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
|
void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
|
||||||
const vehicle_local_position_s &lpos) const
|
const vehicle_local_position_s &lpos) const
|
||||||
{
|
{
|
||||||
const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
|
|
||||||
&& (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
|
|
||||||
|
|
||||||
if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy
|
bool position_valid_but_low_accuracy = false;
|
||||||
|
|
||||||
|
if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) ||
|
||||||
|
(reporter.failsafeFlags().mode_req_global_position_relaxed
|
||||||
|
&& !reporter.failsafeFlags().global_position_invalid_relaxed) ||
|
||||||
|
(reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid)) {
|
||||||
|
|
||||||
|
position_valid_but_low_accuracy = (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!reporter.failsafeFlags().position_accuracy_low && position_valid_but_low_accuracy
|
||||||
&& _param_com_pos_low_act.get()) {
|
&& _param_com_pos_low_act.get()) {
|
||||||
|
|
||||||
// only report if armed
|
// only report if armed
|
||||||
@@ -658,7 +666,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
|
reporter.failsafeFlags().position_accuracy_low = position_valid_but_low_accuracy;
|
||||||
}
|
}
|
||||||
|
|
||||||
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
|
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
|
||||||
@@ -698,6 +706,12 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||||||
!checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
|
!checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
|
||||||
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
|
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
|
||||||
|
|
||||||
|
// for relaxed global condition we don't have any accuracy requirement
|
||||||
|
const float pos_eph_relaxed_treshold = INFINITY;
|
||||||
|
failsafe_flags.global_position_invalid_relaxed = !checkPosVelValidity(now, global_pos_valid, gpos.eph,
|
||||||
|
pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us,
|
||||||
|
!failsafe_flags.global_position_invalid_relaxed);
|
||||||
|
|
||||||
// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
|
// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
|
||||||
const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
|
const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
|
||||||
const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
|
const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
|
||||||
@@ -712,6 +726,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||||||
|| estimator_status_flags.cs_wind_dead_reckoning;
|
|| estimator_status_flags.cs_wind_dead_reckoning;
|
||||||
|
|
||||||
if (!failsafe_flags.global_position_invalid
|
if (!failsafe_flags.global_position_invalid
|
||||||
|
&& failsafe_flags.mode_req_global_position
|
||||||
&& !_nav_failure_imminent_warned
|
&& !_nav_failure_imminent_warned
|
||||||
&& gpos.eph > gpos_critical_warning_thrld
|
&& gpos.eph > gpos_critical_warning_thrld
|
||||||
&& dead_reckoning) {
|
&& dead_reckoning) {
|
||||||
|
|||||||
@@ -96,6 +96,7 @@ private:
|
|||||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||||
|
|
||||||
hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec)
|
hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec)
|
||||||
|
hrt_abstime _last_gpos_relaxed_fail_time_us{0}; ///< Last time that the global position relaxed validity recovery check failed (usec)
|
||||||
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
|
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
|
||||||
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
|
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
|
||||||
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)
|
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)
|
||||||
|
|||||||
@@ -173,6 +173,8 @@ void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
|
|||||||
reporter.failsafeFlags().mode_req_local_position_relaxed);
|
reporter.failsafeFlags().mode_req_local_position_relaxed);
|
||||||
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
|
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
|
||||||
reporter.failsafeFlags().mode_req_global_position);
|
reporter.failsafeFlags().mode_req_global_position);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_global_position_relaxed, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_global_position_relaxed);
|
||||||
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
|
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
|
||||||
reporter.failsafeFlags().mode_req_mission);
|
reporter.failsafeFlags().mode_req_mission);
|
||||||
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
|
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
|
||||||
|
|||||||
@@ -86,16 +86,27 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||||||
reporter.clearCanRunBits(local_position_modes);
|
reporter.clearCanRunBits(local_position_modes);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
NavModes global_position_modes = NavModes::None;
|
||||||
|
|
||||||
if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
|
if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
|
||||||
|
global_position_modes = (NavModes)reporter.failsafeFlags().mode_req_global_position;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (reporter.failsafeFlags().global_position_invalid_relaxed
|
||||||
|
&& reporter.failsafeFlags().mode_req_global_position_relaxed != 0) {
|
||||||
|
global_position_modes = global_position_modes | (NavModes)reporter.failsafeFlags().mode_req_global_position_relaxed;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (global_position_modes != NavModes::None) {
|
||||||
/* EVENT
|
/* EVENT
|
||||||
* @description
|
* @description
|
||||||
* The available positioning data is not sufficient to execute the selected mode.
|
* The available positioning data is not sufficient to execute the selected mode.
|
||||||
*/
|
*/
|
||||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position,
|
reporter.armingCheckFailure(global_position_modes,
|
||||||
health_component_t::global_position_estimate,
|
health_component_t::global_position_estimate,
|
||||||
events::ID("check_modes_global_pos"),
|
events::ID("check_modes_global_pos"),
|
||||||
events::Log::Error, "Navigation error: No valid global position estimate");
|
events::Log::Error, "Navigation error: No valid global position estimate");
|
||||||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
|
reporter.clearCanRunBits(global_position_modes);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
|
if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
|
||||||
|
|||||||
@@ -50,6 +50,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
|
|||||||
flags.mode_req_local_position = 0;
|
flags.mode_req_local_position = 0;
|
||||||
flags.mode_req_local_position_relaxed = 0;
|
flags.mode_req_local_position_relaxed = 0;
|
||||||
flags.mode_req_global_position = 0;
|
flags.mode_req_global_position = 0;
|
||||||
|
flags.mode_req_global_position_relaxed = 0;
|
||||||
flags.mode_req_local_alt = 0;
|
flags.mode_req_local_alt = 0;
|
||||||
flags.mode_req_mission = 0;
|
flags.mode_req_mission = 0;
|
||||||
flags.mode_req_offboard_signal = 0;
|
flags.mode_req_offboard_signal = 0;
|
||||||
@@ -85,8 +86,16 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
|
|||||||
// NAVIGATION_STATE_AUTO_MISSION
|
// NAVIGATION_STATE_AUTO_MISSION
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
|
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
|
||||||
|
}
|
||||||
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);
|
||||||
@@ -94,16 +103,32 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
|
|||||||
// NAVIGATION_STATE_AUTO_LOITER
|
// NAVIGATION_STATE_AUTO_LOITER
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
|
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position_relaxed);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position_relaxed);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
|
||||||
|
}
|
||||||
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);
|
||||||
|
|
||||||
// NAVIGATION_STATE_AUTO_RTL
|
// NAVIGATION_STATE_AUTO_RTL
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);
|
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position_relaxed);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position_relaxed);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);
|
||||||
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
|
||||||
|
}
|
||||||
|
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);
|
||||||
|
|||||||
@@ -513,12 +513,13 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
|
|||||||
PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
|
PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Horizontal position error threshold.
|
* Horizontal position error threshold for hovering systems
|
||||||
*
|
*
|
||||||
* This is the horizontal position error (EPH) threshold that will trigger a failsafe.
|
* This is the horizontal position error (EPH) threshold that will trigger a failsafe.
|
||||||
* The default is appropriate for a multicopter. Can be increased for a fixed-wing.
|
|
||||||
* If the previous position error was below this threshold, there is an additional
|
* If the previous position error was below this threshold, there is an additional
|
||||||
* factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
|
* factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
|
||||||
|
* Only used for multicopters and VTOLs in hover mode.
|
||||||
|
* Independent from estimator positioning data timeout threshold (see EKF2_NOAID_TOUT).
|
||||||
*
|
*
|
||||||
* Set to -1 to disable.
|
* Set to -1 to disable.
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -532,7 +532,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
|||||||
// trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter)
|
// trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter)
|
||||||
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
|
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
|
||||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
|
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
|
||||||
CHECK_FAILSAFE(status_flags, local_position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
|
CHECK_FAILSAFE(status_flags, position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
|
||||||
}
|
}
|
||||||
|
|
||||||
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
|
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
|
||||||
|
|||||||
@@ -697,6 +697,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
|
|||||||
(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
|
(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
|
||||||
(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
|
(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
|
||||||
(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
|
(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
|
||||||
|
(!status_flags.global_position_invalid_relaxed || ((status_flags.mode_req_global_position_relaxed & mode_mask) == 0)) &&
|
||||||
(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
|
(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
|
||||||
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
|
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
|
||||||
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
|
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
|
||||||
|
|||||||
Reference in New Issue
Block a user