dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: use new failsafe state machine and add user intention class

This commit is contained in:
Beat Küng 2022-08-30 19:26:59 +02:00 committed by Daniel Agar
parent a04230faa1
commit 455b885f86
44 changed files with 1927 additions and 2280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -59,7 +59,6 @@ set(msg_files
cellular_status.msg
collision_constraints.msg
collision_report.msg
commander_state.msg
control_allocator_status.msg
cpuload.msg
differential_pressure.msg

2
msg/action_request.msg
View File

@ -16,4 +16,4 @@ uint8 SOURCE_RC_SWITCH = 1
uint8 SOURCE_RC_BUTTON = 2
uint8 SOURCE_RC_MODE_SLOT = 3
uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_
uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to vehicle_status_s::NAVIGATION_STATE_*

24
msg/commander_state.msg
View File

@ -1,24 +0,0 @@
# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
uint64 timestamp # time since system start (microseconds)
uint8 MAIN_STATE_MANUAL = 0
uint8 MAIN_STATE_ALTCTL = 1
uint8 MAIN_STATE_POSCTL = 2
uint8 MAIN_STATE_AUTO_MISSION = 3
uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_STAB = 8
# LEGACY RATTITUDE = 9
uint8 MAIN_STATE_AUTO_TAKEOFF = 10
uint8 MAIN_STATE_AUTO_LAND = 11
uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
uint8 MAIN_STATE_AUTO_PRECLAND = 13
uint8 MAIN_STATE_ORBIT = 14
uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF = 15
uint8 MAIN_STATE_MAX = 16
uint8 main_state
uint16 main_state_changes

8
msg/vehicle_status.msg
View File

@ -33,8 +33,9 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13
uint64 nav_state_timestamp # time when current nav_state activated
# Navigation state "what should vehicle do"
uint8 nav_state
uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation)
uint8 nav_state # Currently active mode
uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
@ -83,6 +84,7 @@ uint8 VEHICLE_TYPE_ROVER = 3
uint8 VEHICLE_TYPE_AIRSHIP = 4
bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
uint64 failsafe_timestamp # time when failsafe was activated
# Link loss
@ -97,8 +99,6 @@ bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotatio
bool in_transition_mode # True if VTOL is doing a transition
bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
bool mission_failure # Set to true if mission could not continue/finish
bool geofence_violated
# MAVLink identification
uint8 system_type # system type, contains mavlink MAV_TYPE

12
msg/vehicle_status_flags.msg
View File

@ -36,7 +36,6 @@ bool home_position_valid # Home position valid
bool offboard_control_signal_lost # Offboard signal lost
bool geofence_violated # Geofence violated
bool rc_signal_found_once
bool rc_signal_lost # RC signal lost
bool data_link_lost # Data link lost
bool mission_failure # Mission failure
@ -46,5 +45,14 @@ bool vtol_transition_failure # VTOL transition failed
bool battery_low_remaining_time
bool battery_unhealthy
uint8 battery_warning
bool wind_limit_exceeded
bool flight_time_limit_exceeded
# failure detector
bool fd_esc_arming_failure
bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS)
bool fd_imbalanced_prop # Imbalanced propeller detected
bool fd_motor_failure
uint8 battery_warning # Battery warning level

82
src/lib/events/enums.json
View File

@ -276,41 +276,83 @@
}
}
},
"failsafe_reason_t": {
"failsafe_cause_t": {
"type": "uint8_t",
"description": "Reason for entering failsafe",
"description": "Cause for entering failsafe",
"entries": {
"0": {
"name": "no_rc",
"description": "No manual control stick input"
"name": "generic",
"description": ""
},
"1": {
"name": "no_offboard",
"description": "No offboard control inputs"
"name": "rc_loss",
"description": "manual control loss"
},
"2": {
"name": "no_rc_and_no_offboard",
"description": "No manual control stick and no offboard control inputs"
"name": "datalink_loss",
"description": "data link loss"
},
"3": {
"name": "no_local_position",
"description": "No local position estimate"
"name": "low_battery_level",
"description": "low battery level"
},
"4": {
"name": "no_global_position",
"description": "No global position estimate"
"name": "critical_battery_level",
"description": "critical battery level"
},
"5": {
"name": "no_datalink",
"description": "No datalink"
"name": "emergency_battery_level",
"description": "emergency battery level"
}
}
},
"failsafe_action_t": {
"type": "uint8_t",
"description": "failsafe action",
"entries": {
"0": {
"name": "none",
"description": ""
},
"1": {
"name": "warn",
"description": "warning"
},
"2": {
"name": "fallback_posctrl",
"description": "fallback to position control"
},
"3": {
"name": "fallback_altctrl",
"description": "fallback to altitude control"
},
"4": {
"name": "fallback_stabilized",
"description": "fallback to stabilized"
},
"5": {
"name": "hold",
"description": "hold"
},
"6": {
"name": "no_rc_and_no_datalink",
"description": "No RC and no datalink"
"name": "rtl",
"description": "RTL"
},
"7": {
"name": "no_gps",
"description": "No valid GPS"
"name": "land",
"description": "land"
},
"8": {
"name": "descend",
"description": "descend"
},
"9": {
"name": "disarm",
"description": "disarm"
},
"10": {
"name": "terminate",
"description": "terminate"
}
}
},
@ -373,6 +415,10 @@
"13": {
"name": "rc_button",
"description": "RC (button)"
},
"14": {
"name": "failsafe",
"description": "failsafe"
}
}
},

6
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
View File

@ -41,7 +41,11 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
} transition_result_t;
using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;

2
src/modules/commander/CMakeLists.txt
View File

