From 5ac43e7236cf057ea4acafc93c0f2b6f4aa49777 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 7 May 2021 15:42:09 +0200 Subject: [PATCH] commander: switch to events --- src/lib/events/enums.json | 192 ++++++++ src/modules/commander/Commander.cpp | 420 +++++++++++++----- .../commander/calibration_routines.cpp | 5 +- .../state_machine_helper_test.cpp | 2 +- src/modules/commander/rc_calibration.cpp | 12 +- .../commander/state_machine_helper.cpp | 135 ++++-- src/modules/commander/state_machine_helper.h | 20 +- src/modules/commander/worker_thread.cpp | 7 +- 8 files changed, 625 insertions(+), 168 deletions(-) diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 937f59b665..f472545478 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -48,6 +48,198 @@ "description": "High Error Density" } } + }, + "arming_state_t": { + "type": "uint8_t", + "description": "State of the main arming state machine", + "entries": { + "0": { + "name": "init", + "description": "Init" + }, + "1": { + "name": "standby", + "description": "Standby" + }, + "2": { + "name": "armed", + "description": "Armed" + }, + "3": { + "name": "standby_error", + "description": "Standby Error" + }, + "4": { + "name": "shutdown", + "description": "Shutdown" + }, + "5": { + "name": "inair_restore", + "description": "In-air Restore" + } + } + }, + "failsafe_reason_t": { + "type": "uint8_t", + "description": "Reason for entering failsafe", + "entries": { + "0": { + "name": "no_rc", + "description": "No manual control stick input" + }, + "1": { + "name": "no_offboard", + "description": "No offboard control inputs" + }, + "2": { + "name": "no_rc_and_no_offboard", + "description": "No manual control stick and no offboard control inputs" + }, + "3": { + "name": "no_local_position", + "description": "No local position estimate" + }, + "4": { + "name": "no_global_position", + "description": "No global position estimate" + }, + "5": { + "name": "no_datalink", + "description": "No datalink" + }, + "6": { + "name": "no_rc_and_no_datalink", + "description": "No RC and no datalink" + } + } + }, + "arm_disarm_reason_t": { + "type": "uint8_t", + "description": "Reason for arming/disarming", + "entries": { + "0": { + "name": "transition_to_standby", + "description": "Transition to standby" + }, + "1": { + "name": "rc_stick", + "description": "RC" + }, + "2": { + "name": "rc_switch", + "description": "RC (switch)" + }, + "3": { + "name": "command_internal", + "description": "internal command" + }, + "4": { + "name": "command_external", + "description": "external command" + }, + "5": { + "name": "mission_start", + "description": "mission start" + }, + "6": { + "name": "safety_button", + "description": "safety button" + }, + "7": { + "name": "auto_disarm_land", + "description": "landing" + }, + "8": { + "name": "auto_disarm_preflight", + "description": "auto preflight disarming" + }, + "9": { + "name": "kill_switch", + "description": "kill switch" + }, + "10": { + "name": "lockdown", + "description": "lockdown" + }, + "11": { + "name": "failure_detector", + "description": "failure detector" + }, + "12": { + "name": "shutdown", + "description": "shutdown request" + }, + "13": { + "name": "unit_test", + "description": "unit tests" + } + } + }, + "navigation_mode_t": { + "type": "uint8_t", + "description": "Flight mode", + "entries": { + "0": { + "name": "manual", + "description": "Manual" + }, + "1": { + "name": "altctl", + "description": "Altitude control" + }, + "2": { + "name": "posctl", + "description": "Position control" + }, + "3": { + "name": "auto_mission", + "description": "Mission" + }, + "4": { + "name": "auto_loiter", + "description": "Hold" + }, + "5": { + "name": "auto_rtl", + "description": "RTL" + }, + "6": { + "name": "acro", + "description": "Acro" + }, + "7": { + "name": "offboard", + "description": "Offboard" + }, + "8": { + "name": "stab", + "description": "Stabilized" + }, + "10": { + "name": "auto_takeoff", + "description": "Takeoff" + }, + "11": { + "name": "auto_land", + "description": "Land" + }, + "12": { + "name": "auto_follow_target", + "description": "Follow Target" + }, + "13": { + "name": "auto_precland", + "description": "Precision Landing" + }, + "14": { + "name": "orbit", + "description": "Orbit" + }, + "255": { + "name": "unknown", + "description": "[Unknown]" + } + } } } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 233df5c17b..671a528508 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -426,44 +427,83 @@ bool Commander::shutdown_if_allowed() { return TRANSITION_DENIED != arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, _armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements, - hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); + hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown); } static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { switch (calling_reason) { - case arm_disarm_reason_t::TRANSITION_TO_STANDBY: return ""; + case arm_disarm_reason_t::transition_to_standby: return ""; - case arm_disarm_reason_t::RC_STICK: return "RC"; + case arm_disarm_reason_t::rc_stick: return "RC"; - case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)"; + case arm_disarm_reason_t::rc_switch: return "RC (switch)"; - case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command"; + case arm_disarm_reason_t::command_internal: return "internal command"; - case arm_disarm_reason_t::COMMAND_EXTERNAL: return "external command"; + case arm_disarm_reason_t::command_external: return "external command"; - case arm_disarm_reason_t::MISSION_START: return "mission start"; + case arm_disarm_reason_t::mission_start: return "mission start"; - case arm_disarm_reason_t::SAFETY_BUTTON: return "safety button"; + case arm_disarm_reason_t::safety_button: return "safety button"; - case arm_disarm_reason_t::AUTO_DISARM_LAND: return "landing"; + case arm_disarm_reason_t::auto_disarm_land: return "landing"; - case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: return "auto preflight disarming"; + case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming"; - case arm_disarm_reason_t::KILL_SWITCH: return "kill-switch"; + case arm_disarm_reason_t::kill_switch: return "kill-switch"; - case arm_disarm_reason_t::LOCKDOWN: return "lockdown"; + case arm_disarm_reason_t::lockdown: return "lockdown"; - case arm_disarm_reason_t::FAILURE_DETECTOR: return "failure detector"; + case arm_disarm_reason_t::failure_detector: return "failure detector"; - case arm_disarm_reason_t::SHUTDOWN: return "shutdown request"; + case arm_disarm_reason_t::shutdown: return "shutdown request"; - case arm_disarm_reason_t::UNIT_TEST: return "unit tests"; + case arm_disarm_reason_t::unit_test: return "unit tests"; } return ""; }; +using navigation_mode_t = events::px4::enums::navigation_mode_t; + +static inline navigation_mode_t navigation_mode(uint8_t main_state) +{ + switch (main_state) { + case commander_state_s::MAIN_STATE_MANUAL: return navigation_mode_t::manual; + + case commander_state_s::MAIN_STATE_ALTCTL: return navigation_mode_t::altctl; + + case commander_state_s::MAIN_STATE_POSCTL: return navigation_mode_t::posctl; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter; + + case commander_state_s::MAIN_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl; + + case commander_state_s::MAIN_STATE_ACRO: return navigation_mode_t::acro; + + case commander_state_s::MAIN_STATE_OFFBOARD: return navigation_mode_t::offboard; + + case commander_state_s::MAIN_STATE_STAB: return navigation_mode_t::stab; + + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff; + + case commander_state_s::MAIN_STATE_AUTO_LAND: return navigation_mode_t::auto_land; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland; + + case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit; + } + + static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::orbit, "enum definition mismatch"); + + return navigation_mode_t::unknown; +} + static constexpr const char *main_state_str(uint8_t main_state) { switch (main_state) { @@ -509,14 +549,20 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks) { if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t"); + events::send(events::ID("commander_arm_denied_throttle_center"), + {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: throttle above center"); tune_negative(true); return TRANSITION_DENIED; } if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); + events::send(events::ID("commander_arm_denied_throttle_high"), + {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: high throttle"); tune_negative(true); return TRANSITION_DENIED; } @@ -524,7 +570,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) && !_status_flags.condition_home_position_valid) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home\t"); + events::send(events::ID("commander_arm_denied_geofence_rtl"), + {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: Geofence RTL requires valid home"); tune_negative(true); return TRANSITION_DENIED; } @@ -541,7 +590,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ hrt_elapsed_time(&_boot_timestamp), calling_reason); if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Armed by %s", arm_disarm_reason_str(calling_reason)); + mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_armed_by"), events::Log::Info, + "Armed by {1}", calling_reason); _status_changed = true; @@ -565,7 +616,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason) hrt_elapsed_time(&_boot_timestamp), calling_reason); if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s", arm_disarm_reason_str(calling_reason)); + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_disarmed_by"), events::Log::Info, + "Disarmed by {1}", calling_reason); _status_changed = true; @@ -707,7 +760,13 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - mavlink_log_critical(&_mavlink_log_pub, "Reposition command rejected"); + mavlink_log_critical(&_mavlink_log_pub, "Reposition command rejected\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_reposition_rejected"), + {events::Log::Error, events::LogInternal::Info}, + "Reposition command rejected"); } } else { @@ -781,7 +840,9 @@ Commander::handle_command(const vehicle_command_s &cmd) default: main_ret = TRANSITION_DENIED; - mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode"); + mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t"); + events::send(events::ID("commander_unsupported_auto_mode"), events::Log::Error, + "Unsupported auto mode"); break; } @@ -842,7 +903,9 @@ Commander::handle_command(const vehicle_command_s &cmd) const int param1_arm = static_cast(roundf(cmd.param1)); if (param1_arm != 0 && param1_arm != 1) { - mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1); + mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1); + events::send(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error, + "Unsupported ARM_DISARM param: {1:.3}", cmd.param1); } else { const bool cmd_arms = (param1_arm == 1); @@ -854,14 +917,23 @@ Commander::handle_command(const vehicle_command_s &cmd) if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) { if (cmd_arms) { if (_armed.armed) { - mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed"); + mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed\t"); + events::send(events::ID("commander_arming_denied_already_armed"), + {events::Log::Warning, events::LogInternal::Info }, + "Arming denied, already armed"); } else { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Not landed"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Not landed\t"); + events::send(events::ID("commander_arming_denied_not_landed"), + {events::Log::Error, events::LogInternal::Info }, + "Arming denied, not landed"); } } else { - mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed"); + mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t"); + events::send(events::ID("commander_disarming_denied_not_landed"), + {events::Log::Critical, events::LogInternal::Info }, + "Disarming denied, not landed"); } cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; @@ -881,18 +953,18 @@ Commander::handle_command(const vehicle_command_s &cmd) if (cmd_arms) { if (cmd.from_external) { - arming_res = arm(arm_disarm_reason_t::COMMAND_EXTERNAL); + arming_res = arm(arm_disarm_reason_t::command_external); } else { - arming_res = arm(arm_disarm_reason_t::COMMAND_INTERNAL, !forced); + arming_res = arm(arm_disarm_reason_t::command_internal, !forced); } } else { if (cmd.from_external) { - arming_res = disarm(arm_disarm_reason_t::COMMAND_EXTERNAL); + arming_res = disarm(arm_disarm_reason_t::command_external); } else { - arming_res = disarm(arm_disarm_reason_t::COMMAND_INTERNAL); + arming_res = disarm(arm_disarm_reason_t::command_internal); } } @@ -1003,11 +1075,17 @@ Commander::handle_command(const vehicle_command_s &cmd) /* switch to RTL which ends the mission */ if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state)) { - mavlink_log_info(&_mavlink_log_pub, "Returning to launch"); + mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t"); + events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied"); + mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_rtl_denied"), {events::Log::Critical, events::LogInternal::Info}, + "Return to launch denied"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -1023,7 +1101,12 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry"); + mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_takeoff_denied"), {events::Log::Critical, events::LogInternal::Info}, + "Takeoff denied! Please disarm and retry"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -1032,11 +1115,18 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state)) { - mavlink_log_info(&_mavlink_log_pub, "Landing at current position"); + mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t"); + events::send(events::ID("commander_landing_current_pos"), events::Log::Info, + "Landing at current position"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually"); + mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_landing_current_pos_denied"), {events::Log::Critical, events::LogInternal::Info}, + "Landing denied! Please land manually"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -1045,11 +1135,18 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, _internal_state)) { - mavlink_log_info(&_mavlink_log_pub, "Precision landing"); + mavlink_log_info(&_mavlink_log_pub, "Precision landing\t"); + events::send(events::ID("commander_landing_prec_land"), events::Log::Info, + "Landing using precision landing"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually"); + mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_landing_prec_land_denied"), {events::Log::Critical, events::LogInternal::Info}, + "Precision landing denied! Please land manually"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -1068,18 +1165,25 @@ Commander::handle_command(const vehicle_command_s &cmd) // switch to AUTO_MISSION and ARM if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state)) - && (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) { + && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - mavlink_log_critical(&_mavlink_log_pub, "Mission start denied"); + mavlink_log_critical(&_mavlink_log_pub, "Mission start denied\t"); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_mission_start_denied"), {events::Log::Critical, events::LogInternal::Info}, + "Mission start denied"); } } } else { - mavlink_log_critical(&_mavlink_log_pub, "Mission start denied! No valid mission"); + mavlink_log_critical(&_mavlink_log_pub, "Mission start denied! No valid mission\t"); + events::send(events::ID("commander_mission_start_denied_no_mission"), {events::Log::Critical, events::LogInternal::Info}, + "Mission start denied! No valid mission"); } } break; @@ -1088,7 +1192,9 @@ Commander::handle_command(const vehicle_command_s &cmd) // if no high latency telemetry exists send a failed acknowledge if (_high_latency_datalink_heartbeat > _boot_timestamp) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; - mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable"); + mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t"); + events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info}, + "Control high latency failed! Telemetry unavailable"); } } break; @@ -1170,7 +1276,7 @@ Commander::handle_command(const vehicle_command_s &cmd) false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT - (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) + (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) ) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); @@ -1205,7 +1311,9 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); /* disable RC control input completely */ _status_flags.rc_input_blocked = true; - mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input"); + mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t"); + events::send(events::ID("commander_calib_rc_off"), events::Log::Info, + "Calibration: Disabling RC input"); } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ @@ -1255,7 +1363,9 @@ Commander::handle_command(const vehicle_command_s &cmd) if (_status_flags.rc_input_blocked) { /* enable RC control input */ _status_flags.rc_input_blocked = false; - mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input"); + mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t"); + events::send(events::ID("commander_calib_rc_on"), events::Log::Info, + "Calibration: Restoring RC input"); } answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1680,20 +1790,6 @@ Commander::run() // update parameters from storage updateParams(); - // NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 - if (_param_nav_dll_act.get() == 4) { - mavlink_log_critical(&_mavlink_log_pub, "CASA Outback Challenge rules (NAV_DLL_ACT = 4) retired"); - _param_nav_dll_act.set(2); // value 2 Return mode - _param_nav_dll_act.commit_no_notification(); - } - - // NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 - if (_param_nav_rcl_act.get() == 4) { - mavlink_log_critical(&_mavlink_log_pub, "CASA Outback Challenge rules (NAV_RCL_ACT = 4) retired"); - _param_nav_rcl_act.set(2); // value 2 Return mode - _param_nav_rcl_act.commit_no_notification(); - } - /* update parameters */ if (!_armed.armed) { _status.system_type = _param_mav_type.get(); @@ -1754,7 +1850,12 @@ Commander::run() if (airmode == 2 && rc_map_arm_switch == 0) { airmode = 1; // change to roll/pitch airmode param_set(param_airmode, &airmode); - mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch") + mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch\t") + /* EVENT + * @description MC_AIRMODE is now set to roll/pitch airmode. + */ + events::send(events::ID("commander_airmode_requires_arm_sw"), {events::Log::Error, events::LogInternal::Disabled}, + "Yaw Airmode requires the use of an Arm Switch"); } } @@ -1814,7 +1915,9 @@ Commander::run() * without a need. The clean approach to unload it is to reboot. */ if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { - mavlink_log_critical(&_mavlink_log_pub, "USB disconnected, rebooting for flight safety"); + mavlink_log_critical(&_mavlink_log_pub, "USB disconnected, rebooting for flight safety\t"); + events::send(events::ID("commander_reboot_usb_disconnect"), {events::Log::Critical, events::LogInternal::Info}, + "USB disconnected, rebooting for flight safety"); while (1) { px4_usleep(1); } } @@ -1835,11 +1938,13 @@ Commander::run() // Only take actions if armed if (_armed.armed) { if (!was_landed && _land_detector.landed) { - mavlink_log_info(&_mavlink_log_pub, "Landing detected"); + mavlink_log_info(&_mavlink_log_pub, "Landing detected\t"); + events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected"); _status.takeoff_time = 0; } else if (was_landed && !_land_detector.landed) { - mavlink_log_info(&_mavlink_log_pub, "Takeoff detected"); + mavlink_log_info(&_mavlink_log_pub, "Takeoff detected\t"); + events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected"); _status.takeoff_time = hrt_absolute_time(); _have_taken_off_since_arming = true; @@ -1892,7 +1997,7 @@ Commander::run() } if (safety_disarm_allowed) { - disarm(arm_disarm_reason_t::SAFETY_BUTTON); + disarm(arm_disarm_reason_t::safety_button); } } @@ -1969,12 +2074,16 @@ Commander::run() * Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that */ - mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout"); + mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t"); + events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical, + "ESCs telemetry timeout"); _status_flags.condition_escs_error = true; } else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) { /* Detect if esc telemetry is not connected after reboot */ - mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected "); + mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t"); + events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical, + "ESCs telemetry not connected"); _status_flags.condition_escs_error = true; } } @@ -1999,10 +2108,10 @@ Commander::run() if (_auto_disarm_landed.get_state()) { if (_have_taken_off_since_arming) { - disarm(arm_disarm_reason_t::AUTO_DISARM_LAND); + disarm(arm_disarm_reason_t::auto_disarm_land); } else { - disarm(arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT); + disarm(arm_disarm_reason_t::auto_disarm_preflight); } } } @@ -2020,10 +2129,10 @@ Commander::run() if (_auto_disarm_killed.get_state()) { if (_armed.manual_lockdown) { - disarm(arm_disarm_reason_t::KILL_SWITCH); + disarm(arm_disarm_reason_t::kill_switch); } else { - disarm(arm_disarm_reason_t::LOCKDOWN); + disarm(arm_disarm_reason_t::lockdown); } } @@ -2051,7 +2160,7 @@ Commander::run() arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - arm_disarm_reason_t::TRANSITION_TO_STANDBY); + arm_disarm_reason_t::transition_to_standby); } /* start mission result check */ @@ -2073,7 +2182,10 @@ Commander::run() _status_changed = true; if (_status.mission_failure) { - mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed"); + // navigator sends out the exact reason + mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed\t"); + events::send(events::ID("commander_mission_cannot_be_completed"), {events::Log::Critical, events::LogInternal::Info}, + "Mission cannot be completed"); } } @@ -2170,7 +2282,9 @@ Commander::run() if (!_flight_termination_triggered && !_lockdown_triggered) { _flight_termination_triggered = true; - mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); + mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated\t"); + events::send(events::ID("commander_geofence_termination"), {events::Log::Alert, events::LogInternal::Warning}, + "Geofence violation! Flight terminated"); _armed.force_failsafe = true; _status_changed = true; send_parachute_command(); @@ -2225,7 +2339,10 @@ Commander::run() if (!_flight_termination_triggered && !_lockdown_triggered) { - mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); + // navigator only requests flight termination on GPS failure + mavlink_log_critical(&_mavlink_log_pub, "GPS failure: Flight terminated\t"); + events::send(events::ID("commander_mission_termination"), {events::Log::Alert, events::LogInternal::Warning}, + "GPS failure: Flight terminated"); _flight_termination_triggered = true; _armed.force_failsafe = true; _status_changed = true; @@ -2233,7 +2350,9 @@ Commander::run() } if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(&_mavlink_log_pub, "Flight termination active"); + mavlink_log_critical(&_mavlink_log_pub, "Flight termination active\t"); + events::send(events::ID("commander_mission_termination_active"), {events::Log::Alert, events::LogInternal::Warning}, + "Flight termination active"); } } @@ -2251,8 +2370,10 @@ Commander::run() } else { if (_status.rc_signal_lost) { if (_rc_signal_lost_timestamp > 0) { - mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs", - hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6); + float elapsed = hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6f; + mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed); + events::send(events::ID("commander_rc_regained"), events::Log::Info, + "Manual control regained after {1:.1} s", elapsed); } set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status); @@ -2266,15 +2387,17 @@ Commander::run() if (rc_arming_enabled) { if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { - disarm(arm_disarm_reason_t::RC_STICK); + disarm(arm_disarm_reason_t::rc_stick); } if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { if (_vehicle_control_mode.flag_control_manual_enabled) { - arm(arm_disarm_reason_t::RC_STICK); + arm(arm_disarm_reason_t::rc_stick); } else { - mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first"); + mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first\t"); + events::send(events::ID("commander_arming_denied_not_manual"), {events::Log::Error, events::LogInternal::Info}, + "Not arming! Switch to a manual mode first"); tune_negative(true); } } @@ -2287,13 +2410,17 @@ Commander::run() if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state) == TRANSITION_CHANGED) { tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks"); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t"); + events::send(events::ID("commander_rc_override_pos"), events::Log::Info, + "Pilot took over position control using sticks"); _status_changed = true; } else if (main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state) == TRANSITION_CHANGED) { tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks"); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t"); + events::send(events::ID("commander_rc_override_alt"), events::Log::Info, + "Pilot took over altitude control using sticks"); _status_changed = true; } } @@ -2322,7 +2449,9 @@ Commander::run() gear = landing_gear_s::GEAR_UP; } else { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t") + events::send(events::ID("commander_landed_landing_gear_error"), events::Log::Critical, + "Landed, unable to retract landing gear"); } } @@ -2346,22 +2475,27 @@ Commander::run() if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { /* set lockdown flag */ if (!_armed.manual_lockdown) { - const char kill_switch_string[] = "Kill-switch engaged"; + const char kill_switch_string[] = "Kill-switch engaged\t"; + events::LogLevels log_levels{events::Log::Info}; if (_land_detector.landed) { mavlink_log_info(&_mavlink_log_pub, kill_switch_string); } else { mavlink_log_critical(&_mavlink_log_pub, kill_switch_string); + log_levels.external = events::Log::Critical; } + events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill-switch engaged"); + _status_changed = true; _armed.manual_lockdown = true; } } else if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { if (_armed.manual_lockdown) { - mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged"); + mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t"); + events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged"); _status_changed = true; _armed.manual_lockdown = false; } @@ -2375,7 +2509,9 @@ Commander::run() if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) { // ignore RC lost during calibration if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) { - mavlink_log_critical(&_mavlink_log_pub, "Manual control lost"); + mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t"); + events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, + "Manual control lost"); _status.rc_signal_lost = true; _rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp(); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status); @@ -2471,8 +2607,9 @@ Commander::run() if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { - disarm(arm_disarm_reason_t::FAILURE_DETECTOR); - mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request"); + disarm(arm_disarm_reason_t::failure_detector); + mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t"); + events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request"); } } @@ -2484,14 +2621,35 @@ Commander::run() // This handles the case where something fails during the early takeoff phase _armed.lockdown = true; _lockdown_triggered = true; - mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown"); + mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown\t"); + /* EVENT + * @description + * When a critical failure is detected right after takeoff, the system turns off the motors. + * Failures include an exceeding tilt angle, altitude failure or an external failure trigger. + * + * + * This can be configured with the parameter COM_LKDOWN_TKO. + * + */ + events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning}, + "Critical failure detected: lockdown"); } else if (!_status_flags.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered && !_lockdown_triggered) { _armed.force_failsafe = true; _flight_termination_triggered = true; - mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight"); + mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight\t"); + /* EVENT + * @description + * Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger. + * + * + * Flight termination can be disabled with the parameter CBRK_FLIGHTTERM. + * + */ + events::send(events::ID("commander_fd_terminate"), {events::Log::Emergency, events::LogInternal::Warning}, + "Critical failure detected: terminate flight"); send_parachute_command(); } } @@ -2586,10 +2744,12 @@ Commander::run() _status_changed = true; if (_status.failsafe) { - mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated"); + mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated\t"); + events::send(events::ID("commander_failsafe_activated"), events::Log::Info, "Failsafe mode activated"); } else { - mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated"); + mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated\t"); + events::send(events::ID("commander_failsafe_deactivated"), events::Log::Info, "Failsafe mode deactivated"); } _failsafe_old = _status.failsafe; @@ -3366,7 +3526,12 @@ Commander::print_reject_mode(uint8_t main_state) { if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) { - mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", main_state_str(main_state)); + mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available\t", main_state_str(main_state)); + /* EVENT + * @description Check for a valid position estimate + */ + events::send(events::ID("commander_modeswitch_not_avail"), {events::Log::Critical, events::LogInternal::Info}, + "Switching to mode '{1}' is currently not possible", navigation_mode(main_state)); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -3523,7 +3688,8 @@ void Commander::data_link_check() _status_changed = true; if (_datalink_last_heartbeat_gcs != 0) { - mavlink_log_info(&_mavlink_log_pub, "Data link regained"); + mavlink_log_info(&_mavlink_log_pub, "Data link regained\t"); + events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained"); } if (!_armed.armed && !_status_flags.condition_calibration_enabled) { @@ -3542,7 +3708,8 @@ void Commander::data_link_check() _status_changed = true; if (_datalink_last_heartbeat_onboard_controller != 0) { - mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained"); + mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained\t"); + events::send(events::ID("commander_onboard_ctrl_regained"), events::Log::Info, "Onboard controller regained"); } } @@ -3570,7 +3737,9 @@ void Commander::data_link_check() _status.data_link_lost = true; _status.data_link_lost_counter++; - mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost"); + mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t"); + events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info}, + "Connection to ground station lost"); _status_changed = true; } @@ -3581,7 +3750,8 @@ void Commander::data_link_check() && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > (_param_com_obc_loss_t.get() * 1_s)) && !_onboard_controller_lost) { - mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost"); + mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost\t"); + events::send(events::ID("commander_mission_comp_lost"), events::Log::Critical, "Connection to mission computer lost"); _onboard_controller_lost = true; _status_changed = true; } @@ -3604,7 +3774,8 @@ void Commander::data_link_check() if (!_status.high_latency_data_link_lost) { _status.high_latency_data_link_lost = true; - mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost"); + mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t"); + events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost"); _status_changed = true; } } @@ -3758,12 +3929,19 @@ void Commander::battery_status_check() #if defined(CONFIG_BOARDCTL_POWEROFF) if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { - mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down"); + mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down\t"); + events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning}, + "Dangerously low battery! Shutting system down"); while (1) { px4_usleep(1); } } else { - mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown"); + mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown\t"); + /* EVENT + * @description Cannot shut down, most likely the system does not support it. + */ + events::send(events::ID("commander_low_bat_shutdown_failed"), {events::Log::Emergency, events::LogInternal::Error}, + "Dangerously low battery! System shut down failed"); } #endif // CONFIG_BOARDCTL_POWEROFF @@ -3812,12 +3990,16 @@ void Commander::estimator_check() const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT)); if (!mag_fault_prev && mag_fault) { - mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!"); + mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t"); + events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical, + "Stopping compass use! Land now and calibrate the compass"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status); } if (!gnss_heading_fault_prev && gnss_heading_fault) { - mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!"); + mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t"); + events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical, + "GNSS heading not reliable. Land now!"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status); } @@ -3862,7 +4044,9 @@ void Commander::estimator_check() if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) { // if the innovation test has failed continuously, declare the nav as failed _nav_test_failed = true; - mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors"); + mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t"); + events::send(events::ID("commander_navigation_failure"), events::Log::Emergency, + "Navigation failure! Land and recalibrate the sensors"); } } } @@ -4019,10 +4203,11 @@ void Commander::esc_status_check() if ((esc_status.esc_online_flags & (1 << index)) == 0) { snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; + events::send(events::ID("commander_esc_offline"), events::Log::Critical, "ESC{1} offline", index + 1); } } - mavlink_log_critical(&_mavlink_log_pub, "%soffline", esc_fail_msg); + mavlink_log_critical(&_mavlink_log_pub, "%soffline\t", esc_fail_msg); _last_esc_online_flags = esc_status.esc_online_flags; _status_flags.condition_escs_error = true; @@ -4038,31 +4223,46 @@ void Commander::esc_status_check() if (esc_status.esc[index].failures != _last_esc_failure[index]) { if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_CURRENT_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over current", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over current\t", index + 1); + events::send(events::ID("commander_esc_over_current"), events::Log::Critical, + "ESC{1}: over current", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_VOLTAGE_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over voltage", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over voltage\t", index + 1); + events::send(events::ID("commander_esc_over_voltage"), events::Log::Critical, + "ESC{1}: over voltage", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_TEMPERATURE_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1); + events::send(events::ID("commander_esc_over_temp"), events::Log::Critical, + "ESC{1}: over temperature", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM\t", index + 1); + events::send(events::ID("commander_esc_over_rpm"), events::Log::Critical, + "ESC{1}: over RPM", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency\t", index + 1); + events::send(events::ID("commander_esc_cmd_inconsistent"), events::Log::Critical, + "ESC{1}: command inconsistency", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck", index + 1); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck\t", index + 1); + events::send(events::ID("commander_esc_motor_stuck"), events::Log::Critical, + "ESC{1}: motor stuck", index + 1); } if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) { - mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d", index + 1, esc_status.esc[index].esc_state); + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d\t", index + 1, + esc_status.esc[index].esc_state); + events::send(events::ID("commander_esc_generic_failure"), events::Log::Critical, + "ESC{1}: generic failure (code {2})", index + 1, esc_status.esc[index].esc_state); } } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index c9ba9aa635..76d373a664 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include @@ -345,7 +346,9 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &ca } else { command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32, cmd.command); + mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32 "\t", cmd.command); + events::send(events::ID("commander_cal_cmd_denied"), {events::Log::Error, events::LogInternal::Info}, + "Command denied during calibration: {1}", cmd.command); tune_negative(true); ret = false; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 177c65463e..8d74a4643c 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -306,7 +306,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() status_flags, arm_req, 2e6, /* 2 seconds after boot, everything should be checked */ - arm_disarm_reason_t::UNIT_TEST); + arm_disarm_reason_t::unit_test); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index c7d4561258..973e651aa2 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -36,6 +36,7 @@ * Remote Control calibration routine */ +#include #include #include #include @@ -57,7 +58,9 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) bool changed = manual_control_setpoint_sub.updated(); if (!changed) { - mavlink_log_critical(mavlink_log_pub, "no inputs, aborting"); + mavlink_log_critical(mavlink_log_pub, "no inputs, aborting\t"); + events::send(events::ID("commander_cal_no_inputs"), {events::Log::Error, events::LogInternal::Info}, + "No inputs, aborting RC trim calibration"); return PX4_ERROR; } @@ -94,11 +97,14 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) int p3r = param_set(param_find("TRIM_YAW"), &p); if (p1r != 0 || p2r != 0 || p3r != 0) { - mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL"); + mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL\t"); + events::send(events::ID("commander_cal_trim_param_set_failed"), events::Log::Critical, + "RC trim calibration: failed to set parameters"); return PX4_ERROR; } - mavlink_log_info(mavlink_log_pub, "trim cal done"); + mavlink_log_info(mavlink_log_pub, "trim cal done\t"); + events::send(events::ID("commander_cal_trim_done"), events::Log::Info, "RC trim calibration completed"); return PX4_OK; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0c56e73c92..91102c3347 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -86,6 +86,28 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { "IN_AIR_RESTORE", }; +static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state) +{ + switch (arming_state) { + case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; + + case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; + + case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, + "enum def mismatch"); + + return events::px4::enums::arming_state_t::init; +} + // You can index into the array with an navigation_state_t in order to get its textual representation const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "MANUAL", @@ -273,8 +295,13 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { - mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s", + // FIXME: this catch-all does not provide helpful information to the user + mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t", arming_state_names[status.arming_state], arming_state_names[new_arming_state]); + events::send( + events::ID("commander_transition_denied"), events::Log::Critical, + "Arming state transition denied: {1} to {2}", + eventArmingState(status.arming_state), eventArmingState(new_arming_state)); } } @@ -410,10 +437,12 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai return ret; } +using event_failsafe_reason_t = events::px4::enums::failsafe_reason_t; /** * Enable failsafe and report to user */ -void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason) +static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, + event_failsafe_reason_t event_failsafe_reason) { if (!old_failsafe && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // make sure intermittent failsafes don't lead to infinite delay by not constantly reseting the timestamp @@ -422,7 +451,28 @@ void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t * status.failsafe_timestamp = hrt_absolute_time(); } - mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason); + const char *reason = ""; + + switch (event_failsafe_reason) { + case event_failsafe_reason_t::no_rc: reason = reason_no_rc; break; + + case event_failsafe_reason_t::no_offboard: reason = reason_no_offboard; break; + + case event_failsafe_reason_t::no_rc_and_no_offboard: reason = reason_no_rc_and_no_offboard; break; + + case event_failsafe_reason_t::no_local_position: reason = reason_no_local_position; break; + + case event_failsafe_reason_t::no_global_position: reason = reason_no_global_position; break; + + case event_failsafe_reason_t::no_datalink: reason = reason_no_datalink; break; + + case event_failsafe_reason_t::no_rc_and_no_datalink: reason = reason_no_rc_and_no_datalink; break; + } + + mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s\t", reason); + events::send( + events::ID("commander_enable_failsafe"), {events::Log::Critical, events::LogInternal::Info}, + "Failsafe enabled: {1}", event_failsafe_reason); } status.failsafe = true; @@ -461,7 +511,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // Require RC for all manual modes if (status.rc_signal_lost && is_armed) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else { @@ -495,7 +545,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed; if (status.rc_signal_lost && is_armed) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ @@ -534,14 +584,14 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (status.data_link_lost && data_link_loss_act_configured && is_armed && !landed) { // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) && status_flags.rc_signal_found_once && is_armed && !landed) { // RC link lost, rc loss not disabled in mission, RC was used before -> RC loss reaction after delay // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) @@ -550,7 +600,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // All links lost, no data link loss reaction configured -> immediately do RC loss reaction // Lost all communication, by default it's considered unsafe to continue the mission // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); } else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) @@ -559,7 +609,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ && mission_finished) { // All links lost, all link loss reactions disabled -> return after mission finished // Pilot disabled all reactions, finish mission but then return to avoid lost vehicle - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0); } else if (!stay_in_failsafe) { @@ -579,14 +629,14 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // nothing to do - everything done in check_invalid_pos_nav_state } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) && status_flags.rc_signal_found_once && is_armed && !landed) { // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) @@ -595,7 +645,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // All links lost, no data link loss reaction configured -> immediately do RC loss reaction // Lost all communication, by default it's considered unsafe to continue the mission // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); } else { @@ -655,7 +705,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // failsafe: just datalink is lost and we're in air set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit // is not possible and therefore the internal_state needs to be adjusted. @@ -663,7 +713,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (status.rc_signal_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) { // Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); @@ -689,14 +739,14 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // nothing to do - everything done in check_invalid_pos_nav_state } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { // Data link lost, data link loss reaction configured -> do configured reaction - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) && status_flags.rc_signal_found_once && is_armed && !landed) { // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay // Safety pilot expects to be able to take over by RC in case anything unexpected happens - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) @@ -705,7 +755,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ // All links lost, no data link loss reaction configured -> immediately do RC loss reaction // Lost all communication, by default it's considered unsafe to continue the mission // This is only reached when flying mission completely without RC (it was not present since boot) - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); } else { @@ -751,17 +801,17 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ if (status_flags.offboard_control_signal_lost) { if (status.rc_signal_lost) { // Offboard and RC are lost - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_offboard); set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); } else { // Offboard is lost, RC is ok if (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_offboard); set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); } else { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_offboard); set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); } @@ -770,7 +820,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) { // Only RC is lost - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc); set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); } else { @@ -832,10 +882,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or } if (using_global_pos) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_global_position); } else { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position); + enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_local_position); } } @@ -1082,7 +1132,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta break; case battery_status_s::BATTERY_WARNING_LOW: - mavlink_log_critical(mavlink_log_pub, "Low battery level! Return advised"); + mavlink_log_critical(mavlink_log_pub, "Low battery level! Return advised\t"); + events::send(events::ID("commander_bat_low"), {events::Log::Warning, events::LogInternal::Info}, + "Low battery level! Return advised"); break; case battery_status_s::BATTERY_WARNING_CRITICAL: @@ -1091,7 +1143,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta switch (low_battery_action) { case LOW_BAT_ACTION::WARNING: - mavlink_log_critical(mavlink_log_pub, "%s, landing advised", battery_critical); + mavlink_log_critical(mavlink_log_pub, "%s, landing advised\t", battery_critical); + events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info}, + "Critical battery level! Landing advised"); break; case LOW_BAT_ACTION::RETURN: @@ -1106,7 +1160,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL\t", battery_critical); + events::send(events::ID("commander_bat_crit_rtl"), {events::Log::Critical, events::LogInternal::Info}, + "Critical battery level! Executing RTL"); } } else { @@ -1114,7 +1170,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead\t", battery_critical); + events::send(events::ID("commander_bat_crit_no_rtl_land"), {events::Log::Emergency, events::LogInternal::Info}, + "Critical battery level! Cannot execute RTL, landing instead"); } } @@ -1125,7 +1183,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical); + mavlink_log_emergency(mavlink_log_pub, "%s, landing\t", battery_critical); + events::send(events::ID("commander_bat_crit_land"), {events::Log::Warning, events::LogInternal::Info}, + "Critical battery level! Landing now"); } break; @@ -1139,7 +1199,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta switch (low_battery_action) { case LOW_BAT_ACTION::WARNING: - mavlink_log_emergency(mavlink_log_pub, "%s, please land!", battery_dangerous); + mavlink_log_emergency(mavlink_log_pub, "%s, please land!\t", battery_dangerous); + events::send(events::ID("commander_bat_emerg_warn"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level! Please land immediately"); break; case LOW_BAT_ACTION::RETURN: @@ -1149,7 +1211,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_dangerous); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL\t", battery_dangerous); + events::send(events::ID("commander_bat_emerg_rtl"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level! Executing RTL"); } } else { @@ -1157,7 +1221,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_dangerous); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead\t", battery_dangerous); + events::send(events::ID("commander_bat_emerg_no_rtl_land"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level! Cannot execute RTL, landing instead"); } } @@ -1171,7 +1237,9 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; internal_state.timestamp = hrt_absolute_time(); - mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_dangerous); + mavlink_log_emergency(mavlink_log_pub, "%s, landing\t", battery_dangerous); + events::send(events::ID("commander_bat_emerg_land"), {events::Log::Emergency, events::LogInternal::Info}, + "Dangerous battery level! Landing now"); } break; @@ -1180,7 +1248,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta break; case battery_status_s::BATTERY_WARNING_FAILED: - mavlink_log_emergency(mavlink_log_pub, "Battery failure detected"); + mavlink_log_emergency(mavlink_log_pub, "Battery failure detected\t"); + events::send(events::ID("commander_bat_failure"), events::Log::Emergency, "Battery failure detected"); break; } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f5ce524861..f810625ce1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -53,6 +53,7 @@ #include #include #include +#include typedef enum { TRANSITION_DENIED = -1, @@ -98,22 +99,7 @@ enum class position_nav_loss_actions_t { extern const char *const arming_state_names[]; extern const char *const nav_state_names[]; -enum class arm_disarm_reason_t { - TRANSITION_TO_STANDBY = 0, - RC_STICK = 1, - RC_SWITCH = 2, - COMMAND_INTERNAL = 3, - COMMAND_EXTERNAL = 4, - MISSION_START = 5, - SAFETY_BUTTON = 6, - AUTO_DISARM_LAND = 7, - AUTO_DISARM_PREFLIGHT = 8, - KILL_SWITCH = 9, - LOCKDOWN = 10, - FAILURE_DETECTOR = 11, - SHUTDOWN = 12, - UNIT_TEST = 13 -}; +using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; enum RCLossExceptionBits { RCL_EXCEPT_MISSION = (1 << 0), @@ -131,8 +117,6 @@ 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); -void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason); - 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, diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index f8bf4cad2f..d39f47f556 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -41,6 +41,7 @@ #include "mag_calibration.h" #include "rc_calibration.h" +#include #include #include #include @@ -147,7 +148,8 @@ void WorkerThread::threadEntry() _ret_value = param_load_default(); if (_ret_value != 0) { - mavlink_log_critical(&_mavlink_log_pub, "Error loading settings"); + mavlink_log_critical(&_mavlink_log_pub, "Error loading settings\t"); + events::send(events::ID("commander_load_param_failed"), events::Log::Critical, "Error loading settings"); } break; @@ -156,7 +158,8 @@ void WorkerThread::threadEntry() _ret_value = param_save_default(); if (_ret_value != 0) { - mavlink_log_critical(&_mavlink_log_pub, "Error saving settings"); + mavlink_log_critical(&_mavlink_log_pub, "Error saving settings\t"); + events::send(events::ID("commander_save_param_failed"), events::Log::Critical, "Error saving settings"); } break;