fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Apply Google Style to Commander Private methods, rename geofence message geofence_violation to primary_geofence_breached.

Browse Source
This commit is contained in:
mcsauder 2022-09-05 18:17:24 -06:00 committed by Daniel Agar
parent 693af897b3
commit ebc88afe46
8 changed files with 145 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
msg/geofence_result.msg
View File

@ -1,12 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
uint8 GF_ACTION_WARN = 1 # critical mavlink message
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
uint64 timestamp # time since system start (microseconds)
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
uint8 GF_ACTION_WARN = 1 # critical mavlink message
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
bool geofence_violated # true if the geofence is violated
uint8 geofence_action # action to take when geofence is violated
bool home_required # true if the geofence requires a valid home position
bool primary_geofence_breached # true if the primary geofence is breached
uint8 primary_geofence_action # action to take when the primary geofence is breached
bool home_required # true if the geofence requires a valid home position

2
msg/vehicle_status_flags.msg
View File

@ -41,7 +41,7 @@ uint8 battery_warning # Battery warning level
bool battery_low_remaining_time # Low battery based on remaining flight time
bool battery_unhealthy # Battery unhealthy
bool geofence_violated # Geofence violated
bool primary_geofence_breached # Primary Geofence breached
bool mission_failure # Mission failure
bool vtol_transition_failure # VTOL transition failed (quadchute)

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

@ -73,15 +73,15 @@
#include <uORB/topics/tune_control.h>
typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
} VEHICLE_MODE_FLAG;
// TODO: generate
@ -487,7 +487,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
return Commander::main(argc, argv);
}
bool Commander::shutdown_if_allowed()
bool Commander::shutdownIfAllowed()
{
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
@ -815,7 +815,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
if (cmd.from_external && cmd.source_component == 190) { // MAV_COMP_ID_MISSIONPLANNER
print_reject_mode(desired_nav_state);
printRejectMode(desired_nav_state);
}
main_ret = TRANSITION_DENIED;
@ -1130,7 +1130,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
cmd_result = handle_command_actuator_test(cmd);
cmd_result = handleCommandActuatorTest(cmd);
break;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
@ -1143,7 +1143,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
// 1: Reboot autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1153,7 +1153,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(BOARD_HAS_POWER_CONTROL)
} else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
// 2: Shutdown autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1163,7 +1163,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) {
} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1435,8 +1435,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
return true;
}
unsigned
Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
{
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
@ -1562,7 +1561,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
case action_request_s::ACTION_SWITCH_MODE:
if (!_user_mode_intention.change(action_request.mode, true)) {
print_reject_mode(action_request.mode);
printRejectMode(action_request.mode);
}
break;
@ -1624,8 +1623,7 @@ void Commander::updateParameters()
}
void
Commander::run()
void Commander::run()
{
/* initialize */
led_init();
@ -1708,10 +1706,10 @@ Commander::run()
checkForMissionUpdate();
manual_control_check();
manualControlCheck();
// data link checks which update the status
data_link_check();
dataLinkCheck();
/* handle commands last, as the system needs to be updated to handle them */
if (_vehicle_command_sub.updated()) {
@ -1788,7 +1786,7 @@ Commander::run()
_vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
perf_end(_preflight_check_perf);
check_and_inform_ready_for_takeoff();
checkAndInformReadyForTakeoff();
}
// publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed
@ -1802,7 +1800,7 @@ Commander::run()
_actuator_armed_pub.publish(_actuator_armed);
// update and publish vehicle_control_mode
update_control_mode();
updateControlMode();
// vehicle_status publish (after prearm/preflight updates above)
_vehicle_status.arming_state = _arm_state_machine.getArmState();
@ -1941,7 +1939,7 @@ void Commander::handlePowerButtonState()
if (_power_button_state_sub.copy(&button_state)) {
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) {
if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) {
while (1) { px4_usleep(1); }
}
}
@ -2239,7 +2237,7 @@ bool Commander::handleModeIntentionAndFailsafe()
return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
}
void Commander::check_and_inform_ready_for_takeoff()
void Commander::checkAndInformReadyForTakeoff()
{
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
static bool ready_for_takeoff_printed = false;
@ -2406,8 +2404,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
}
}
void
Commander::update_control_mode()
void Commander::updateControlMode()
{
_vehicle_control_mode = {};
_offboard_control_mode_sub.update();
@ -2417,8 +2414,7 @@ Commander::update_control_mode()
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
}
void
Commander::print_reject_mode(uint8_t nav_state)
void Commander::printRejectMode(uint8_t nav_state)
{
if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
@ -2517,7 +2513,7 @@ void Commander::enable_hil()
_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
}
void Commander::data_link_check()
void Commander::dataLinkCheck()
{
for (auto &telemetry_status : _telemetry_status_subs) {
telemetry_status_s telemetry;
@ -2706,7 +2702,7 @@ void Commander::battery_status_check()
if (_vehicle_status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL)
if (shutdown_if_allowed() && (px4_shutdown_request(60_s) == 0)) {
if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) {
mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t");
events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
"Dangerously low battery! Shutting system down");
@ -2729,7 +2725,7 @@ void Commander::battery_status_check()
_battery_warning = _vehicle_status_flags.battery_warning;
}
void Commander::manual_control_check()
void Commander::manualControlCheck()
{
manual_control_setpoint_s manual_control_setpoint;
const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint);

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

@ -119,6 +119,7 @@ private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
void battery_status_check();
@ -128,9 +129,9 @@ private:
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void data_link_check();
void dataLinkCheck();
void manual_control_check();
void manualControlCheck();
/**
* @brief Handle incoming vehicle command relavant to Commander
@ -142,71 +143,43 @@ private:
*/
bool handle_command(const vehicle_command_s &cmd);
unsigned handle_command_actuator_test(const vehicle_command_s &cmd);
unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
void executeActionRequest(const action_request_s &action_request);
void print_reject_mode(uint8_t nav_state);
void printRejectMode(uint8_t nav_state);
void update_control_mode();
void updateControlMode();
bool shutdown_if_allowed();
bool shutdownIfAllowed();
void send_parachute_command();
void checkForMissionUpdate();
void handlePowerButtonState();
void systemPowerUpdate();
void landDetectorUpdate();
void safetyButtonUpdate();
void vtolStatusUpdate();
void updateTunes();
void checkWorkerThread();
bool getPrearmState() const;
void handleAutoDisarm();
bool handleModeIntentionAndFailsafe();
void updateParameters();
void check_and_inform_ready_for_takeoff();
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
(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,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action
)
// optional parameters
param_t _param_mav_comp_id{PARAM_INVALID};
param_t _param_mav_sys_id{PARAM_INVALID};
param_t _param_mav_type{PARAM_INVALID};
param_t _param_rc_map_fltmode{PARAM_INVALID};
void checkAndInformReadyForTakeoff();
enum class PrearmedMode {
DISABLED = 0,
@ -223,77 +196,87 @@ private:
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
ArmStateMachine _arm_state_machine{};
vehicle_status_s _vehicle_status{};
FailureDetector _failure_detector{this};
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
ArmStateMachine _arm_state_machine{};
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance};
FailureDetector _failure_detector{this};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
Safety _safety{};
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
WorkerThread _worker_thread{};
const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_vehicle_status_flags};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
bool _onboard_controller_lost{false};
bool _avoidance_system_lost{false};
bool _parachute_system_lost{true};
bool _open_drone_id_system_lost{true};
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
bool _mode_switch_mapped{false};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
bool _last_overload{false};
bool _is_throttle_above_center{false};
bool _is_throttle_low{false};
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
hrt_abstime _overload_start{0}; ///< time when CPU overload started
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
hrt_abstime _overload_start{0}; ///< time when CPU overload started
hrt_abstime _led_armed_state_toggle{0};
hrt_abstime _led_overload_toggle{0};
hrt_abstime _last_termination_message_sent{0};
hrt_abstime _last_health_and_arming_check{0};
bool _status_changed{true};
bool _arm_tune_played{false};
bool _was_armed{false};
bool _have_taken_off_since_arming{false};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
bool _open_drone_id_system_lost{true};
bool _avoidance_system_lost{false};
bool _onboard_controller_lost{false};
bool _parachute_system_lost{true};
bool _last_overload{false};
bool _mode_switch_mapped{false};
bool _is_throttle_above_center{false};
bool _is_throttle_low{false};
bool _arm_tune_played{false};
bool _was_armed{false};
bool _have_taken_off_since_arming{false};
bool _status_changed{true};
vehicle_land_detected_s _vehicle_land_detected{};
vtol_vehicle_status_s _vtol_vehicle_status{};
// commander publications
actuator_armed_s _actuator_armed{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
Safety _safety;
WorkerThread _worker_thread;
vtol_vehicle_status_s _vtol_vehicle_status{};
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
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 _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
#if defined(BOARD_HAS_POWER_CONTROL)
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
@ -304,25 +287,41 @@ private:
// Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
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<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _mavlink_log_pub{nullptr};
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{this, _vehicle_status};
hrt_abstime _last_health_and_arming_check{0};
const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()};
// optional parameters
param_t _param_mav_comp_id{PARAM_INVALID};
param_t _param_mav_sys_id{PARAM_INVALID};
param_t _param_mav_type{PARAM_INVALID};
param_t _param_rc_map_fltmode{PARAM_INVALID};
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};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
(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_FLT_PROFILE>) _param_com_flt_profile,
(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action
)
};

