fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: move mavlink_log_pub into class

Browse Source
This commit is contained in:
Daniel Agar
2020-12-02 10:20:50 +01:00
committed by Beat Küng
parent a03b91c01e
commit 741a0b43e8
2 changed files with 79 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+77 -83
View File
@@ -88,12 +88,7 @@ typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
} VEHICLE_MODE_FLAG; } VEHICLE_MODE_FLAG;
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
static struct vehicle_status_s status = {}; static struct vehicle_status_s status = {};
static struct vehicle_status_flags_s status_flags = {}; static struct vehicle_status_flags_s status_flags = {};
#if defined(BOARD_HAS_POWER_CONTROL) #if defined(BOARD_HAS_POWER_CONTROL)
@@ -374,24 +369,23 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
bool Commander::shutdown_if_allowed() bool Commander::shutdown_if_allowed()
{ {
return TRANSITION_DENIED != arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, return TRANSITION_DENIED != arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
&_armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, _arm_requirements, &_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);
} }
transition_result_t transition_result_t
Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, Commander::arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason)
arm_disarm_reason_t calling_reason)
{ {
transition_result_t arming_res = TRANSITION_NOT_CHANGED; transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will // Transition the armed state. By passing _mavlink_log_pub to arming_state_transition it will
// output appropriate error messages if the state cannot transition. // output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, arming_res = arming_state_transition(&status,
_safety, _safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&_armed, &_armed,
run_preflight_checks, run_preflight_checks,
mavlink_log_pub_local, &_mavlink_log_pub,
&status_flags, &status_flags,
_arm_requirements, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason); hrt_elapsed_time(&_boot_timestamp), calling_reason);
@@ -429,7 +423,7 @@ Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink
case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break; case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break;
} }
mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "Armed" : "Disarmed", reason); mavlink_log_info(&_mavlink_log_pub, "%s by %s", arm ? "Armed" : "Disarmed", reason);
} else if (arming_res == TRANSITION_DENIED) { } else if (arming_res == TRANSITION_DENIED) {
tune_negative(true); tune_negative(true);
@@ -501,7 +495,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; 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");
} }
} else { } else {
@@ -586,7 +580,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
default: default:
main_ret = TRANSITION_DENIED; main_ret = TRANSITION_DENIED;
mavlink_log_critical(&mavlink_log_pub, "Unsupported auto mode"); mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode");
break; break;
} }
@@ -645,7 +639,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
if (arming_ret == TRANSITION_DENIED) { if (arming_ret == TRANSITION_DENIED) {
mavlink_log_critical(&mavlink_log_pub, "Arming command rejected"); mavlink_log_critical(&_mavlink_log_pub, "Arming command rejected");
} }
} }
} }
@@ -656,7 +650,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters // for logic state parameters
if (static_cast<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) { if (static_cast<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 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", (double)cmd.param1);
} else { } else {
@@ -669,14 +663,14 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&status)) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&status)) {
if (cmd_arms) { if (cmd_arms) {
if (armed_local->armed) { if (armed_local->armed) {
mavlink_log_warning(&mavlink_log_pub, "Arming denied! Already armed"); mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed");
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Not landed"); mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Not landed");
} }
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Disarming denied! Not landed"); mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed");
} }
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
@@ -694,7 +688,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// Refuse to arm if preflight checks have failed // Refuse to arm if preflight checks have failed
if (status_local->hil_state != vehicle_status_s::HIL_STATE_ON if (status_local->hil_state != vehicle_status_s::HIL_STATE_ON
&& !status_flags.condition_system_sensors_initialized) { && !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Preflight checks have failed"); mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Preflight checks have failed");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break; break;
} }
@@ -705,7 +699,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (cmd_arms && throttle_above_center && if (cmd_arms && throttle_above_center &&
(status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not centered"); mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break; break;
} }
@@ -715,14 +709,14 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) { status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not zero"); mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break; break;
} }
} }
} }
transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, transition_result_t arming_res = arm_disarm(cmd_arms, !enforce,
(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));
if (arming_res == TRANSITION_DENIED) { if (arming_res == TRANSITION_DENIED) {
@@ -871,11 +865,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* switch to RTL which ends the mission */ /* switch to RTL which ends the mission */
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&_internal_state)) { &_internal_state)) {
mavlink_log_info(&mavlink_log_pub, "Returning to launch"); mavlink_log_info(&_mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Return to launch denied"); mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} }
} }
@@ -891,7 +885,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } 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");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} }
} }
@@ -900,11 +894,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
&_internal_state)) { &_internal_state)) {
mavlink_log_info(&mavlink_log_pub, "Landing at current position"); mavlink_log_info(&_mavlink_log_pub, "Landing at current position");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Landing denied! Please land manually"); mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} }
} }
@@ -913,11 +907,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND,
status_flags, &_internal_state)) { status_flags, &_internal_state)) {
mavlink_log_info(&mavlink_log_pub, "Precision landing"); mavlink_log_info(&_mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } 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");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} }
} }
@@ -936,18 +930,18 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// switch to AUTO_MISSION and ARM // switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&_internal_state)) &_internal_state))
&& (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, arm_disarm_reason_t::MISSION_START))) { && (TRANSITION_DENIED != arm_disarm(true, true, arm_disarm_reason_t::MISSION_START))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; 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");
} }
} }
} else { } 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");
} }
} }
break; break;
@@ -956,7 +950,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// if no high latency telemetry exists send a failed acknowledge // if no high latency telemetry exists send a failed acknowledge
if (_high_latency_datalink_heartbeat > _boot_timestamp) { if (_high_latency_datalink_heartbeat > _boot_timestamp) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; 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");
} }
} }
break; break;
@@ -1035,7 +1029,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &_armed, if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &_armed,
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, false /* fRunPreArmChecks */, &_mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT 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 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))
@@ -1073,7 +1067,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */ /* disable RC control input completely */
status_flags.rc_input_blocked = true; 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");
} else if ((int)(cmd.param4) == 2) { } else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */ /* RC trim calibration */
@@ -1108,7 +1102,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else if ((int)(cmd.param7) == 1) { } else if ((int)(cmd.param7) == 1) {
/* do esc calibration */ /* do esc calibration */
if (check_battery_disconnected(&mavlink_log_pub)) { if (check_battery_disconnected(&_mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
status_flags.condition_calibration_enabled = true; status_flags.condition_calibration_enabled = true;
_armed.in_esc_calibration_mode = true; _armed.in_esc_calibration_mode = true;
@@ -1123,7 +1117,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (status_flags.rc_input_blocked) { if (status_flags.rc_input_blocked) {
/* enable RC control input */ /* enable RC control input */
status_flags.rc_input_blocked = false; 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");
} }
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -1452,10 +1446,10 @@ Commander::run()
int32_t rc_map_arm_switch = 0; int32_t rc_map_arm_switch = 0;
status.system_id = _param_mav_sys_id.get(); status.system_id = _param_mav_sys_id.get();
arm_auth_init(&mavlink_log_pub, &status.system_id); arm_auth_init(&_mavlink_log_pub, &status.system_id);
// run preflight immediately to find all relevant parameters, but don't report // run preflight immediately to find all relevant parameters, but don't report
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, _arm_requirements.global_position, false, true, PreFlightCheck::preflightCheck(&_mavlink_log_pub, status, status_flags, _arm_requirements.global_position, false, true,
hrt_elapsed_time(&_boot_timestamp)); hrt_elapsed_time(&_boot_timestamp));
while (!should_exit()) { while (!should_exit()) {
@@ -1475,14 +1469,14 @@ Commander::run()
// NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 // NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
if (_param_nav_dll_act.get() == 4) { if (_param_nav_dll_act.get() == 4) {
mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_DLL_ACT = 4) retired"); 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.set(2); // value 2 Return mode
_param_nav_dll_act.commit_no_notification(); _param_nav_dll_act.commit_no_notification();
} }
// NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 // NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
if (_param_nav_rcl_act.get() == 4) { if (_param_nav_rcl_act.get() == 4) {
mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_RCL_ACT = 4) retired"); 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.set(2); // value 2 Return mode
_param_nav_rcl_act.commit_no_notification(); _param_nav_rcl_act.commit_no_notification();
} }
@@ -1548,7 +1542,7 @@ Commander::run()
if (airmode == 2 && rc_map_arm_switch == 0) { if (airmode == 2 && rc_map_arm_switch == 0) {
airmode = 1; // change to roll/pitch airmode airmode = 1; // change to roll/pitch airmode
param_set(param_airmode, &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")
} }
} }
@@ -1612,7 +1606,7 @@ Commander::run()
* without a need. The clean approach to unload it is to reboot. * without a need. The clean approach to unload it is to reboot.
*/ */
if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { 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");
while (1) { px4_usleep(1); } while (1) { px4_usleep(1); }
} }
@@ -1633,10 +1627,10 @@ Commander::run()
// Only take actions if armed // Only take actions if armed
if (_armed.armed) { if (_armed.armed) {
if (!was_landed && _land_detector.landed) { if (!was_landed && _land_detector.landed) {
mavlink_log_info(&mavlink_log_pub, "Landing detected"); mavlink_log_info(&_mavlink_log_pub, "Landing detected");
} else if (was_landed && !_land_detector.landed) { } else if (was_landed && !_land_detector.landed) {
mavlink_log_info(&mavlink_log_pub, "Takeoff detected"); mavlink_log_info(&_mavlink_log_pub, "Takeoff detected");
_time_at_takeoff = hrt_absolute_time(); _time_at_takeoff = hrt_absolute_time();
_have_taken_off_since_arming = true; _have_taken_off_since_arming = true;
@@ -1680,7 +1674,7 @@ Commander::run()
} }
if (safety_disarm_allowed) { if (safety_disarm_allowed) {
if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::SAFETY_BUTTON)) { if (TRANSITION_CHANGED == arm_disarm(false, true, arm_disarm_reason_t::SAFETY_BUTTON)) {
_status_changed = true; _status_changed = true;
} }
} }
@@ -1774,7 +1768,7 @@ Commander::run()
} }
if (_auto_disarm_landed.get_state()) { if (_auto_disarm_landed.get_state()) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm(false, true,
(_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT)); (_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT));
} }
} }
@@ -1792,10 +1786,10 @@ Commander::run()
if (_auto_disarm_killed.get_state()) { if (_auto_disarm_killed.get_state()) {
if (_armed.manual_lockdown) { if (_armed.manual_lockdown) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::KILL_SWITCH); arm_disarm(false, true, arm_disarm_reason_t::KILL_SWITCH);
} else { } else {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::LOCKDOWN); arm_disarm(false, true, arm_disarm_reason_t::LOCKDOWN);
} }
} }
@@ -1822,7 +1816,7 @@ Commander::run()
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, true /* fRunPreArmChecks */, &_mavlink_log_pub, &status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp), _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY); arm_disarm_reason_t::TRANSITION_TO_STANDBY);
@@ -1851,7 +1845,7 @@ Commander::run()
_status_changed = true; _status_changed = true;
if (status.mission_failure) { if (status.mission_failure) {
mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed"); mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed");
} }
} }
@@ -1934,7 +1928,7 @@ Commander::run()
case (geofence_result_s::GF_ACTION_TERMINATE) : { case (geofence_result_s::GF_ACTION_TERMINATE) : {
PX4_WARN("Flight termination because of geofence"); PX4_WARN("Flight termination because of geofence");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated"); mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
_armed.force_failsafe = true; _armed.force_failsafe = true;
_status_changed = true; _status_changed = true;
break; break;
@@ -2008,7 +2002,7 @@ Commander::run()
(fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
// revert to position control in any case // revert to position control in any case
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
mavlink_log_info(&mavlink_log_pub, "Pilot took over control using sticks"); mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
} }
} }
@@ -2020,12 +2014,12 @@ Commander::run()
_status_changed = true; _status_changed = true;
if (!_flight_termination_printed) { if (!_flight_termination_printed) {
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated"); mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
_flight_termination_printed = true; _flight_termination_printed = true;
} }
if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { 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");
} }
} }
@@ -2042,7 +2036,7 @@ Commander::run()
} else { } else {
if (status.rc_signal_lost) { if (status.rc_signal_lost) {
if (_rc_signal_lost_timestamp > 0) { if (_rc_signal_lost_timestamp > 0) {
mavlink_log_info(&mavlink_log_pub, "Manual control regained after %.1fs", mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs",
hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6); hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6);
} }
@@ -2086,7 +2080,7 @@ Commander::run()
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, true /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), &_mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); (arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
} }
@@ -2141,7 +2135,7 @@ Commander::run()
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &_armed, arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &_armed,
!in_rearming_grace_period /* fRunPreArmChecks */, !in_rearming_grace_period /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), &_mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); (arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
if (arming_ret != TRANSITION_CHANGED) { if (arming_ret != TRANSITION_CHANGED) {
@@ -2187,10 +2181,10 @@ Commander::run()
const char kill_switch_string[] = "Kill-switch engaged"; const char kill_switch_string[] = "Kill-switch engaged";
if (_land_detector.landed) { if (_land_detector.landed) {
mavlink_log_info(&mavlink_log_pub, kill_switch_string); mavlink_log_info(&_mavlink_log_pub, kill_switch_string);
} else { } else {
mavlink_log_critical(&mavlink_log_pub, kill_switch_string); mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
} }
_status_changed = true; _status_changed = true;
@@ -2199,7 +2193,7 @@ Commander::run()
} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { } else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (_armed.manual_lockdown) { if (_armed.manual_lockdown) {
mavlink_log_info(&mavlink_log_pub, "Kill-switch disengaged"); mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged");
_status_changed = true; _status_changed = true;
_armed.manual_lockdown = false; _armed.manual_lockdown = false;
} }
@@ -2212,7 +2206,7 @@ Commander::run()
if (status_flags.rc_signal_found_once && !status.rc_signal_lost) { if (status_flags.rc_signal_found_once && !status.rc_signal_lost) {
// ignore RC lost during calibration // ignore RC lost during calibration
if (!status_flags.condition_calibration_enabled && !status_flags.rc_input_blocked) { 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");
status.rc_signal_lost = true; status.rc_signal_lost = true;
_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp; _rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
@@ -2329,8 +2323,8 @@ Commander::run()
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&time_at_arm) < 500_ms) { if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR); arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR);
mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request"); mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request");
} }
} }
@@ -2342,14 +2336,14 @@ Commander::run()
// This handles the case where something fails during the early takeoff phase // This handles the case where something fails during the early takeoff phase
_armed.lockdown = true; _armed.lockdown = true;
_lockdown_triggered = true; _lockdown_triggered = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown"); mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown");
} else if (!status_flags.circuit_breaker_flight_termination_disabled && } else if (!status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered && !_lockdown_triggered) { !_flight_termination_triggered && !_lockdown_triggered) {
_armed.force_failsafe = true; _armed.force_failsafe = true;
_flight_termination_triggered = 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");
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
} }
} }
@@ -2425,7 +2419,7 @@ Commander::run()
bool nav_state_changed = set_nav_state(&status, bool nav_state_changed = set_nav_state(&status,
&_armed, &_armed,
&_internal_state, &_internal_state,
&mavlink_log_pub, &_mavlink_log_pub,
(link_loss_actions_t)_param_nav_dll_act.get(), (link_loss_actions_t)_param_nav_dll_act.get(),
_mission_result_sub.get().finished, _mission_result_sub.get().finished,
_mission_result_sub.get().stay_in_failsafe, _mission_result_sub.get().stay_in_failsafe,
@@ -2444,10 +2438,10 @@ Commander::run()
_status_changed = true; _status_changed = true;
if (status.failsafe) { if (status.failsafe) {
mavlink_log_info(&mavlink_log_pub, "Failsafe mode activated"); mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated");
} else { } else {
mavlink_log_info(&mavlink_log_pub, "Failsafe mode deactivated"); mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated");
} }
_failsafe_old = status.failsafe; _failsafe_old = status.failsafe;
@@ -3471,7 +3465,7 @@ Commander::print_reject_mode(const char *msg)
if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
_last_print_mode_reject_time = t; _last_print_mode_reject_time = t;
mavlink_log_critical(&mavlink_log_pub, "REJECT %s", msg); mavlink_log_critical(&_mavlink_log_pub, "REJECT %s", msg);
/* only buzz if armed, because else we're driving people nuts indoors /* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */ they really need to look at the leds as well. */
@@ -3486,7 +3480,7 @@ Commander::print_reject_arm(const char *msg)
if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
_last_print_mode_reject_time = t; _last_print_mode_reject_time = t;
mavlink_log_critical(&mavlink_log_pub, "%s", msg); mavlink_log_critical(&_mavlink_log_pub, "%s", msg);
tune_negative(true); tune_negative(true);
} }
} }
@@ -3637,12 +3631,12 @@ void Commander::data_link_check()
_status_changed = true; _status_changed = true;
if (_datalink_last_heartbeat_gcs != 0) { if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained"); mavlink_log_info(&_mavlink_log_pub, "Data link regained");
} }
if (!_armed.armed && !status_flags.condition_calibration_enabled) { if (!_armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS // make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, PreFlightCheck::preflightCheck(&_mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp)); _arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
} }
} }
@@ -3656,7 +3650,7 @@ void Commander::data_link_check()
_status_changed = true; _status_changed = true;
if (_datalink_last_heartbeat_onboard_controller != 0) { 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");
} }
} }
@@ -3684,7 +3678,7 @@ void Commander::data_link_check()
status.data_link_lost = true; status.data_link_lost = true;
status.data_link_lost_counter++; status.data_link_lost_counter++;
mavlink_log_critical(&mavlink_log_pub, "Connection to ground station lost"); mavlink_log_critical(&_mavlink_log_pub, "Connection to ground station lost");
_status_changed = true; _status_changed = true;
} }
@@ -3695,7 +3689,7 @@ void Commander::data_link_check()
&& (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > 5_s) && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > 5_s)
&& !_onboard_controller_lost) { && !_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");
_onboard_controller_lost = true; _onboard_controller_lost = true;
_status_changed = true; _status_changed = true;
} }
@@ -3718,7 +3712,7 @@ void Commander::data_link_check()
if (!status.high_latency_data_link_lost) { if (!status.high_latency_data_link_lost) {
status.high_latency_data_link_lost = true; 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");
_status_changed = true; _status_changed = true;
} }
} }
@@ -3833,7 +3827,7 @@ void Commander::battery_status_check()
// execute battery failsafe if the state has gotten worse while we are armed // execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) { if (battery_warning_level_increased_while_armed) {
battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning, battery_failsafe(&_mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
(low_battery_action_t)_param_com_low_bat_act.get()); (low_battery_action_t)_param_com_low_bat_act.get());
} }
@@ -3844,12 +3838,12 @@ void Commander::battery_status_check()
#if defined(CONFIG_BOARDCTL_POWEROFF) #if defined(CONFIG_BOARDCTL_POWEROFF)
if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { 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");
while (1) { px4_usleep(1); } while (1) { px4_usleep(1); }
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown");
} }
#endif // CONFIG_BOARDCTL_POWEROFF #endif // CONFIG_BOARDCTL_POWEROFF
@@ -3896,7 +3890,7 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
if (!mag_fault_prev && mag_fault) { if (!mag_fault_prev && mag_fault) {
mavlink_log_critical(&mavlink_log_pub, "Stopping compass use! Check calibration on landing"); mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing");
} }
// Set the allowable position uncertainty based on combination of flight and estimator state // Set the allowable position uncertainty based on combination of flight and estimator state
@@ -3949,7 +3943,7 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
// if the innovation test has failed continuously, declare the nav as failed // if the innovation test has failed continuously, declare the nav as failed
if (hrt_elapsed_time(&_time_last_innov_pass) > 1_s) { if (hrt_elapsed_time(&_time_last_innov_pass) > 1_s) {
_nav_test_failed = true; _nav_test_failed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical navigation failure! Check sensor calibration"); mavlink_log_emergency(&_mavlink_log_pub, "Critical navigation failure! Check sensor calibration");
} }
} }
} }
@@ -4050,7 +4044,7 @@ void Commander::esc_status_check(const esc_status_s &esc_status)
} }
} }
mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg); mavlink_log_critical(&_mavlink_log_pub, "%soffline", esc_fail_msg);
_last_esc_online_flags = esc_status.esc_online_flags; _last_esc_online_flags = esc_status.esc_online_flags;
status_flags.condition_escs_error = true; status_flags.condition_escs_error = true;
src/modules/commander/Commander.hpp
+2 -2
View File
@@ -118,8 +118,7 @@ public:
private: private:
void answer_command(const vehicle_command_s &cmd, uint8_t result); void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, transition_result_t arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason);
arm_disarm_reason_t calling_reason);
void battery_status_check(); void battery_status_check();
@@ -430,4 +429,5 @@ private:
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _mavlink_log_pub{nullptr};
}; };