From f9a13aa82f90b1c484ed79666991fea686c2fbc9 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Thu, 6 Apr 2023 12:18:08 -0600 Subject: [PATCH] Format/group/organize/alphabetize methods and variables in Commander.cpp/hpp, format GeofenceResult.msg Scope VEHICLE_MODE_FLAG to the Commander class. --- msg/GeofenceResult.msg | 22 +- src/modules/commander/Commander.cpp | 308 +++++++++++++--------------- src/modules/commander/Commander.hpp | 284 ++++++++++++------------- 3 files changed, 300 insertions(+), 314 deletions(-) diff --git a/msg/GeofenceResult.msg b/msg/GeofenceResult.msg index 4121f6f502..16b15466c4 100644 --- a/msg/GeofenceResult.msg +++ b/msg/GeofenceResult.msg @@ -1,14 +1,14 @@ -uint64 timestamp # time since system start (microseconds) -uint8 GF_ACTION_NONE = 0 # no action on geofence violation -uint8 GF_ACTION_WARN = 1 # critical mavlink message -uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER -uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL -uint8 GF_ACTION_TERMINATE = 4 # flight termination -uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND +uint64 timestamp # time since system start (microseconds) +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND -uint8 geofence_violation_reason # one of geofence_violation_reason_t::* +uint8 geofence_violation_reason # one of geofence_violation_reason_t::* -bool primary_geofence_breached # true if the primary geofence is breached -uint8 primary_geofence_action # action to take when the primary geofence is breached +bool primary_geofence_breached # true if the primary geofence is breached +uint8 primary_geofence_action # action to take when the primary geofence is breached -bool home_required # true if the geofence requires a valid home position +bool home_required # true if the geofence requires a valid home position diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c317f6bda..59845c4eea 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -32,23 +32,22 @@ ****************************************************************************/ /** - * @file commander.cpp - * - * Main state machine / business logic - * + * @file Commander.cpp + * Primary state machine and flight stack logic */ #include "Commander.hpp" -/* commander module headers */ +/* Commander module headers */ #include "Arming/ArmAuthorization/ArmAuthorization.h" #include "commander_helper.h" #include "esc_calibration.h" -#define DEFINE_GET_PX4_CUSTOM_MODE -#include "px4_custom_mode.h" #include "ModeUtil/control_mode.hpp" #include "ModeUtil/conversions.hpp" +#define DEFINE_GET_PX4_CUSTOM_MODE +#include "px4_custom_mode.h" + /* PX4 headers */ #include #include @@ -69,23 +68,8 @@ #include #include -#include -#include - -typedef enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ - VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ -} VEHICLE_MODE_FLAG; - // TODO: generate -static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b) +static constexpr bool operator == (const actuator_armed_s &a, const actuator_armed_s &b) { return (a.armed == b.armed && a.prearmed == b.prearmed && @@ -95,9 +79,11 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme a.force_failsafe == b.force_failsafe && a.in_esc_calibration_mode == b.in_esc_calibration_mode); } + static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review"); #if defined(BOARD_HAS_POWER_CONTROL) +static orb_advert_t power_button_state_pub = nullptr; static orb_advert_t tune_control_pub = nullptr; static void play_power_button_down_tune() @@ -114,7 +100,6 @@ static void stop_tune() orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); } -static orb_advert_t power_button_state_pub = nullptr; static int power_button_state_notification_cb(board_power_button_state_notification_e request) { // Note: this can be called from IRQ handlers, so we publish a message that will be handled @@ -228,6 +213,56 @@ static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = N } #endif +Commander::Commander() : + ModuleParams(nullptr) +{ + _vehicle_land_detected.landed = true; + + _vehicle_status.system_id = 1; + _vehicle_status.component_id = 1; + + _vehicle_status.system_type = 0; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; + + _vehicle_status.nav_state = _user_mode_intention.get(); + _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); + _vehicle_status.nav_state_timestamp = hrt_absolute_time(); + + /* mark all signals lost as long as they haven't been found */ + _vehicle_status.gcs_connection_lost = true; + + _vehicle_status.power_input_valid = true; + + // default for vtol is rotary wing + _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + + _param_mav_comp_id = param_find("MAV_COMP_ID"); + _param_mav_sys_id = param_find("MAV_SYS_ID"); + _param_mav_type = param_find("MAV_TYPE"); + _param_rc_map_fltmode = param_find("RC_MAP_FLTMODE"); + + updateParameters(); +} + +Commander::~Commander() +{ + perf_free(_loop_perf); + perf_free(_preflight_check_perf); +} + +Commander *Commander::instantiate(int argc, char *argv[]) +{ + Commander *instance = new Commander(); + + if (instance) { + if (argc >= 2 && !strcmp(argv[1], "-h")) { + instance->enableHIL(); + } + } + + return instance; +} + int Commander::custom_command(int argc, char *argv[]) { if (!is_running()) { @@ -471,22 +506,6 @@ int Commander::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int Commander::print_status() -{ - PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); - PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]); - PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]); - PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no"); - perf_print_counter(_loop_perf); - perf_print_counter(_preflight_check_perf); - return 0; -} - -extern "C" __EXPORT int commander_main(int argc, char *argv[]) -{ - return Commander::main(argc, argv); -} - bool Commander::shutdownIfAllowed() { return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, @@ -638,45 +657,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f return arming_res; } -Commander::Commander() : - ModuleParams(nullptr) -{ - _vehicle_land_detected.landed = true; - - _vehicle_status.system_id = 1; - _vehicle_status.component_id = 1; - - _vehicle_status.system_type = 0; - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - - _vehicle_status.nav_state = _user_mode_intention.get(); - _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); - _vehicle_status.nav_state_timestamp = hrt_absolute_time(); - - /* mark all signals lost as long as they haven't been found */ - _vehicle_status.gcs_connection_lost = true; - - _vehicle_status.power_input_valid = true; - - // default for vtol is rotary wing - _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - - _param_mav_comp_id = param_find("MAV_COMP_ID"); - _param_mav_sys_id = param_find("MAV_SYS_ID"); - _param_mav_type = param_find("MAV_TYPE"); - _param_rc_map_fltmode = param_find("RC_MAP_FLTMODE"); - - updateParameters(); -} - -Commander::~Commander() -{ - perf_free(_loop_perf); - perf_free(_preflight_check_perf); -} - -bool -Commander::handle_command(const vehicle_command_s &cmd) +bool Commander::handleCommand(const vehicle_command_s &cmd) { /* only handle commands that are meant to be handled by this system and component, or broadcast */ if (((cmd.target_system != _vehicle_status.system_id) && (cmd.target_system != 0)) @@ -738,7 +719,7 @@ Commander::handle_command(const vehicle_command_s &cmd) uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX; transition_result_t main_ret = TRANSITION_NOT_CHANGED; - if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { + if (base_mode == VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; @@ -805,14 +786,14 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { /* use base mode */ - if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + if (base_mode == VEHICLE_MODE_FLAG_AUTO_ENABLED) { desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + } else if (base_mode == VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode == VEHICLE_MODE_FLAG_GUIDED_ENABLED) { desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { + } else if (base_mode == VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; } else { @@ -913,7 +894,7 @@ Commander::handle_command(const vehicle_command_s &cmd) _actuator_armed.force_failsafe = true; _flight_termination_triggered = true; PX4_WARN("forcing failsafe (termination)"); - send_parachute_command(); + sendParachuteCommand(); } } else { @@ -1126,13 +1107,13 @@ Commander::handle_command(const vehicle_command_s &cmd) if (param1 == 0) { // 0: Do nothing for autopilot - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); #if defined(CONFIG_BOARDCTL_RESET) } else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) { // 1: Reboot autopilot - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); while (1) { px4_usleep(1); } @@ -1142,7 +1123,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) { // 2: Shutdown autopilot - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); while (1) { px4_usleep(1); } @@ -1152,14 +1133,14 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); while (1) { px4_usleep(1); } #endif // CONFIG_BOARDCTL_RESET } else { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); } } @@ -1170,7 +1151,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { @@ -1181,14 +1162,14 @@ Commander::handle_command(const vehicle_command_s &cmd) (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) ) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); break; } if ((int)(cmd.param1) == 1) { /* gyro calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::GyroCalibration); @@ -1200,19 +1181,19 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::MagCalibration); } else if ((int)(cmd.param3) == 1) { /* baro calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::BaroCalibration); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); /* disable RC control input completely */ _vehicle_status.rc_calibration_in_progress = true; mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t"); @@ -1221,39 +1202,39 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AccelCalibration); } else if ((int)(cmd.param5) == 2) { // board offset calibration - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::LevelCalibration); } else if ((int)(cmd.param5) == 4) { // accelerometer quick calibration - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) /* airspeed calibration */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ if (check_battery_disconnected(&_mavlink_log_pub)) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) { mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t"); @@ -1267,7 +1248,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } } else { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); } } else if ((int)(cmd.param4) == 0) { @@ -1280,10 +1261,10 @@ Commander::handle_command(const vehicle_command_s &cmd) "Calibration: Restoring RC input"); } - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); } else { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } } @@ -1295,10 +1276,10 @@ Commander::handle_command(const vehicle_command_s &cmd) if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); // parameter 1: Heading (degrees) // parameter 3: Latitude (degrees) // parameter 4: Longitude (degrees) @@ -1330,28 +1311,28 @@ Commander::handle_command(const vehicle_command_s &cmd) if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { if (((int)(cmd.param1)) == 0) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _worker_thread.startTask(WorkerThread::Request::ParamLoadDefault); } else if (((int)(cmd.param1)) == 1) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _worker_thread.startTask(WorkerThread::Request::ParamSaveDefault); } else if (((int)(cmd.param1)) == 2) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig); } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory); } else if (((int)(cmd.param1)) == 4) { - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); _worker_thread.startTask(WorkerThread::Request::ParamResetAll); } } @@ -1361,7 +1342,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS: _health_and_arming_checks.update(true); - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); break; case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: @@ -1410,13 +1391,13 @@ Commander::handle_command(const vehicle_command_s &cmd) default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); break; } if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(cmd, cmd_result); + answerCommand(cmd, cmd_result); } return true; @@ -1540,7 +1521,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) _status_changed = true; _actuator_armed.manual_lockdown = true; - send_parachute_command(); + sendParachuteCommand(); } break; @@ -1678,7 +1659,7 @@ void Commander::run() handleAutoDisarm(); - battery_status_check(); + batteryStatusCheck(); /* If in INIT state, try to proceed to STANDBY state */ if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) { @@ -1735,7 +1716,7 @@ void Commander::run() PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); } - if (handle_command(cmd)) { + if (handleCommand(cmd)) { _status_changed = true; } } @@ -1818,7 +1799,7 @@ void Commander::run() checkWorkerThread(); updateTunes(); - control_status_leds(_status_changed, _battery_warning); + controlStatusLEDs(_status_changed, _battery_warning); _status_changed = false; @@ -2214,7 +2195,7 @@ bool Commander::handleModeIntentionAndFailsafe() if (!_flight_termination_triggered) { _flight_termination_triggered = true; - send_parachute_command(); + sendParachuteCommand(); } break; @@ -2250,7 +2231,7 @@ void Commander::checkAndInformReadyForTakeoff() #endif // CONFIG_ARCH_BOARD_PX4_SITL } -void Commander::control_status_leds(bool changed, const uint8_t battery_warning) +void Commander::controlStatusLEDs(bool changed, const uint8_t battery_warning) { switch (blink_msg_state()) { case 1: @@ -2432,7 +2413,7 @@ void Commander::printRejectMode(uint8_t nav_state) } } -void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result) +void Commander::answerCommand(const vehicle_command_s &cmd, uint8_t result) { switch (result) { case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -2468,43 +2449,7 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result) _vehicle_command_ack_pub.publish(command_ack); } -int Commander::task_spawn(int argc, char *argv[]) -{ - _task_id = px4_task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 40, - 3250, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; - } - - // wait until task is up & running - if (wait_until_running() < 0) { - _task_id = -1; - return -1; - } - - return 0; -} - -Commander *Commander::instantiate(int argc, char *argv[]) -{ - Commander *instance = new Commander(); - - if (instance) { - if (argc >= 2 && !strcmp(argv[1], "-h")) { - instance->enable_hil(); - } - } - - return instance; -} - -void Commander::enable_hil() +void Commander::enableHIL() { _vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON; } @@ -2690,7 +2635,7 @@ void Commander::dataLinkCheck() } } -void Commander::battery_status_check() +void Commander::batteryStatusCheck() { // Handle shutdown request from emergency battery action if (_battery_warning != _failsafe_flags.battery_warning) { @@ -2788,7 +2733,7 @@ void Commander::offboardControlCheck() } } -void Commander::send_parachute_command() +void Commander::sendParachuteCommand() { vehicle_command_s vcmd{}; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE; @@ -2807,6 +2752,17 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } +int Commander::print_status() +{ + PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); + PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]); + PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]); + PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no"); + perf_print_counter(_loop_perf); + perf_print_counter(_preflight_check_perf); + return 0; +} + int Commander::print_usage(const char *reason) { if (reason) { @@ -2849,3 +2805,31 @@ The commander module contains the state machine for mode switching and failsafe return 1; } + +int Commander::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 40, + 3250, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + // wait until task is up & running + if (wait_until_running() < 0) { + _task_id = -1; + return -1; + } + + return 0; +} + +extern "C" __EXPORT int commander_main(int argc, char *argv[]) +{ + return Commander::main(argc, argv); +} diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3678b9ea9d..008784491e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -50,25 +50,21 @@ #include #include -// publications #include -#include -#include -#include -#include -#include -#include - -// subscriptions #include #include #include + +#include +#include +#include #include #include #include #include #include #include +#include #include #include #include @@ -78,10 +74,14 @@ #include #include #include +#include #include +#include +#include #include #include #include +#include #include using math::constrain; @@ -96,13 +96,13 @@ public: ~Commander(); /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); + static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static Commander *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); + /** @see ModuleBase::print_status() */ + int print_status() override; /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); @@ -110,79 +110,12 @@ public: /** @see ModuleBase::run() */ void run() override; - /** @see ModuleBase::print_status() */ - int print_status() override; + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - void enable_hil(); + void enableHIL(); private: - void answer_command(const vehicle_command_s &cmd, uint8_t result); - - transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); - - transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false); - - void battery_status_check(); - - void control_status_leds(bool changed, const uint8_t battery_warning); - - /** - * Checks the status of all available data links and handles switching between different system telemetry states. - */ - void dataLinkCheck(); - - void manualControlCheck(); - - void offboardControlCheck(); - - /** - * @brief Handle incoming vehicle command relavant to Commander - * - * It ignores irrelevant vehicle commands defined inside the switch case statement - * in the function. - * - * @param cmd Vehicle command to handle - */ - bool handle_command(const vehicle_command_s &cmd); - - unsigned handleCommandActuatorTest(const vehicle_command_s &cmd); - - void executeActionRequest(const action_request_s &action_request); - - void printRejectMode(uint8_t nav_state); - - void updateControlMode(); - - bool shutdownIfAllowed(); - - void send_parachute_command(); - - void checkForMissionUpdate(); - - void handlePowerButtonState(); - - void systemPowerUpdate(); - - void landDetectorUpdate(); - - void safetyButtonUpdate(); - - void vtolStatusUpdate(); - - void updateTunes(); - - void checkWorkerThread(); - - bool getPrearmState() const; - - void handleAutoDisarm(); - - bool handleModeIntentionAndFailsafe(); - - void updateParameters(); - - void checkAndInformReadyForTakeoff(); - enum class PrearmedMode { DISABLED = 0, SAFETY_BUTTON = 1, @@ -194,108 +127,177 @@ private: OFFBOARD_MODE_BIT = (1 << 1), }; + enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ + }; + + transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); + + transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false); + + void answerCommand(const vehicle_command_s &cmd, uint8_t result); + + void batteryStatusCheck(); + + void checkAndInformReadyForTakeoff(); + + void checkForMissionUpdate(); + + void checkWorkerThread(); + + void controlStatusLEDs(bool changed, const uint8_t battery_warning); + + /** Checks the status of all available data links and handles switching between different system telemetry states. */ + void dataLinkCheck(); + + void executeActionRequest(const action_request_s &action_request); + + bool getPrearmState() const; + + void handleAutoDisarm(); + + bool handleCommand(const vehicle_command_s &cmd); + + unsigned handleCommandActuatorTest(const vehicle_command_s &cmd); + + bool handleModeIntentionAndFailsafe(); + + void handlePowerButtonState(); + + void landDetectorUpdate(); + + void manualControlCheck(); + + void offboardControlCheck(); + + void printRejectMode(uint8_t nav_state); + + void safetyButtonUpdate(); + + void sendParachuteCommand(); + + bool shutdownIfAllowed(); + + void systemPowerUpdate(); + + void updateControlMode(); + + void updateParameters(); + + void updateTunes(); + + void vtolStatusUpdate(); + /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; - vehicle_status_s _vehicle_status{}; - - ArmStateMachine _arm_state_machine{}; - Failsafe _failsafe_instance{this}; - FailsafeBase &_failsafe{_failsafe_instance}; - FailureDetector _failure_detector{this}; - HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; - Safety _safety{}; - UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; - WorkerThread _worker_thread{}; + ArmStateMachine _arm_state_machine{}; + Failsafe _failsafe_instance{this}; + FailsafeBase &_failsafe{_failsafe_instance}; + FailureDetector _failure_detector{this}; + HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; + Safety _safety{}; + UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; + WorkerThread _worker_thread{}; const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()}; - HomePosition _home_position{_failsafe_flags}; + HomePosition _home_position{_failsafe_flags}; Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_killed{false}; + hrt_abstime _boot_timestamp{0}; + hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; - hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent - hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; - hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0}; - hrt_abstime _overload_start{0}; ///< time when CPU overload started + hrt_abstime _last_health_and_arming_check{0}; + hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent + + hrt_abstime _led_overload_toggle{0}; + + hrt_abstime _overload_start{0}; ///< time when CPU overload started #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) hrt_abstime _led_armed_state_toggle {0}; #endif - hrt_abstime _led_overload_toggle {0}; - hrt_abstime _last_health_and_arming_check{0}; + actuator_armed_s _actuator_armed{}; + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_land_detected_s _vehicle_land_detected{}; + vehicle_status_s _vehicle_status{}; + vtol_vehicle_status_s _vtol_vehicle_status{}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; + uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; - bool _failsafe_user_override_request{false}; ///< override request due to stick movements - - bool _flight_termination_triggered{false}; - bool _lockdown_triggered{false}; - - bool _open_drone_id_system_lost{true}; + bool _arm_tune_played{false}; bool _avoidance_system_lost{false}; - bool _onboard_controller_lost{false}; - bool _parachute_system_lost{true}; - bool _last_overload{false}; - bool _mode_switch_mapped{false}; + bool _failsafe_user_override_request{false}; // override request due to stick movements + bool _flight_termination_triggered{false}; + + bool _have_taken_off_since_arming{false}; bool _is_throttle_above_center{false}; bool _is_throttle_low{false}; - bool _arm_tune_played{false}; - bool _was_armed{false}; - bool _have_taken_off_since_arming{false}; + bool _last_overload{false}; + bool _lockdown_triggered{false}; + + bool _mode_switch_mapped{false}; + + bool _onboard_controller_lost{false}; + bool _open_drone_id_system_lost{true}; + + bool _parachute_system_lost{true}; + bool _status_changed{true}; - - vehicle_land_detected_s _vehicle_land_detected{}; - - // commander publications - actuator_armed_s _actuator_armed{}; - vehicle_control_mode_s _vehicle_control_mode{}; - vtol_vehicle_status_s _vtol_vehicle_status{}; + bool _was_armed{false}; // Subscriptions - uORB::Subscription _action_request_sub{ORB_ID(action_request)}; - uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _system_power_sub{ORB_ID(system_power)}; - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; + uORB::Subscription _action_request_sub{ORB_ID(action_request)}; + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _system_power_sub{ORB_ID(system_power)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; #if defined(BOARD_HAS_POWER_CONTROL) - uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; + uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; #endif // BOARD_HAS_POWER_CONTROL - uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; - uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; + + uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; // Publications - uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; - uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; - uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; - uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; - uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; - uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; + uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; + uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; + uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; + uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; orb_advert_t _mavlink_log_pub{nullptr};