@ -52,12 +52,12 @@ px4_add_module(
factory_calibration_storage.cpp
gyro_calibration.cpp
HomePosition.cpp
UserModeIntention.cpp
level_calibration.cpp
lm_fit.cpp
mag_calibration.cpp
rc_calibration.cpp
Safety.cpp
state_machine_helper.cpp
worker_thread.cpp
DEPENDS
circuit_breaker

854
src/modules/commander/Commander.cpp
View File

File diff suppressed because it is too large Load Diff

85
src/modules/commander/Commander.hpp
View File

@ -36,11 +36,12 @@
/* Helper classes */
#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "failsafe/failsafe.h"
#include "Safety.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include "HomePosition.hpp"
#include "UserModeIntention.hpp"
#include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
@ -68,7 +69,6 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mission_result.h>
@ -84,7 +84,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind.h>
using math::constrain;
using systemlib::Hysteresis;
@ -117,8 +116,6 @@ public:
void enable_hil();
void get_circuit_breaker_params();
private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
@ -152,7 +149,7 @@ private:
void offboard_control_update();
void print_reject_mode(uint8_t main_state);
void print_reject_mode(uint8_t nav_state);
void update_control_mode();
@ -160,7 +157,6 @@ private:
void send_parachute_command();
void checkWindSpeedThresholds();
void checkForMissionUpdate();
void handlePowerButtonState();
void systemPowerUpdate();
@ -172,6 +168,7 @@ private:
bool getPrearmState() const;
void handleAutoDisarm();
bool handleModeIntentionAndFailsafe();
void updateParameters();
@ -179,7 +176,6 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
@ -187,19 +183,9 @@ private:
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
(ParamFloat<px4::params::COM_BAT_ACT_T>) _param_com_bat_act_t,
(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
@ -209,40 +195,17 @@ private:
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
// Quadchute
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
// Offboard
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
// Engine failure
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
// Circuit breakers
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action
)
// optional parameters
@ -262,34 +225,16 @@ private:
OFFBOARD_MODE_BIT = (1 << 1),
};
enum class ActuatorFailureActions {
DISABLED = 0,
AUTO_LOITER = 1,
AUTO_LAND = 2,
AUTO_RTL = 3,
TERMINATE = 4,
};
/* Decouple update interval and hysteresis counters, all depends on intervals */
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
ArmStateMachine _arm_state_machine{};
bool _geofence_loiter_on{false};
bool _geofence_rtl_on{false};
bool _geofence_land_on{false};
bool _geofence_warning_action_on{false};
bool _geofence_violated_prev{false};
bool _circuit_breaker_flight_termination_disabled{false};
bool _rtl_time_actions_done{false};
FailureDetector _failure_detector;
FailureDetector _failure_detector{this};
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
bool _imbalanced_propeller_check_triggered{false};
hrt_abstime _datalink_last_heartbeat_gcs{0};
@ -306,7 +251,6 @@ private:
hrt_abstime _high_latency_datalink_lost{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _battery_failsafe_timestamp{0};
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
Hysteresis _offboard_available{false};
@ -316,8 +260,6 @@ private:
bool _last_overload{false};
hrt_abstime _last_valid_manual_control_setpoint{0};
bool _is_throttle_above_center{false};
bool _is_throttle_low{false};
@ -333,18 +275,13 @@ private:
bool _status_changed{true};
bool _arm_tune_played{false};
bool _was_armed{false};
bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag
bool _have_taken_off_since_arming{false};
geofence_result_s _geofence_result{};
vehicle_land_detected_s _vehicle_land_detected{};
vtol_vehicle_status_s _vtol_vehicle_status{};
hrt_abstime _last_wind_warning{0};
// commander publications
actuator_armed_s _actuator_armed{};
commander_state_s _commander_state{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
vehicle_status_flags_s _vehicle_status_flags{};
@ -356,14 +293,12 @@ private:
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -378,7 +313,6 @@ private:
// Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
@ -391,6 +325,11 @@ private:
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
HealthAndArmingChecks _health_and_arming_checks;
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status_flags, _vehicle_status};
HomePosition _home_position{_vehicle_status_flags};
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance};
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
};

7
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
View File

@ -53,6 +53,13 @@ px4_add_library(health_and_arming_checks
checks/sdcardCheck.cpp
checks/parachuteCheck.cpp
checks/batteryCheck.cpp
checks/windCheck.cpp
checks/geofenceCheck.cpp
checks/homePositionCheck.cpp
checks/flightTimeCheck.cpp
checks/missionCheck.cpp
checks/rcAndDataLinkCheck.cpp
checks/vtolCheck.cpp
)
add_dependencies(health_and_arming_checks mode_util)

2
src/modules/commander/HealthAndArmingChecks/Common.hpp
View File

