mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-08 02:20:34 +08:00
commander: switch to events
This commit is contained in:
Beat Küng
+310
-110
@@ -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()
|
||||
{
|
||||
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::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)
|
||||
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)
|
||||
|
||||
} 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<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)
|
||||
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 <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()
|
||||
* 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<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()
|
||||
|
||||
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.
|
||||
*
|
||||
* <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()
|
||||
_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::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()
|
||||
_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<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()
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
} 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;
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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::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
|
||||
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::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_
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
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,
|
||||
|
||||
@@ -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()
|
||||
_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;
|
||||
|
||||
Reference in New Issue
Block a user