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

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
+3 -2
View File
@@ -6,7 +6,8 @@ uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND 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 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 bool home_required # true if the geofence requires a valid home position
+1 -1
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_low_remaining_time # Low battery based on remaining flight time
bool battery_unhealthy # Battery unhealthy bool battery_unhealthy # Battery unhealthy
bool geofence_violated # Geofence violated bool primary_geofence_breached # Primary Geofence breached
bool mission_failure # Mission failure bool mission_failure # Mission failure
bool vtol_transition_failure # VTOL transition failed (quadchute) bool vtol_transition_failure # VTOL transition failed (quadchute)
+20 -24
View File
@@ -487,7 +487,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
return Commander::main(argc, 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, return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks, 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 { } else {
if (cmd.from_external && cmd.source_component == 190) { // MAV_COMP_ID_MISSIONPLANNER 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; main_ret = TRANSITION_DENIED;
@@ -1130,7 +1130,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
break; break;
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST: case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
cmd_result = handle_command_actuator_test(cmd); cmd_result = handleCommandActuatorTest(cmd);
break; break;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { 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) #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 // 1: Reboot autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); 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) #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 // 2: Shutdown autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); 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) #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. // 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); 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; return true;
} }
unsigned unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
{ {
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; 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: case action_request_s::ACTION_SWITCH_MODE:
if (!_user_mode_intention.change(action_request.mode, true)) { if (!_user_mode_intention.change(action_request.mode, true)) {
print_reject_mode(action_request.mode); printRejectMode(action_request.mode);
} }
break; break;
@@ -1624,8 +1623,7 @@ void Commander::updateParameters()
} }
void void Commander::run()
Commander::run()
{ {
/* initialize */ /* initialize */
led_init(); led_init();
@@ -1708,10 +1706,10 @@ Commander::run()
checkForMissionUpdate(); checkForMissionUpdate();
manual_control_check(); manualControlCheck();
// data link checks which update the status // data link checks which update the status
data_link_check(); dataLinkCheck();
/* handle commands last, as the system needs to be updated to handle them */ /* handle commands last, as the system needs to be updated to handle them */
if (_vehicle_command_sub.updated()) { 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); _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
perf_end(_preflight_check_perf); 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 // 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); _actuator_armed_pub.publish(_actuator_armed);
// update and publish vehicle_control_mode // update and publish vehicle_control_mode
update_control_mode(); updateControlMode();
// vehicle_status publish (after prearm/preflight updates above) // vehicle_status publish (after prearm/preflight updates above)
_vehicle_status.arming_state = _arm_state_machine.getArmState(); _vehicle_status.arming_state = _arm_state_machine.getArmState();
@@ -1941,7 +1939,7 @@ void Commander::handlePowerButtonState()
if (_power_button_state_sub.copy(&button_state)) { if (_power_button_state_sub.copy(&button_state)) {
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { 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); } 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(); 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 #ifdef CONFIG_ARCH_BOARD_PX4_SITL
static bool ready_for_takeoff_printed = false; static bool ready_for_takeoff_printed = false;
@@ -2406,8 +2404,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
} }
} }
void void Commander::updateControlMode()
Commander::update_control_mode()
{ {
_vehicle_control_mode = {}; _vehicle_control_mode = {};
_offboard_control_mode_sub.update(); _offboard_control_mode_sub.update();
@@ -2417,8 +2414,7 @@ Commander::update_control_mode()
_vehicle_control_mode_pub.publish(_vehicle_control_mode); _vehicle_control_mode_pub.publish(_vehicle_control_mode);
} }
void void Commander::printRejectMode(uint8_t nav_state)
Commander::print_reject_mode(uint8_t nav_state)
{ {
if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) { 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; _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) { for (auto &telemetry_status : _telemetry_status_subs) {
telemetry_status_s telemetry; 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 (_vehicle_status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL) #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"); 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}, events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
"Dangerously low battery! Shutting system down"); "Dangerously low battery! Shutting system down");
@@ -2729,7 +2725,7 @@ void Commander::battery_status_check()
_battery_warning = _vehicle_status_flags.battery_warning; _battery_warning = _vehicle_status_flags.battery_warning;
} }
void Commander::manual_control_check() void Commander::manualControlCheck()
{ {
manual_control_setpoint_s manual_control_setpoint; manual_control_setpoint_s manual_control_setpoint;
const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint); const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint);
+86 -87
View File
@@ -119,6 +119,7 @@ private:
void answer_command(const vehicle_command_s &cmd, uint8_t result); 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 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); transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
void battery_status_check(); 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. * 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 * @brief Handle incoming vehicle command relavant to Commander
@@ -142,71 +143,43 @@ private:
*/ */
bool handle_command(const vehicle_command_s &cmd); 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 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 send_parachute_command();
void checkForMissionUpdate(); void checkForMissionUpdate();
void handlePowerButtonState(); void handlePowerButtonState();
void systemPowerUpdate(); void systemPowerUpdate();
void landDetectorUpdate(); void landDetectorUpdate();
void safetyButtonUpdate(); void safetyButtonUpdate();
void vtolStatusUpdate(); void vtolStatusUpdate();
void updateTunes(); void updateTunes();
void checkWorkerThread(); void checkWorkerThread();
bool getPrearmState() const; bool getPrearmState() const;
void handleAutoDisarm(); void handleAutoDisarm();
bool handleModeIntentionAndFailsafe(); bool handleModeIntentionAndFailsafe();
void updateParameters(); void updateParameters();
void check_and_inform_ready_for_takeoff(); void checkAndInformReadyForTakeoff();
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};
enum class PrearmedMode { enum class PrearmedMode {
DISABLED = 0, DISABLED = 0,
@@ -223,37 +196,34 @@ private:
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
vehicle_status_s _vehicle_status{};
ArmStateMachine _arm_state_machine{}; ArmStateMachine _arm_state_machine{};
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance};
FailureDetector _failure_detector{this}; FailureDetector _failure_detector{this};
bool _flight_termination_triggered{false}; HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
bool _lockdown_triggered{false}; 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};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false}; Hysteresis _auto_disarm_killed{false};
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};
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};
bool _last_overload{false}; hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
bool _is_throttle_above_center{false};
bool _is_throttle_low{false};
hrt_abstime _boot_timestamp{0}; hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0};
@@ -262,33 +232,46 @@ private:
hrt_abstime _led_armed_state_toggle{0}; hrt_abstime _led_armed_state_toggle{0};
hrt_abstime _led_overload_toggle{0}; hrt_abstime _led_overload_toggle{0};
hrt_abstime _last_termination_message_sent{0}; hrt_abstime _last_health_and_arming_check{0};
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 _status_changed{true};
bool _arm_tune_played{false}; bool _arm_tune_played{false};
bool _was_armed{false}; bool _was_armed{false};
bool _have_taken_off_since_arming{false}; bool _have_taken_off_since_arming{false};
bool _status_changed{true};
vehicle_land_detected_s _vehicle_land_detected{}; vehicle_land_detected_s _vehicle_land_detected{};
vtol_vehicle_status_s _vtol_vehicle_status{};
// commander publications // commander publications
actuator_armed_s _actuator_armed{}; actuator_armed_s _actuator_armed{};
vehicle_control_mode_s _vehicle_control_mode{}; vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{}; vtol_vehicle_status_s _vtol_vehicle_status{};
Safety _safety;
WorkerThread _worker_thread;
// Subscriptions // 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 _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; 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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; 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::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -304,25 +287,41 @@ private:
// Publications // Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)}; 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<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_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_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}; orb_advert_t _mavlink_log_pub{nullptr};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; 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")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; // optional parameters
hrt_abstime _last_health_and_arming_check{0}; param_t _param_mav_comp_id{PARAM_INVALID};
const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; 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}; DEFINE_PARAMETERS(
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance}; (ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
bool _failsafe_user_override_request{false}; ///< override request due to stick movements (ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; (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
)
}; };
@@ -41,9 +41,9 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
geofence_result = {}; 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 /* EVENT
* @description * @description
* <profile name="dev"> * <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) { && reporter.failsafeFlags().home_position_invalid) {
/* EVENT /* EVENT
* @description * @description
+1 -1
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, CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded,
ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never)); 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 // Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow)); CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
@@ -335,7 +335,7 @@ private:
geofence_result_s geofence; geofence_result_s geofence;
if (_geofence_sub.update(&geofence)) { if (_geofence_sub.update(&geofence)) {
if (geofence.geofence_violated) { if (geofence.primary_geofence_breached) {
msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
} }
+3 -3
View File
@@ -863,12 +863,12 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
have_geofence_position_data = false; have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time(); _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(); _geofence_result.home_required = _geofence.isHomeRequired();
if (gf_violation_type.value) { if (gf_violation_type.value) {
/* inform other apps via the mission result */ /* 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 */ /* 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) { 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 { } else {
/* inform other apps via the mission result */ /* 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 */ /* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false; _geofence_violation_warning_sent = false;