@ -254,6 +254,8 @@ public:
const HealthResults &healthResults() const { return _results[_current_result].health; }
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
bool modePreventsArming(uint8_t nav_state) const { return _status_flags.mode_req_prevent_arming & (1u << nav_state); }
private:
/**

30
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
View File

@ -51,6 +51,7 @@
#include "checks/imuConsistencyCheck.hpp"
#include "checks/magnetometerCheck.hpp"
#include "checks/manualControlCheck.hpp"
#include "checks/homePositionCheck.hpp"
#include "checks/modeCheck.hpp"
#include "checks/parachuteCheck.hpp"
#include "checks/powerCheck.hpp"
@ -58,6 +59,12 @@
#include "checks/sdcardCheck.hpp"
#include "checks/systemCheck.hpp"
#include "checks/batteryCheck.hpp"
#include "checks/windCheck.hpp"
#include "checks/geofenceCheck.hpp"
#include "checks/flightTimeCheck.hpp"
#include "checks/missionCheck.hpp"
#include "checks/rcAndDataLinkCheck.hpp"
#include "checks/vtolCheck.hpp"
class HealthAndArmingChecks : public ModuleParams
@ -84,6 +91,11 @@ public:
*/
bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); }
/**
* Query the mode requirements: check if a mode prevents arming
*/
bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
protected:
void updateParams() override;
private:
@ -106,6 +118,7 @@ private:
ImuConsistencyChecks _imu_consistency_checks;
MagnetometerChecks _magnetometer_checks;
ManualControlChecks _manual_control_checks;
HomePositionChecks _home_position_checks;
ModeChecks _mode_checks;
ParachuteChecks _parachute_checks;
PowerChecks _power_checks;
@ -113,6 +126,12 @@ private:
SdCardChecks _sd_card_checks;
SystemChecks _system_checks;
BatteryChecks _battery_checks;
WindChecks _wind_checks;
GeofenceChecks _geofence_checks;
FlightTimeChecks _flight_time_checks;
MissionChecks _mission_checks;
RcAndDataLinkChecks _rc_and_data_link_checks;
VtolChecks _vtol_checks;
HealthAndArmingCheckBase *_checks[30] = {
&_accelerometer_checks,
@ -127,13 +146,20 @@ private:
&_imu_consistency_checks,
&_magnetometer_checks,
&_manual_control_checks,
&_mode_checks, // must be after _estimator_checks
&_home_position_checks,
&_mode_checks, // must be after _estimator_checks & _home_position_checks
&_parachute_checks,
&_power_checks,
&_rc_calibration_checks,
&_sd_card_checks,
&_system_checks,
&_system_checks, // must be after _estimator_checks & _home_position_checks
&_battery_checks,
&_wind_checks,
&_geofence_checks, // must be after _home_position_checks
&_flight_time_checks,
&_mission_checks,
&_rc_and_data_link_checks,
&_vtol_checks,
};
};

8
src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
View File

@ -199,7 +199,8 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
reporter.failsafeFlags().battery_warning = worst_warning;
}
if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) {
if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE
&& reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) {
bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL;
NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None;
events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning;
@ -222,12 +223,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
// There is at least one connected battery (in any slot)
|| num_connected_batteries < battery_required_count
// No currently-connected batteries have any fault
|| battery_has_fault;
|| battery_has_fault
|| reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED;
if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already
/* EVENT
* @description
* Make sure all batteries are connected.
* Make sure all batteries are connected and operational.
*/
reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
events::Log::Error, "Battery unhealthy");

186
src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp
View File