6
src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp
View File

@ -41,9 +41,9 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
geofence_result = {};
}
reporter.failsafeFlags().geofence_violated = geofence_result.geofence_violated;
reporter.failsafeFlags().primary_geofence_breached = geofence_result.primary_geofence_breached;
if (geofence_result.geofence_action != 0 && reporter.failsafeFlags().geofence_violated) {
if (geofence_result.primary_geofence_action != 0 && reporter.failsafeFlags().primary_geofence_breached) {
/* EVENT
* @description
* <profile name="dev">
@ -58,7 +58,7 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
}
}
if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL
if (geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL
&& reporter.failsafeFlags().home_position_invalid) {
/* EVENT
* @description

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

@ -395,7 +395,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
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()));
CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
// Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));

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

@ -335,7 +335,7 @@ private:
geofence_result_s geofence;
if (_geofence_sub.update(&geofence)) {
if (geofence.geofence_violated) {
if (geofence.primary_geofence_breached) {
msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
}

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

@ -863,12 +863,12 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.geofence_action = _geofence.getGeofenceAction();
_geofence_result.primary_geofence_action = _geofence.getGeofenceAction();
_geofence_result.home_required = _geofence.isHomeRequired();
if (gf_violation_type.value) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
_geofence_result.primary_geofence_breached = true;
/* 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) {
@ -918,7 +918,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
} else {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = false;
_geofence_result.primary_geofence_breached = false;
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;