zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: switch to events

master
Beat Küng 4 years ago committed by Daniel Agar
parent
8866215d1d
commit
5ac43e7236
8 changed files with 625 additions and 168 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 192
      src/lib/events/enums.json
  2. 420
      src/modules/commander/Commander.cpp
  3. 5
      src/modules/commander/calibration_routines.cpp
  4. 2
      src/modules/commander/commander_tests/state_machine_helper_test.cpp
  5. 12
      src/modules/commander/rc_calibration.cpp
  6. 135
      src/modules/commander/state_machine_helper.cpp
  7. 20
      src/modules/commander/state_machine_helper.h
  8. 7
      src/modules/commander/worker_thread.cpp

192
src/lib/events/enums.json
Unescape View File

@ -48,6 +48,198 @@ @@ -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]"
}
}
}
}
}

420
src/modules/commander/Commander.cpp
Unescape View File

@ -60,6 +60,7 @@ @@ -60,6 +60,7 @@
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/external_reset_lockout.h>
@ -426,44 +427,83 @@ bool Commander::shutdown_if_allowed() @@ -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_ @@ -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_ @@ -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_ @@ -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::px4::enums::arm_disarm_reason_t>(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) @@ -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::px4::enums::arm_disarm_reason_t>(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) @@ -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) @@ -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) @@ -842,7 +903,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
const int param1_arm = static_cast<int>(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<float>(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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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 <param>MC_AIRMODE</param> 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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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<float>(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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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.
*
* <profile name="dev">
* This can be configured with the parameter <param>COM_LKDOWN_TKO</param>.
* </profile>
*/
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.
*
* <profile name="dev">
* Flight termination can be disabled with the parameter <param>CBRK_FLIGHTTERM</param>.
* </profile>
*/
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() @@ -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) @@ -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::px4::enums::navigation_mode_t>(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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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<uint8_t>(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() @@ -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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t, uint8_t>(events::ID("commander_esc_generic_failure"), events::Log::Critical,
"ESC{1}: generic failure (code {2})", index + 1, esc_status.esc[index].esc_state);
}
}

5
src/modules/commander/calibration_routines.cpp
Unescape View File

@ -39,6 +39,7 @@ @@ -39,6 +39,7 @@
*/
#include <px4_platform_common/defines.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
@ -345,7 +346,9 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &ca @@ -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<uint32_t>(events::ID("commander_cal_cmd_denied"), {events::Log::Error, events::LogInternal::Info},
"Command denied during calibration: {1}", cmd.command);
tune_negative(true);
ret = false;
}

2
src/modules/commander/commander_tests/state_machine_helper_test.cpp
Unescape View File

@ -306,7 +306,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() @@ -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);

12
src/modules/commander/rc_calibration.cpp
Unescape View File

@ -36,6 +36,7 @@ @@ -36,6 +36,7 @@
* Remote Control calibration routine
*/
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/defines.h>
@ -57,7 +58,9 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) @@ -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) @@ -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;
}

135
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -86,6 +86,28 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { @@ -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 @@ -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::px4::enums::arming_state_t, events::px4::enums::arming_state_t>(
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 @@ -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 * @@ -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::px4::enums::failsafe_reason_t>(
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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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_ @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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;
}
}

20
src/modules/commander/state_machine_helper.h
Unescape View File

@ -53,6 +53,7 @@ @@ -53,6 +53,7 @@
#include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <px4_platform_common/events.h>
typedef enum {
TRANSITION_DENIED = -1,
@ -98,22 +99,7 @@ enum class position_nav_loss_actions_t { @@ -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 @@ -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,

7
src/modules/commander/worker_thread.cpp
Unescape View File

@ -41,6 +41,7 @@ @@ -41,6 +41,7 @@
#include "mag_calibration.h"
#include "rc_calibration.h"
#include <px4_platform_common/events.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/shutdown.h>
#include <parameters/param.h>
@ -147,7 +148,8 @@ void WorkerThread::threadEntry() @@ -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() @@ -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;

Write Preview
Loading…
Cancel
Save