@ -35,81 +35,125 @@
void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
{
if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) {
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured roll angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_R</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
events::Log::Critical, "Attitude failure detected");
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured roll angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_R</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
events::Log::Critical, "Attitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
}
} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured pitch angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_P</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
events::Log::Critical, "Attitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
}
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
}
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
/* EVENT
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
events::Log::Critical, "Altitude failure detected");
} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
/* EVENT
* @description
* The vehicle exceeded the maximum configured pitch angle.
*
* <profile name="dev">
* This check can be configured via <param>FD_FAIL_P</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
events::Log::Critical, "Attitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
}
}
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
events::Log::Critical, "Failure triggered by external system");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
}
}
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
/* EVENT
* @description
* One or more ESCs failed to arm.
*
* <profile name="dev">
* This check can be configured via <param>FD_ESCS_EN</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
events::Log::Critical, "ESC failure");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
}
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
}
}
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
/* EVENT
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
events::Log::Critical, "Altitude failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
}
}
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
events::Log::Critical, "Failure triggered by external system");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
}
}
reporter.failsafeFlags().fd_critical_failure = context.status().failure_detector_status &
(vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT |
vehicle_status_s::FAILURE_EXT);
reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status &
vehicle_status_s::FAILURE_ARM_ESC;
if (reporter.failsafeFlags().fd_esc_arming_failure) {
/* EVENT
* @description
* One or more ESCs failed to arm.
*
* <profile name="dev">
* This check can be configured via <param>FD_ESCS_EN</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
events::Log::Critical, "ESC failure");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
}
}
reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status &
vehicle_status_s::FAILURE_IMBALANCED_PROP;
if (reporter.failsafeFlags().fd_imbalanced_prop) {
/* EVENT
* @description
* Check that all propellers are mounted correctly and are not damaged.
*
* <profile name="dev">
* This check can be configured via <param>FD_IMB_PROP_THR</param> and <param>COM_IMB_PROP_ACT</param> parameters.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_imbalanced_prop"),
events::Log::Critical, "Imbalanced propeller detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Imbalanced propeller detected");
}
}
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
if (reporter.failsafeFlags().fd_motor_failure) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"),
events::Log::Critical, "Motor failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected");
}
}
}

58
src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "flightTimeCheck.hpp"
void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
{
if (_param_com_flt_time_max.get() > FLT_EPSILON && context.status().takeoff_time != 0 &&
(hrt_absolute_time() - context.status().takeoff_time) > (1_s * _param_com_flt_time_max.get())) {
reporter.failsafeFlags().flight_time_limit_exceeded = true;
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_FLT_TIME_MAX</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_flight_time_limit"),
events::Log::Error, "Maximum flight time reached");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Maximum flight time reached\t");
}
} else {
reporter.failsafeFlags().flight_time_limit_exceeded = false;
}
}

50
src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.hpp Normal file
View File

@ -0,0 +1,50 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
class FlightTimeChecks : public HealthAndArmingCheckBase
{
public:
FlightTimeChecks() = default;
~FlightTimeChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max
)
};

76
src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "geofenceCheck.hpp"
void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
{
geofence_result_s geofence_result;
if (!_geofence_result_sub.copy(&geofence_result)) {
geofence_result = {};
}
reporter.failsafeFlags().geofence_violated = geofence_result.geofence_violated;
if (geofence_result.geofence_action != 0 && reporter.failsafeFlags().geofence_violated) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_violation"),
events::Log::Error, "Vehicle outside geofence");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence");
}
}
if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL
&& !reporter.failsafeFlags().home_position_valid) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_no_home"),
events::Log::Error, "Geofence RTL requires valid home");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence RTL requires valid home");
}
}
}

51
src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.hpp Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
class GeofenceChecks : public HealthAndArmingCheckBase
{
public:
GeofenceChecks() = default;
~GeofenceChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
};

48
src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "homePositionCheck.hpp"
void HomePositionChecks::checkAndReport(const Context &context, Report &reporter)
{
home_position_s home_position;
if (_home_position_sub.copy(&home_position)) {
reporter.failsafeFlags().home_position_valid = home_position.valid_alt && home_position.valid_hpos;
} else {
reporter.failsafeFlags().home_position_valid = false;
}
// No need to report, as it's a mode requirement
}

51
src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.hpp Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
class HomePositionChecks : public HealthAndArmingCheckBase
{
public:
HomePositionChecks() = default;
~HomePositionChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
};

57
src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp Normal file
View File

@ -0,0 +1,57 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "missionCheck.hpp"
void MissionChecks::checkAndReport(const Context &context, Report &reporter)
{
reporter.failsafeFlags().mission_failure = false;
mission_result_s mission_result;
if (_mission_result_sub.copy(&mission_result) && mission_result.valid) {
reporter.failsafeFlags().mission_failure = mission_result.failure;
if (reporter.failsafeFlags().mission_failure) {
// navigator sends out the exact reason
/* EVENT
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_mission_failure"),
events::Log::Error, "Mission cannot be completed");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t");
}
}
}
}

51
src/modules/commander/HealthAndArmingChecks/checks/missionCheck.hpp Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/mission_result.h>
class MissionChecks : public HealthAndArmingCheckBase
{
public:
MissionChecks() = default;
~MissionChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
};

2
src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
View File

@ -109,7 +109,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
events::ID("check_modes_mission"),
events::Log::Info, "No mission available");
events::Log::Info, "No valid mission available");
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
}

129
src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp Normal file
View File

@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "rcAndDataLinkCheck.hpp"
using namespace time_literals;
void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
{
// RC
bool rc_is_optional = true;
if (_param_com_rc_in_mode.get() == 4) { // RC disabled
reporter.failsafeFlags().rc_signal_lost = false;
} else {
manual_control_setpoint_s manual_control_setpoint;
if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
manual_control_setpoint = {};
reporter.failsafeFlags().rc_signal_lost = true;
}
// Check if RC is valid
if (!manual_control_setpoint.valid
|| hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
if (!reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) {
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
"Manual control lost");
}
reporter.failsafeFlags().rc_signal_lost = true;
} else {
reporter.setIsPresent(health_component_t::remote_control);
if (reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) {
float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
"Manual control regained after {1:.1} s", elapsed);
}
reporter.failsafeFlags().rc_signal_lost = false;
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
}
if (reporter.failsafeFlags().rc_signal_lost) {
NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All;
events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error;
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_RC_IN_MODE</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::remote_control, events::ID("check_rc_dl_no_rc"),
log_level, "No manual control input");
if (reporter.mavlink_log_pub()) {
mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: No manual control input\t");
}
}
}
// Data Link
reporter.failsafeFlags().data_link_lost = context.status().data_link_lost;
if (reporter.failsafeFlags().data_link_lost) {
// Prevent arming if we neither have RC nor a Data Link. TODO: disabled for now due to MAVROS tests
bool data_link_required = _param_nav_dll_act.get() > 0
/*|| (rc_is_optional && reporter.failsafeFlags().rc_signal_lost) */;
NavModes affected_modes = data_link_required ? NavModes::All : NavModes::None;
events::LogLevel log_level = data_link_required ? events::Log::Error : events::Log::Info;
/* EVENT
* @description
* To arm, at least a data link or manual control (RC) must be present.
*
* <profile name="dev">
* This check can be configured via <param>NAV_DLL_ACT</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::communication_links,
events::ID("check_rc_dl_no_dllink"),
log_level, "No connection to the ground control station");
if (reporter.mavlink_log_pub()) {
mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
}
} else {
reporter.setIsPresent(health_component_t::communication_links);
}
}

59
src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.hpp Normal file
View File

@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
class RcAndDataLinkChecks : public HealthAndArmingCheckBase
{
public:
RcAndDataLinkChecks() = default;
~RcAndDataLinkChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
hrt_abstime _last_valid_manual_control_setpoint{0};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act
)
};

19
src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp
View File

@ -191,21 +191,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
}
}
if (_param_gf_action.get() != 0 && context.status().geofence_violated) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_gf_violation"),
events::Log::Error, "Vehicle outside geofence");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence");
}
}
// Arm Requirements: authorization
if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) {
if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
@ -215,8 +200,4 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
events::Log::Error, "Arm authorization denied");
}
}
if (reporter.failsafeFlags().rc_signal_found_once) {
reporter.setIsPresent(health_component_t::remote_control);
}
}

56
src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.cpp Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "vtolCheck.hpp"
using namespace time_literals;
void VtolChecks::checkAndReport(const Context &context, Report &reporter)
{
vtol_vehicle_status_s vtol_vehicle_status;
if (_vtol_vehicle_status_sub.copy(&vtol_vehicle_status)) {
reporter.failsafeFlags().vtol_transition_failure = vtol_vehicle_status.vtol_transition_failsafe;
if (reporter.failsafeFlags().vtol_transition_failure) {
/* EVENT
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_vtol_transition_failure"),
events::Log::Error, "VTOL transition failure");
if (reporter.mavlink_log_pub()) {
mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: VTOL transition failure\t");
}
}
}
}

51
src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.hpp Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/vtol_vehicle_status.h>
class VtolChecks : public HealthAndArmingCheckBase
{
public:
VtolChecks() = default;
~VtolChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
};

79
src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "windCheck.hpp"
void WindChecks::checkAndReport(const Context &context, Report &reporter)
{
if (_param_com_wind_warn.get() < FLT_EPSILON && _param_com_wind_max.get() < FLT_EPSILON) {
reporter.failsafeFlags().wind_limit_exceeded = false;
return;
}
wind_s wind_estimate;
const hrt_abstime now = hrt_absolute_time();
if (_wind_sub.copy(&wind_estimate)) {
const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
// publish a warning if it's the first since in air or 60s have passed since the last warning
const bool warning_timeout_passed = _last_wind_warning == 0 || now - _last_wind_warning > 60_s;
reporter.failsafeFlags().wind_limit_exceeded = _param_com_wind_max.get() > FLT_EPSILON
&& wind.longerThan(_param_com_wind_max.get());
if (reporter.failsafeFlags().wind_limit_exceeded) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_WIND_MAX</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<float>(NavModes::All, health_component_t::system,
events::ID("check_wind_too_high"),
events::Log::Warning, "Wind speed is above limit ({1:.1m/s})", wind.norm());
} else if (_param_com_wind_warn.get() > FLT_EPSILON
&& wind.longerThan(_param_com_wind_warn.get())
&& warning_timeout_passed
&& context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
&& context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
events::send<float>(events::ID("check_high_wind_warning"),
{events::Log::Warning, events::LogInternal::Info},
"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
_last_wind_warning = now;
}
}
}

58
src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/wind.h>
class WindChecks : public HealthAndArmingCheckBase
{
public:
WindChecks() = default;
~WindChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _wind_sub{ORB_ID(wind)};
hrt_abstime _last_wind_warning{0};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn
)
};

110
src/modules/commander/ModeUtil/conversions.hpp Normal file
View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/events.h>
#include <uORB/topics/vehicle_status.h>
#include <stdint.h>
using navigation_mode_t = events::px4::enums::navigation_mode_t;
namespace mode_util
{
static inline navigation_mode_t navigation_mode(uint8_t nav_state)
{
switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: return navigation_mode_t::manual;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return navigation_mode_t::altctl;
case vehicle_status_s::NAVIGATION_STATE_POSCTL: return navigation_mode_t::posctl;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
case vehicle_status_s::NAVIGATION_STATE_ACRO: return navigation_mode_t::acro;
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: return navigation_mode_t::offboard;
case vehicle_status_s::NAVIGATION_STATE_STAB: return navigation_mode_t::stab;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return navigation_mode_t::auto_land;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target;
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
}
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "code requires update");
return navigation_mode_t::unknown;
}
const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"MANUAL",
"ALTCTL",
"POSCTL",
"AUTO_MISSION",
"AUTO_LOITER",
"AUTO_RTL",
"6: unallocated",
"7: unallocated",
"AUTO_LANDENGFAIL",
"9: unallocated",
"ACRO",
"11: UNUSED",
"DESCEND",
"TERMINATION",
"OFFBOARD",
"STAB",
"16: UNUSED2",
"AUTO_TAKEOFF",
"AUTO_LAND",
"AUTO_FOLLOW_TARGET",
"AUTO_PRECLAND",
"ORBIT"
};
} // namespace mode_util

83
src/modules/commander/UserModeIntention.cpp Normal file
View File

@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "UserModeIntention.hpp"
UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
const HealthAndArmingChecks &health_and_arming_checks)
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
{
}
bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
{
_ever_had_mode_change = true;
_had_mode_change = true;
if (_user_intented_nav_state == user_intended_nav_state) {
return true;
}
// Always allow mode change while disarmed
bool always_allow = force || !isArmed();
bool allow_change = true;
if (!always_allow) {
allow_change = _health_and_arming_checks.canRun(user_intended_nav_state);
// Check fallback
if (!allow_change && allow_fallback && _param_com_posctl_navl.get() == 0) {
if (user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
allow_change = _health_and_arming_checks.canRun(vehicle_status_s::NAVIGATION_STATE_ALTCTL);
// We still use the original user intended mode. The failsafe state machine will then set the
// fallback and once can_run becomes true, the actual user intended mode will be selected.
}
}
}
if (allow_change) {
_user_intented_nav_state = user_intended_nav_state;
if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
_nav_state_after_disarming = user_intended_nav_state;
}
}
return allow_change;
}
void UserModeIntention::onDisarm()
{
_user_intented_nav_state = _nav_state_after_disarming;
}

85
src/modules/commander/UserModeIntention.hpp Normal file
View File

@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <uORB/topics/vehicle_status.h>
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include <px4_platform_common/module_params.h>
class UserModeIntention : ModuleParams
{
public:
UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
const HealthAndArmingChecks &health_and_arming_checks);
~UserModeIntention() = default;
/**
* Change the user intended mode
* @param user_intended_nav_state new mode
* @param allow_fallback allow to fallback to a lower mode if current mode cannot run
* @param force always set if true
* @return true if successfully set (also if unchanged)
*/
bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
uint8_t get() const { return _user_intented_nav_state; }
/**
* Change the user intention to the last user intended mode where arming is possible
*/
void onDisarm();
/**
* Returns false if there has not been any mode change yet
*/
bool everHadModeChange() const { return _ever_had_mode_change; }
bool getHadModeChangeAndClear() { bool ret = _had_mode_change; _had_mode_change = false; return ret; }
private:
bool isArmed() const { return _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; }
const vehicle_status_s &_vehicle_status;
const HealthAndArmingChecks &_health_and_arming_checks;
uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
bool _ever_had_mode_change{false}; ///< true if there was ever a mode change call (also if the same mode as already set)
bool _had_mode_change{false}; ///< true if there was a mode change call since the last getHadModeChangeAndClear()
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl
);
};

53
src/modules/commander/commander_params.c
View File

@ -283,13 +283,14 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
/**
* Delay between battery state change and failsafe reaction
* Delay between failsafe condition triggered and failsafe reaction
*
* Battery state requires action -> wait COM_BAT_ACT_T seconds in Hold mode
* for the user to realize and take a custom action
* -> React with failsafe action COM_LOW_BAT_ACT
* Before entering failsafe (RTL, Land, Hold), wait COM_FAIL_ACT_T seconds in Hold mode
* for the user to realize.
* During that time the user cannot take over control.
* Afterwards the configured failsafe action is triggered and the user may take over.
*
* A zero value disables the delay.
* A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed).
*
* @group Commander
* @unit s
@ -297,7 +298,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
* @max 25.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f);
PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
/**
* Imbalanced propeller failsafe mode
@ -319,7 +320,7 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
/**
* Time-out to wait when offboard connection is lost before triggering offboard lost action.
*
* See COM_OBL_ACT and COM_OBL_RC_ACT to configure action.
* See COM_OBL_RC_ACT to configure action.
*
* @group Commander
* @unit s
@ -329,27 +330,10 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
*/
PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
/**
* Set offboard loss failsafe mode
*
* The offboard loss failsafe will only be entered after a timeout,
* set by COM_OF_LOSS_T in seconds.
*
* @value -1 Disabled
* @value 0 Land mode
* @value 1 Hold mode
* @value 2 Return mode
* @value 3 Terminate
* @value 4 Lockdown
*
* @group Commander
*/
PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
/**
* Set command after a quadchute
*
* @value -1 No action: stay in current flight mode
* @value -1 Warning only
* @value 0 Return mode
* @value 1 Land mode
* @value 2 Hold mode
@ -358,12 +342,11 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
PARAM_DEFINE_INT32(COM_QC_ACT, 0);
/**
* Set offboard loss failsafe mode when RC is available
* Set offboard loss failsafe mode
*
* The offboard loss failsafe will only be entered after a timeout,
* set by COM_OF_LOSS_T in seconds.
*
* @value -1 Disabled
* @value 0 Position mode
* @value 1 Altitude mode
* @value 2 Manual
@ -371,7 +354,7 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
* @value 4 Land mode
* @value 5 Hold mode
* @value 6 Terminate
* @value 7 Lockdown
* @value 7 Disarm
* @group Commander
*/
PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@ -676,10 +659,10 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
*
* If Altitude/Manual is selected: assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
*
* If Land/Terminate is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
* If Land/Descend is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to Descend.
*
* @value 0 Altitude/Manual
* @value 1 Land/Terminate
* @value 1 Land/Descend
*
* @group Commander
*/
@ -813,7 +796,7 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
* @value 2 Return mode
* @value 3 Land mode
* @value 5 Terminate
* @value 6 Lockdown
* @value 6 Disarm
* @min 0
* @max 6
*
@ -832,7 +815,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
* @value 2 Return mode
* @value 3 Land mode
* @value 5 Terminate
* @value 6 Lockdown
* @value 6 Disarm
* @min 1
* @max 6
*
@ -862,7 +845,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
*
* @min 0
* @max 3
* @value 0 Disabled
* @value 0 Warning only
* @value 1 Hold mode
* @value 2 Land mode
* @value 3 Return mode
@ -988,8 +971,8 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);
/**
* Timeout for detecting a failure after takeoff
*
* A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into
* a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R.
* A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to disarm the vehicle
* if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R.
* The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW) and Manual (FW).
* A zero or negative value means that the check is disabled.
*

288
src/modules/commander/failsafe/failsafe.cpp
View File

@ -35,8 +35,12 @@
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <lib/circuit_breaker/circuit_breaker.h>
FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) const
using namespace time_literals;
FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
ActionOptions options{};
@ -77,7 +81,7 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) c
return options;
}
FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const
FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
{
ActionOptions options{};
@ -119,9 +123,211 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const
return options;
}
FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case -1:
default:
options.action = Action::None;
break;
case 0:
options.action = Action::Warn;
break;
case 1:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case 0:
default:
options.action = Action::Warn;
break;
case 1:
options.action = Action::Hold;
break;
case 2:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 3:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 4:
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value, uint8_t battery_warning)
{
ActionOptions options{};
switch (battery_warning) {
default:
case battery_status_s::BATTERY_WARNING_NONE:
options.action = Action::None;
break;
case battery_status_s::BATTERY_WARNING_LOW:
options.action = Action::Warn;
options.cause = Cause::BatteryLow;
break;
case battery_status_s::BATTERY_WARNING_CRITICAL:
options.action = Action::Warn;
options.cause = Cause::BatteryCritical;
switch ((LowBatteryAction)param_value) {
case LowBatteryAction::Return:
case LowBatteryAction::ReturnOrLand:
options.action = Action::RTL;
break;
case LowBatteryAction::Land:
options.action = Action::Land;
break;
case LowBatteryAction::Warning:
options.action = Action::Warn;
break;
}
break;
case battery_status_s::BATTERY_WARNING_EMERGENCY:
options.action = Action::Warn;
options.cause = Cause::BatteryEmergency;
switch ((LowBatteryAction)param_value) {
case LowBatteryAction::Return:
options.action = Action::RTL;
break;
case LowBatteryAction::ReturnOrLand:
case LowBatteryAction::Land:
options.action = Action::Land;
break;
case LowBatteryAction::Warning:
options.action = Action::Warn;
break;
}
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case -1:
default:
options.action = Action::Warn;
break;
case 0:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 1:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
options.action = Action::Hold;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
}
return options;
}
FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode)
{
Action action{Action::None};
switch (param_value) {
case 0:
default:
action = Action::FallbackPosCtrl;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
case 1:
action = Action::FallbackAltCtrl;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case 2:
action = Action::FallbackStab;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case 3:
action = Action::RTL;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
break;
case 4:
action = Action::Land;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
break;
case 5:
action = Action::Hold;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
break;
case 6:
action = Action::Terminate;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
break;
case 7:
action = Action::Disarm;
break;
}
return action;
}
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags)
{
updateArmingState(time_us, state.armed, status_flags);
const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
|| state.vtol_in_transition_mode;
@ -131,6 +337,11 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
&& in_forward_flight && !state.mission_finished;
// RC loss
if (!status_flags.rc_signal_lost) {
// If RC was lost and arming was allowed, consider it optional until we regain RC
_rc_lost_at_arming = false;
}
const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Mission);
const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
@ -140,11 +351,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold);
const bool rc_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND ||
rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff ||
ignore_link_failsafe;
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
rc_loss_ignored_takeoff || ignore_link_failsafe || _rc_lost_at_arming;
if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) {
CHECK_FAILSAFE(status_flags, rc_signal_lost,
@ -160,12 +368,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::DatalinkLoss));
}
// VTOL transition failure
// VTOL transition failure (quadchute)
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_TAKEOFF ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
CHECK_FAILSAFE(status_flags, vtol_transition_failure, Action::RTL);
CHECK_FAILSAFE(status_flags, vtol_transition_failure, fromQuadchuteActParam(_param_com_qc_act.get()));
}
// Mission
@ -182,10 +390,53 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
}
CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded,
ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
CHECK_FAILSAFE(status_flags, geofence_violated, fromGfActParam(_param_gf_action.get()));
// Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW));
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
_last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical,
_last_state_battery_warning_critical,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL));
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
_last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency,
_last_state_battery_warning_emergency,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY));
}
// Failure detector
if (_armed_time != 0 && time_us - _armed_time < _param_com_spoolup_time.get() * 1_s) {
CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm);
}
if (_armed_time != 0 && time_us - _armed_time < (_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s) {
// This handles the case where something fails during the early takeoff phase
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Disarm);
} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate);
} else {
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
}
CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get()));
CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));
// Mode fallback (last)
Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode);
@ -194,6 +445,20 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
}
void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags)
{
if (!_was_armed && armed) {
_armed_time = time_us;
_rc_lost_at_arming = status_flags.rc_signal_lost;
} else if (!armed) {
_rc_lost_at_arming = status_flags.rc_signal_lost; // ensure action isn't added while disarmed
_armed_time = 0;
}
_was_armed = armed;
}
FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &status_flags,
uint8_t user_intended_mode) const
{
@ -201,8 +466,7 @@ FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &s
// offboard signal
if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
action = Action::FallbackPosCtrl; // TODO: use param
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);
}
// posctrl

40
src/modules/commander/failsafe/failsafe.h
View File

@ -51,28 +51,62 @@ protected:
uint8_t user_intended_mode) const override;
private:
void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags);
enum class RCLossExceptionBits : int32_t {
Mission = (1 << 0),
Hold = (1 << 1),
Offboard = (1 << 2)
};
ActionOptions fromNavDllOrRclActParam(int param_value) const;
// COM_LOW_BAT_ACT parameter values
enum class LowBatteryAction : int32_t {
Warning = 0, // Warning
Return = 1, // Return mode (deprecated)
Land = 2, // Land mode
ReturnOrLand = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
};
ActionOptions fromGfActParam(int param_value) const;
static ActionOptions fromNavDllOrRclActParam(int param_value);
static ActionOptions fromGfActParam(int param_value);
static ActionOptions fromImbalancedPropActParam(int param_value);
static ActionOptions fromActuatorFailureActParam(int param_value);
static ActionOptions fromBatteryWarningActParam(int param_value, uint8_t battery_warning);
static ActionOptions fromQuadchuteActParam(int param_value);
static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
const int _caller_id_mode_fallback{genCallerId()};
bool _last_state_mode_fallback{false};
const int _caller_id_mission_control_lost{genCallerId()};
bool _last_state_mission_control_lost{false};
const int _caller_id_battery_warning_low{genCallerId()}; ///< battery warning caller ID's: use different ID's as they have different actions
bool _last_state_battery_warning_low{false};
const int _caller_id_battery_warning_critical{genCallerId()};
bool _last_state_battery_warning_critical{false};
const int _caller_id_battery_warning_emergency{genCallerId()};
bool _last_state_battery_warning_emergency{false};
hrt_abstime _armed_time{0};
bool _was_armed{false};
bool _rc_lost_at_arming{false}; ///< true if RC was lost at arming time
DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase,
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act
);
};

3
src/modules/commander/failsafe/framework.cpp
View File

@ -458,6 +458,9 @@ void FailsafeBase::getSelectedAction(const State &state, const vehicle_status_fl
if (!_user_takeover_active) {
PX4_DEBUG("Activating user takeover");
#ifndef EMSCRIPTEN_BUILD
events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks");
#endif // EMSCRIPTEN_BUILD
}
// We must check for mode fallback again here

1120
src/modules/commander/state_machine_helper.cpp
View File

File diff suppressed because it is too large Load Diff

158
src/modules/commander/state_machine_helper.h
View File

@ -1,158 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file state_machine_helper.h
* State machine helper functions definitions
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef STATE_MACHINE_HELPER_H_
#define STATE_MACHINE_HELPER_H_
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <px4_platform_common/events.h>
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
} transition_result_t;
enum class link_loss_actions_t {
DISABLED = 0,
AUTO_LOITER = 1, // Hold mode
AUTO_RTL = 2, // Return mode
AUTO_LAND = 3, // Land mode
TERMINATE = 5, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values)
};
enum class quadchute_actions_t {
NO_ACTION = -1,
AUTO_RTL = 0, // Return mode
AUTO_LAND = 1, // Land mode
AUTO_LOITER = 2, // Hold mode
};
enum class offboard_loss_actions_t {
DISABLED = -1,
AUTO_LAND = 0, // Land mode
AUTO_LOITER = 1, // Hold mode
AUTO_RTL = 2, // Return mode
TERMINATE = 3, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 4, // Lock actuators (set actuator outputs to disarmed values)
};
enum class offboard_loss_rc_actions_t {
DISABLED = -1, // Disabled
MANUAL_POSITION = 0, // Position mode
MANUAL_ALTITUDE = 1, // Altitude mode
MANUAL_ATTITUDE = 2, // Manual
AUTO_RTL = 3, // Return mode
AUTO_LAND = 4, // Land mode
AUTO_LOITER = 5, // Hold mode
TERMINATE = 6, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 7, // Lock actuators (set actuator outputs to disarmed values)
};
enum class position_nav_loss_actions_t {
ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
};
extern const char *const nav_state_names[];
enum RCLossExceptionBits {
RCL_EXCEPT_MISSION = (1 << 0),
RCL_EXCEPT_HOLD = (1 << 1),
RCL_EXCEPT_OFFBOARD = (1 << 2)
};
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);
bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
const quadchute_actions_t quadchute_act,
const offboard_loss_rc_actions_t offb_loss_rc_act,
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t, const int param_com_rcl_except);
/*
* Checks the validty of position data against the requirements of the current navigation
* mode and switches mode if position data required is not available.
*/
bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
// COM_LOW_BAT_ACT parameter values
typedef enum LOW_BAT_ACTION {
WARNING = 0, // Warning
RETURN = 1, // Return mode
LAND = 2, // Land mode
RETURN_OR_LAND = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
} low_battery_action_t;
void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning,
const uint8_t failsafe_action, const float param_com_bat_act_t,
const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode);
uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, const uint8_t battery_warning,
const low_battery_action_t param_com_low_bat_act);
// COM_IMB_PROP_ACT parameter values
enum class imbalanced_propeller_action_t {
DISABLED = -1,
WARNING = 0,
RETURN = 1,
LAND = 2
};
void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state,
const imbalanced_propeller_action_t failsafe_action);
#endif /* STATE_MACHINE_HELPER_H_ */

47
src/modules/manual_control/ManualControl.cpp
View File

@ -35,7 +35,6 @@
#include <px4_platform_common/events.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_command.h>
ManualControl::ManualControl() :
@ -205,7 +204,7 @@ void ManualControl::Run()
if (switches.return_switch != _previous_switches.return_switch) {
if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
commander_state_s::MAIN_STATE_AUTO_RTL);
vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -215,7 +214,7 @@ void ManualControl::Run()
if (switches.loiter_switch != _previous_switches.loiter_switch) {
if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
commander_state_s::MAIN_STATE_AUTO_LOITER);
vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -225,7 +224,7 @@ void ManualControl::Run()
if (switches.offboard_switch != _previous_switches.offboard_switch) {
if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH,
commander_state_s::MAIN_STATE_OFFBOARD);
vehicle_status_s::NAVIGATION_STATE_OFFBOARD);
} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -364,27 +363,33 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot)
break;
case manual_control_switches_s::MODE_SLOT_1:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_1.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_1.get()));
break;
case manual_control_switches_s::MODE_SLOT_2:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_2.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_2.get()));
break;
case manual_control_switches_s::MODE_SLOT_3:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_3.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_3.get()));
break;
case manual_control_switches_s::MODE_SLOT_4:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_4.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_4.get()));
break;
case manual_control_switches_s::MODE_SLOT_5:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_5.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_5.get()));
break;
case manual_control_switches_s::MODE_SLOT_6:
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_6.get());
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
navStateFromParam(_param_fltmode_6.get()));
break;
default:
@ -520,6 +525,28 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint.
return 0;
}
int8_t ManualControl::navStateFromParam(int32_t param_value)
{
switch(param_value) {
case 0: return vehicle_status_s::NAVIGATION_STATE_MANUAL;
case 1: return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
case 2: return vehicle_status_s::NAVIGATION_STATE_POSCTL;
case 3: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
case 4: return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
case 5: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
case 6: return vehicle_status_s::NAVIGATION_STATE_ACRO;
case 7: return vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
case 8: return vehicle_status_s::NAVIGATION_STATE_STAB;
case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
}
return -1;
}
extern "C" __EXPORT int manual_control_main(int argc, char *argv[])
{
return ManualControl::main(argc, argv);

2
src/modules/manual_control/ManualControl.hpp
View File

@ -80,6 +80,8 @@ private:
void Run() override;
void processStickArming(const manual_control_setpoint_s &input);
static int8_t navStateFromParam(int32_t param_value);
void evaluateModeSlot(uint8_t mode_slot);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);
void publishLandingGear(int8_t action);

8
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -437,10 +437,6 @@ private:
msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
}
if (status.mission_failure) {
msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
}
// flight mode
union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)};
msg->custom_mode = custom_mode.custom_mode_hl;
@ -464,6 +460,10 @@ private:
msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK;
}
if (status_flags.mission_failure) {
msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
}
return true;
}

6
src/modules/navigator/navigator_main.cpp
View File

@ -808,7 +808,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
float test_point_bearing;
float test_point_distance;
float vertical_test_point_distance;
char geofence_violation_warning[50];
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
@ -843,12 +842,10 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
if (_geofence.getPredict()) {
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Approaching on geofence");
} else {
fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
vertical_test_point_distance = 0;
snprintf(geofence_violation_warning, sizeof(geofence_violation_warning), "Geofence exceeded");
}
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
@ -875,9 +872,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
/* Issue a warning about the geofence violation once and only if we are armed */
if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_critical(&_mavlink_log_pub, "%s", geofence_violation_warning);
events::send(events::ID("navigator_geofence_violation"), {events::Log::Warning, events::LogInternal::Info},
geofence_violation_warning);
// we have predicted a geofence violation and if the action is to loiter then
// demand a reposition to a location which is inside the geofence