From 0e70578052d938ea87c70837a15185b99dbb2309 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 23 Dec 2019 23:38:10 -0500 Subject: [PATCH] commander: move most static variables and parameters to class --- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 21 +- .../PreFlightCheck/checks/preArmCheck.cpp | 10 +- src/modules/commander/Commander.cpp | 1404 +++++++---------- src/modules/commander/Commander.hpp | 285 +++- src/modules/commander/commander_params.c | 5 +- .../state_machine_helper_test.cpp | 44 +- .../commander/state_machine_helper.cpp | 24 +- src/modules/commander/state_machine_helper.h | 7 +- 8 files changed, 811 insertions(+), 989 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index f08e1b8294..f820c658c4 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -78,19 +78,18 @@ public: * true if the system power should be checked **/ static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, - const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot); + vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, + const hrt_abstime &time_since_boot); + + struct arm_requirements_t { + bool arm_authorization = false; + bool esc_check = false; + bool global_position = false; + bool mission = false; + }; static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const safety_s &safety, const uint8_t arm_requirements, const vehicle_status_s &status); - - typedef enum { - ARM_REQ_NONE = 0, - ARM_REQ_MISSION_BIT = (1 << 0), - ARM_REQ_ARM_AUTH_BIT = (1 << 1), - ARM_REQ_GPS_BIT = (1 << 2), - ARM_REQ_ESCS_CHECK_BIT = (1 << 3) - } arm_requirements_t; + const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status); private: static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index a38f87c734..ce74c4b649 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -38,7 +38,7 @@ #include bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const safety_s &safety, const uint8_t arm_requirements, const vehicle_status_s &status) + const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status) { bool prearm_ok = true; @@ -70,7 +70,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } // Arm Requirements: mission - if (arm_requirements & ARM_REQ_MISSION_BIT) { + if (arm_requirements.mission) { if (!status_flags.condition_auto_mission_available) { if (prearm_ok) { @@ -90,7 +90,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } // Arm Requirements: global position - if (arm_requirements & ARM_REQ_GPS_BIT) { + if (arm_requirements.global_position) { if (!status_flags.condition_global_position_valid) { if (prearm_ok) { @@ -120,7 +120,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } - if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) { + if (arm_requirements.esc_check && status_flags.condition_escs_error) { if (prearm_ok) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); prearm_ok = false; @@ -136,7 +136,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st // Arm Requirements: authorization // check last, and only if everything else has passed - if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) { + if (arm_requirements.arm_authorization && prearm_ok) { if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) { // feedback provided in arm_auth_check prearm_ok = false; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 20e151bb10..7ab1f31c32 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -80,21 +80,8 @@ #include #include -#include -#include -#include -#include -#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -108,17 +95,6 @@ typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ } VEHICLE_MODE_FLAG; -/* Decouple update interval and hysteresis counters, all depends on intervals */ -static constexpr uint64_t COMMANDER_MONITORING_INTERVAL = 10_ms; -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) - -static constexpr float STICK_ON_OFF_LIMIT = 0.9f; - -static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms; -static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sensors to come online for upto 8 seconds */ -static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms; -static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms; - /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = nullptr; static orb_advert_t power_button_state_pub = nullptr; @@ -127,77 +103,13 @@ static orb_advert_t power_button_state_pub = nullptr; static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ -static hrt_abstime commander_boot_timestamp = 0; -static unsigned int leds_counter; -/* To remember when last notification was sent */ -static uint64_t last_print_mode_reject_time = 0; - -static float min_stick_change = 0.25f; static struct vehicle_status_s status = {}; static struct actuator_armed_s armed = {}; -static struct safety_s safety = {}; -static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]; -static struct commander_state_s internal_state = {}; - -static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint -static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch -static uint8_t _last_sp_man_arm_switch = 0; - -static struct vtol_vehicle_status_s vtol_status = {}; -static struct cpuload_s cpuload = {}; - -static bool last_overload = false; static struct vehicle_status_flags_s status_flags = {}; -static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost - -static uint8_t arm_requirements = PreFlightCheck::ARM_REQ_NONE; - -static bool _last_condition_local_altitude_valid = false; -static bool _last_condition_local_position_valid = false; -static bool _last_condition_global_position_valid = false; - -static struct vehicle_land_detected_s land_detector = {}; - -static float _eph_threshold_adj = - INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition -static bool _skip_pos_accuracy_check = false; - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - * - * @ingroup apps - */ -extern "C" __EXPORT int commander_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -void usage(const char *reason); - -void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, - const uint8_t battery_warning, const cpuload_s *cpuload_local); - -bool stabilization_required(); - -void print_reject_mode(const char *msg); - -void print_reject_arm(const char *msg); - -void print_status(); - -bool shutdown_if_allowed(); - -transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy); - /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -246,31 +158,38 @@ static int power_button_state_notification_cb(board_power_button_state_notificat return ret; } -static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN) +static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN, float param3 = NAN, + float param4 = NAN, float param5 = NAN, float param6 = NAN, float param7 = NAN) { vehicle_command_s vcmd{}; - vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = param1; vcmd.param2 = param2; - vcmd.param3 = NAN; - vcmd.param4 = NAN; - vcmd.param5 = (double)NAN; - vcmd.param6 = (double)NAN; - vcmd.param7 = NAN; + vcmd.param3 = param3; + vcmd.param4 = param4; + vcmd.param5 = (double)param5; + vcmd.param6 = (double)param6; + vcmd.param7 = param7; + vcmd.command = cmd; - vcmd.target_system = status.system_id; - vcmd.target_component = status.component_id; + + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = vehicle_status_sub.get().system_id; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = vehicle_status_sub.get().component_id; + + vcmd.timestamp = hrt_absolute_time(); uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; return vcmd_pub.publish(vcmd); } -int commander_main(int argc, char *argv[]) +extern "C" __EXPORT int commander_main(int argc, char *argv[]) { if (argc < 2) { - usage("missing command"); + Commander::print_usage("missing command"); return 1; } @@ -325,169 +244,153 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - print_status(); + PX4_INFO("arming: %s", arming_state_names[status.arming_state]); return 0; } if (!strcmp(argv[1], "calibrate")) { if (argc > 2) { - int calib_ret = OK; + if (!strcmp(argv[2], "gyro")) { + // gyro calibration: param1 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f); - if (!strcmp(argv[2], "mag")) { - calib_ret = do_mag_calibration(&mavlink_log_pub); + } else if (!strcmp(argv[2], "mag")) { + // magnetometer calibration: param2 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f); } else if (!strcmp(argv[2], "accel")) { - calib_ret = do_accel_calibration(&mavlink_log_pub); - - } else if (!strcmp(argv[2], "gyro")) { - calib_ret = do_gyro_calibration(&mavlink_log_pub); + // accelerometer calibration: param5 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f); } else if (!strcmp(argv[2], "level")) { - calib_ret = do_level_calibration(&mavlink_log_pub); - - } else if (!strcmp(argv[2], "esc")) { - calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); + // board level calibration: param5 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f); } else if (!strcmp(argv[2], "airspeed")) { - calib_ret = do_airspeed_calibration(&mavlink_log_pub); + // airspeed calibration: param6 = 2 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f); + + } else if (!strcmp(argv[2], "esc")) { + // ESC calibration: param7 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 1.f); } else { PX4_ERR("argument %s unsupported.", argv[2]); - } - - if (calib_ret) { - PX4_ERR("calibration failed, exiting."); return 1; - - } else { - return 0; } + return 0; + } else { PX4_ERR("missing argument"); } } if (!strcmp(argv[1], "check")) { - bool preflight_check_res = Commander::preflight_check(true); + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, status, status_flags, true, true, false, 30_s); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - bool prearm_check_res = PreFlightCheck::preArmCheck(&mavlink_log_pub, status_flags, safety, arm_requirements, status); + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, status_flags, safety_s{}, + PreFlightCheck::arm_requirements_t{}, status); PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); return 0; } if (!strcmp(argv[1], "arm")) { - bool force_arming = false; + float param2 = 0.f; + // 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight) if (argc > 2 && !strcmp(argv[2], "-f")) { - force_arming = true; + param2 = 21196.f; } - if (TRANSITION_CHANGED != arm_disarm(true, !force_arming, &mavlink_log_pub, "command line")) { - PX4_ERR("arming failed"); - } + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, param2); return 0; } if (!strcmp(argv[1], "disarm")) { - if (TRANSITION_DENIED == arm_disarm(false, true, &mavlink_log_pub, "command line")) { - PX4_ERR("rejected disarm"); - } + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.f, 0.f); return 0; } if (!strcmp(argv[1], "takeoff")) { + // switch to takeoff mode and arm + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, 0.f); - bool ret = false; - - /* see if we got a home position */ - if (status_flags.condition_local_position_valid) { - - if (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "command line")) { - ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); - - } else { - PX4_ERR("arming failed"); - } - - } else { - PX4_ERR("rejecting takeoff, no position lock yet. Please retry.."); - } - - return (ret ? 0 : 1); + return 0; } if (!strcmp(argv[1], "land")) { - bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND); - return (ret ? 0 : 1); + return 0; } if (!strcmp(argv[1], "transition")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, + (float)(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC)); - const bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, - (float)(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? - vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : - vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC)); - - return (ret ? 0 : 1); + return 0; } if (!strcmp(argv[1], "mode")) { if (argc > 2) { - uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX; if (!strcmp(argv[2], "manual")) { - new_main_state = commander_state_s::MAIN_STATE_MANUAL; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL); } else if (!strcmp(argv[2], "altctl")) { - new_main_state = commander_state_s::MAIN_STATE_ALTCTL; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTCTL); } else if (!strcmp(argv[2], "posctl")) { - new_main_state = commander_state_s::MAIN_STATE_POSCTL; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL); } else if (!strcmp(argv[2], "auto:mission")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION); } else if (!strcmp(argv[2], "auto:loiter")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER); } else if (!strcmp(argv[2], "auto:rtl")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_RTL); } else if (!strcmp(argv[2], "acro")) { - new_main_state = commander_state_s::MAIN_STATE_ACRO; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO); } else if (!strcmp(argv[2], "offboard")) { - new_main_state = commander_state_s::MAIN_STATE_OFFBOARD; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD); } else if (!strcmp(argv[2], "stabilized")) { - new_main_state = commander_state_s::MAIN_STATE_STAB; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED); } else if (!strcmp(argv[2], "rattitude")) { - new_main_state = commander_state_s::MAIN_STATE_RATTITUDE; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_RATTITUDE); } else if (!strcmp(argv[2], "auto:takeoff")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF); } else if (!strcmp(argv[2], "auto:land")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_LAND); } else if (!strcmp(argv[2], "auto:precland")) { - new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND); } else { PX4_ERR("argument %s unsupported.", argv[2]); } - if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) { - PX4_ERR("mode change failed"); - } - return 0; } else { @@ -498,7 +401,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "lockdown")) { if (argc < 3) { - usage("not enough arguments, missing [on, off]"); + Commander::print_usage("not enough arguments, missing [on, off]"); return 1; } @@ -508,38 +411,33 @@ int commander_main(int argc, char *argv[]) return (ret ? 0 : 1); } - usage("unrecognized command"); + Commander::print_usage("unrecognized command"); return 1; } -void print_status() +bool Commander::shutdown_if_allowed() { - PX4_INFO("arming: %s", arming_state_names[status.arming_state]); + 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)); } -bool 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(&commander_boot_timestamp)); -} - -transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, - const char *armedBy) +transition_result_t +Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, - safety, + _safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, run_preflight_checks, mavlink_log_pub_local, &status_flags, - arm_requirements, - hrt_elapsed_time(&commander_boot_timestamp)); + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp)); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -557,9 +455,13 @@ Commander::Commander() : { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _land_detector.landed = true; + + // XXX for now just set sensors as initialized + status_flags.condition_system_sensors_initialized = true; + // We want to accept RC inputs as default status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; - internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.nav_state_timestamp = hrt_absolute_time(); status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -572,12 +474,13 @@ Commander::Commander() : status_flags.condition_power_input_valid = true; status_flags.rc_calibration_valid = true; - status_flags.avoidance_system_valid = false; + // default for vtol is rotary wing + _vtol_status.vtol_in_rw_mode = true; } bool Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, - uORB::PublicationQueued &command_ack_pub, bool *changed) + uORB::PublicationQueued &command_ack_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) @@ -600,7 +503,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // Check if a mode switch had been requested if ((((uint32_t)cmd.param2) & 1) > 0) { transition_result_t main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, - status_flags, &internal_state); + status_flags, &_internal_state); if ((main_ret != TRANSITION_DENIED)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -633,32 +536,32 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - reset_posvel_validity(changed); - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); + reset_posvel_validity(&_status_changed); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ if (custom_sub_mode > 0) { - reset_posvel_validity(changed); + reset_posvel_validity(&_status_changed); switch (custom_sub_mode) { case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, - &internal_state); + &_internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: if (status_flags.condition_auto_mission_available) { main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, - &internal_state); + &_internal_state); } else { main_ret = TRANSITION_DENIED; @@ -667,26 +570,27 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags, - &internal_state); + &_internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_LAND: - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, + &_internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags, - &internal_state); + &_internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags, - &internal_state); + &_internal_state); break; default: @@ -697,25 +601,27 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } else { main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, - &internal_state); + &_internal_state); } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { /* RATTITUDE */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, + &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { /* STABILIZED */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { - reset_posvel_validity(changed); + reset_posvel_validity(&_status_changed); + /* OFFBOARD */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state); } } else { @@ -723,20 +629,20 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, - &internal_state); + &_internal_state); } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { /* STABILIZED */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); } else { /* MANUAL */ - main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); } } } @@ -769,7 +675,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ const bool enforce = (static_cast(roundf(cmd.param2)) == 21196); if (!enforce) { - if (!land_detector.landed && !is_ground_rover(&status)) { + if (!_land_detector.landed && !is_ground_rover(&status)) { if (cmd_arms) { if (armed_local->armed) { mavlink_log_warning(&mavlink_log_pub, "Arming denied! Already armed"); @@ -799,8 +705,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ break; } - const bool throttle_above_low = (sp_man.z > 0.1f); - const bool throttle_above_center = (sp_man.z > 0.6f); + const bool throttle_above_low = (_sp_man.z > 0.1f); + const bool throttle_above_center = (_sp_man.z > 0.6f); if (cmd_arms && throttle_above_center && (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || @@ -832,7 +738,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arms && (arming_res == TRANSITION_CHANGED) && - (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { + (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { set_home_position(); } @@ -923,14 +829,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = commander_state_s::MAIN_STATE_MANUAL; - if (internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) { - main_state_pre_offboard = internal_state.main_state; + if (_internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) { + _main_state_pre_offboard = _internal_state.main_state; } if (cmd.param1 > 0.5f) { - res = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state); + res = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state); if (res == TRANSITION_DENIED) { print_reject_mode("OFFBOARD"); @@ -944,7 +849,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ - res = main_state_transition(*status_local, main_state_pre_offboard, status_flags, &internal_state); + res = main_state_transition(*status_local, _main_state_pre_offboard, status_flags, &_internal_state); status_flags.offboard_control_set_by_command = false; } @@ -960,7 +865,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { /* switch to RTL which ends the mission */ if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, - &internal_state)) { + &_internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -974,10 +879,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags, - &internal_state)) { + &_internal_state)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - } else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { + } else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -989,7 +894,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ 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, - &internal_state)) { + &_internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1002,7 +907,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_NAV_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_and_console_log_info(&mavlink_log_pub, "Precision landing"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1025,7 +930,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // switch to AUTO_MISSION and ARM 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, "Mission start command"))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1044,7 +949,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { // if no high latency telemetry exists send a failed acknowledge - if (_high_latency_datalink_heartbeat > commander_boot_timestamp) { + 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"); } @@ -1055,7 +960,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // Switch to orbit state and let the orbit task handle the command further if (TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags, - &internal_state)) { + &_internal_state)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1068,13 +973,60 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ cmd_result = handle_command_motor_test(cmd); break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + + // do nothing for autopilot + if (((int)(cmd.param1)) == 0) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + + break; + } + + if (shutdown_if_allowed()) { + bool shutdown_ret_val = PX4_ERROR; + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + px4_usleep(200000); + // reboot + shutdown_ret_val = px4_shutdown_request(true, false); + + } else if (((int)(cmd.param1)) == 2) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + px4_usleep(200000); + // shutdown + shutdown_ret_val = px4_shutdown_request(false, false); + + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + px4_usleep(200000); + // reboot to bootloader + shutdown_ret_val = px4_shutdown_request(true, true); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); + break; + } + + if (shutdown_ret_val) { + mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); + + } else { + while (1) { px4_usleep(1); } + } + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); + } + + break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: @@ -1120,7 +1072,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ unsigned Commander::handle_command_motor_test(const vehicle_command_s &cmd) { - if (armed.armed || (safety.safety_switch_available && !safety.safety_off)) { + if (armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1239,49 +1191,10 @@ void Commander::run() { bool sensor_fail_tune_played = false; - bool arm_tune_played = false; - bool was_landed = true; - bool was_falling = false; - bool was_armed = false; - - // XXX for now just set sensors as initialized - status_flags.condition_system_sensors_initialized = true; - - /* set parameters */ - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); - param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); - param_t _param_ef_time_thres = param_find("COM_EF_TIME"); - param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); - param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST"); - param_t _param_min_stick_change = param_find("COM_RC_STICK_OV"); - param_t _param_geofence_action = param_find("GF_ACTION"); - param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS"); - param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN"); - param_t _param_rc_override = param_find("COM_RC_OVERRIDE"); - param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ"); - param_t _param_arm_auth_required = param_find("COM_ARM_AUTH_REQ"); - param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS"); - param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID"); - param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT"); - - param_t _param_fmode_1 = param_find("COM_FLTMODE1"); - param_t _param_fmode_2 = param_find("COM_FLTMODE2"); - param_t _param_fmode_3 = param_find("COM_FLTMODE3"); - param_t _param_fmode_4 = param_find("COM_FLTMODE4"); - param_t _param_fmode_5 = param_find("COM_FLTMODE5"); - param_t _param_fmode_6 = param_find("COM_FLTMODE6"); param_t _param_airmode = param_find("MC_AIRMODE"); param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW"); - status_flags.avoidance_system_required = _param_com_obs_avoid.get(); - - /* pthread for slow low prio thread */ - pthread_t commander_low_prio_thread; - /* initialize */ if (led_init() != OK) { PX4_WARN("LED init failed"); @@ -1291,15 +1204,15 @@ Commander::run() PX4_WARN("Buzzer init failed"); } - uORB::Subscription power_button_state_sub{ORB_ID(power_button_state)}; { // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen // in IRQ context. power_button_state_s button_state{}; + button_state.timestamp = hrt_absolute_time(); button_state.event = 0xff; power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state); - power_button_state_sub.copy(&button_state); + _power_button_state_sub.copy(&button_state); } if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) { @@ -1309,49 +1222,20 @@ Commander::run() get_circuit_breaker_params(); - /* armed topic */ - hrt_abstime last_disarmed_timestamp = 0; - /* command ack */ - uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_init(); - /* Start monitoring loop */ - unsigned counter = 0; - int stick_off_counter = 0; - int stick_on_counter = 0; - - bool status_changed = true; bool param_init_forced = true; - uORB::Subscription actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; - uORB::Subscription cmd_sub{ORB_ID(vehicle_command)}; - uORB::Subscription cpuload_sub{ORB_ID(cpuload)}; - uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)}; - uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription safety_sub{ORB_ID(safety)}; - uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription subsys_sub{ORB_ID(subsystem_info)}; - uORB::Subscription system_power_sub{ORB_ID(system_power)}; - uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; - uORB::Subscription esc_status_sub{ORB_ID(esc_status)}; - - geofence_result_s geofence_result {}; - - land_detector.landed = true; - - vtol_status.vtol_in_rw_mode = true; // default for vtol is rotary wing - - control_status_leds(&status, &armed, true, _battery_warning, &cpuload); + control_status_leds(&status, &armed, true, _battery_warning); thread_running = true; /* update vehicle status to find out vehicle type (required for preflight checks) */ - int32_t system_type; - param_get(_param_sys_type, &system_type); // get system type - status.system_type = (uint8_t)system_type; + status.system_type = _param_mav_type.get(); if (is_rotary_wing(&status) || is_vtol(&status)) { status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -1369,68 +1253,19 @@ Commander::run() status.is_vtol = is_vtol(&status); status.is_vtol_tailsitter = is_vtol_tailsitter(&status); - commander_boot_timestamp = hrt_absolute_time(); + _boot_timestamp = hrt_absolute_time(); // initially set to failed - _last_lpos_fail_time_us = commander_boot_timestamp; - _last_gpos_fail_time_us = commander_boot_timestamp; - _last_lvel_fail_time_us = commander_boot_timestamp; - - // Run preflight check - int32_t rc_in_off = 0; - - param_get(_param_rc_in_off, &rc_in_off); - - int32_t arm_switch_is_button = 0; - param_get(_param_arm_switch_is_button, &arm_switch_is_button); - - int32_t arm_without_gps_param = 0; - param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT; - - int32_t arm_mission_required_param = 0; - param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_requirements |= (arm_mission_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_MISSION_BIT; - - int32_t arm_auth_required_param = 0; - param_get(_param_arm_auth_required, &arm_auth_required_param); - arm_requirements |= (arm_auth_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_ARM_AUTH_BIT; - - int32_t arm_escs_checks_required_param = 0; - param_get(_param_escs_checks_required, &arm_escs_checks_required_param); - arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT; - - status.rc_input_mode = rc_in_off; + _last_lpos_fail_time_us = _boot_timestamp; + _last_gpos_fail_time_us = _boot_timestamp; + _last_lvel_fail_time_us = _boot_timestamp; // user adjustable duration required to assert arm/disarm via throttle/rudder stick - int32_t rc_arm_hyst = 100; - param_get(_param_rc_arm_hyst, &rc_arm_hyst); - rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; + uint32_t rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC; - int32_t geofence_action = 0; - int32_t flight_uuid = 0; int32_t airmode = 0; int32_t rc_map_arm_switch = 0; - /* RC override auto modes */ - int32_t rc_override = 0; - - int32_t takeoff_complete_act = 0; - - /* Thresholds for engine failure detection */ - float ef_throttle_thres = 1.0f; - float ef_current2throttle_thres = 0.0f; - float ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ - - /* check which state machines for changes, clear "changed" flag */ - bool failsafe_old = false; - - bool have_taken_off_since_arming = false; - /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -1439,20 +1274,23 @@ Commander::run() #ifndef __PX4_QURT // This is not supported by QURT (yet). struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); #endif - + pthread_t commander_low_prio_thread; pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr); pthread_attr_destroy(&commander_low_prio_attr); + + status.system_id = _param_mav_sys_id.get(); arm_auth_init(&mavlink_log_pub, &status.system_id); // run preflight immediately to find all relevant parameters, but don't report - preflight_check(false); + PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, _arm_requirements.global_position, false, false, + hrt_elapsed_time(&_boot_timestamp)); while (!should_exit()) { @@ -1471,15 +1309,10 @@ Commander::run() /* update parameters */ if (!armed.armed) { - if (param_get(_param_sys_type, &system_type) != OK) { - PX4_ERR("failed getting new system type"); + status.system_type = _param_mav_type.get(); - } else { - status.system_type = (uint8_t)system_type; - } - - const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode); - const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !vtol_status.vtol_in_rw_mode); + const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && _vtol_status.vtol_in_rw_mode); + const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !_vtol_status.vtol_in_rw_mode); const bool is_ground = is_ground_rover(&status); /* disable manual override for all systems that rely on electronic stabilization */ @@ -1498,61 +1331,35 @@ Commander::run() status.is_vtol_tailsitter = is_vtol_tailsitter(&status); /* check and update system / component ID */ - int32_t sys_id = 0; - param_get(_param_system_id, &sys_id); - status.system_id = sys_id; - - int32_t comp_id = 0; - param_get(_param_component_id, &comp_id); - status.component_id = comp_id; + status.system_id = _param_mav_sys_id.get(); + status.component_id = _param_mav_comp_id.get(); get_circuit_breaker_params(); - status_changed = true; + _status_changed = true; } - /* Safety parameters */ - param_get(_param_rc_in_off, &rc_in_off); - status.rc_input_mode = rc_in_off; - param_get(_param_rc_arm_hyst, &rc_arm_hyst); - param_get(_param_min_stick_change, &min_stick_change); - param_get(_param_rc_override, &rc_override); + status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + + status.rc_input_mode = _param_rc_in_off.get(); + // percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1 - min_stick_change *= 0.02f; - rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; - param_get(_param_ef_throttle_thres, &ef_throttle_thres); - param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); - param_get(_param_ef_time_thres, &ef_time_thres); - param_get(_param_geofence_action, &geofence_action); - param_get(_param_flight_uuid, &flight_uuid); + _min_stick_change = _param_min_stick_change.get() * 0.02f; - param_get(_param_arm_switch_is_button, &arm_switch_is_button); - - param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT; - - param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_requirements |= (arm_mission_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_MISSION_BIT; - - param_get(_param_arm_auth_required, &arm_auth_required_param); - arm_requirements |= (arm_auth_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_ARM_AUTH_BIT; - - param_get(_param_escs_checks_required, &arm_escs_checks_required_param); - arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : - PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT; + rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC; + _arm_requirements.arm_authorization = _param_arm_auth_required.get(); + _arm_requirements.esc_check = _param_escs_checks_required.get(); + _arm_requirements.global_position = !_param_arm_without_gps.get(); + _arm_requirements.mission = _param_arm_mission_required.get(); /* flight mode slots */ - param_get(_param_fmode_1, &_flight_mode_slots[0]); - param_get(_param_fmode_2, &_flight_mode_slots[1]); - param_get(_param_fmode_3, &_flight_mode_slots[2]); - param_get(_param_fmode_4, &_flight_mode_slots[3]); - param_get(_param_fmode_5, &_flight_mode_slots[4]); - param_get(_param_fmode_6, &_flight_mode_slots[5]); - - param_get(_param_takeoff_finished_action, &takeoff_complete_act); + _flight_mode_slots[0] = _param_fltmode_1.get(); + _flight_mode_slots[1] = _param_fltmode_2.get(); + _flight_mode_slots[2] = _param_fltmode_3.get(); + _flight_mode_slots[3] = _param_fltmode_4.get(); + _flight_mode_slots[4] = _param_fltmode_5.get(); + _flight_mode_slots[5] = _param_fltmode_6.get(); _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); @@ -1575,22 +1382,23 @@ Commander::run() status_flags.avoidance_system_required = _param_com_obs_avoid.get(); /* handle power button state */ - if (power_button_state_sub.updated()) { + if (_power_button_state_sub.updated()) { power_button_state_s button_state; - power_button_state_sub.copy(&button_state); - if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { - px4_shutdown_request(false, false); + if (_power_button_state_sub.copy(&button_state)) { + if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { + px4_shutdown_request(false, false); + } } } - sp_man_sub.update(&sp_man); + _sp_man_sub.update(&_sp_man); - offboard_control_update(status_changed); + offboard_control_update(); - if (system_power_sub.updated()) { - system_power_s system_power = {}; - system_power_sub.copy(&system_power); + if (_system_power_sub.updated()) { + system_power_s system_power{}; + _system_power_sub.copy(&system_power); if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { if (system_power.servo_valid && @@ -1617,106 +1425,107 @@ Commander::run() } /* update safety topic */ - if (safety_sub.updated()) { - bool previous_safety_off = safety.safety_off; + if (_safety_sub.updated()) { + const bool previous_safety_off = _safety.safety_off; - if (safety_sub.copy(&safety)) { + if (_safety_sub.copy(&_safety)) { /* disarm if safety is now on and still armed */ if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF) - && safety.safety_switch_available && !safety.safety_off) { + && _safety.safety_switch_available && !_safety.safety_off) { - if (TRANSITION_CHANGED == arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, + if (TRANSITION_CHANGED == arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) + &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp)) ) { - status_changed = true; + _status_changed = true; } } // Notify the user if the status of the safety switch changes - if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { + if (_safety.safety_switch_available && previous_safety_off != _safety.safety_off) { - if (safety.safety_off) { + if (_safety.safety_off) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); } else { tune_neutral(true); } - status_changed = true; + _status_changed = true; } } } /* update vtol vehicle status*/ - if (vtol_vehicle_status_sub.updated()) { + if (_vtol_vehicle_status_sub.updated()) { /* vtol status changed */ - vtol_vehicle_status_sub.copy(&vtol_status); - status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; + _vtol_vehicle_status_sub.copy(&_vtol_status); + status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(&status)) { // Check if there has been any change while updating the flags - const auto new_vehicle_type = vtol_status.vtol_in_rw_mode ? + const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ? vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING; if (new_vehicle_type != status.vehicle_type) { - status.vehicle_type = vtol_status.vtol_in_rw_mode ? + status.vehicle_type = _vtol_status.vtol_in_rw_mode ? vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - status_changed = true; + _status_changed = true; } - if (status.in_transition_mode != vtol_status.vtol_in_trans_mode) { - status.in_transition_mode = vtol_status.vtol_in_trans_mode; - status_changed = true; + if (status.in_transition_mode != _vtol_status.vtol_in_trans_mode) { + status.in_transition_mode = _vtol_status.vtol_in_trans_mode; + _status_changed = true; } - if (status.in_transition_to_fw != vtol_status.in_transition_to_fw) { - status.in_transition_to_fw = vtol_status.in_transition_to_fw; - status_changed = true; + if (status.in_transition_to_fw != _vtol_status.in_transition_to_fw) { + status.in_transition_to_fw = _vtol_status.in_transition_to_fw; + _status_changed = true; } - if (status_flags.vtol_transition_failure != vtol_status.vtol_transition_failsafe) { - status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe; - status_changed = true; + if (status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) { + status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe; + _status_changed = true; } const bool should_soft_stop = (status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); if (armed.soft_stop != should_soft_stop) { armed.soft_stop = should_soft_stop; - status_changed = true; + _status_changed = true; } } } - if (esc_status_sub.updated()) { + if (_esc_status_sub.updated()) { /* ESCs status changed */ - esc_status_s esc_status = {}; + esc_status_s esc_status{}; - esc_status_sub.copy(&esc_status); - esc_status_check(esc_status); + if (_esc_status_sub.copy(&esc_status)) { + esc_status_check(esc_status); + } } - estimator_check(&status_changed); + estimator_check(); /* Update land detector */ - if (land_detector_sub.updated()) { - land_detector_sub.copy(&land_detector); + if (_land_detector_sub.updated()) { + _land_detector_sub.copy(&_land_detector); // Only take actions if armed if (armed.armed) { - if (was_landed != land_detector.landed) { - if (land_detector.landed) { + if (_was_landed != _land_detector.landed) { + if (_land_detector.landed) { mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected"); } else { mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); - have_taken_off_since_arming = true; + _have_taken_off_since_arming = true; // Set all position and velocity test probation durations to takeoff value // This is a larger value to give the vehicle time to complete a failsafe landing @@ -1727,15 +1536,15 @@ Commander::run() } } - if (was_falling != land_detector.freefall) { - if (land_detector.freefall) { + if (_was_falling != _land_detector.freefall) { + if (_land_detector.freefall) { mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected"); } } } - was_landed = land_detector.landed; - was_falling = land_detector.freefall; + _was_landed = _land_detector.landed; + _was_falling = _land_detector.freefall; } @@ -1745,11 +1554,11 @@ Commander::run() // Check for auto-disarm on landing or pre-flight if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { - if (_param_com_disarm_land.get() > 0 && have_taken_off_since_arming) { + if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming) { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); - _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); + _auto_disarm_landed.set_state_and_update(_land_detector.landed, hrt_absolute_time()); - } else if (_param_com_disarm_preflight.get() > 0 && !have_taken_off_since_arming) { + } else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } @@ -1772,31 +1581,32 @@ Commander::run() } if (_geofence_warning_action_on - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + // reset flag again when we switched out of it _geofence_warning_action_on = false; } - cpuload_sub.update(&cpuload); + _cpuload_sub.update(&_cpuload); battery_status_check(); /* update subsystem info which arrives from outside of commander*/ subsystem_info_s info; - while (subsys_sub.update(&info)) { + while (_subsys_sub.update(&info)) { set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status); - status_changed = true; + _status_changed = true; } /* If in INIT state, try to proceed to STANDBY state */ 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, - arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); if (arming_ret == TRANSITION_DENIED) { /* do not complain if not allowed into standby */ @@ -1811,7 +1621,7 @@ Commander::run() const mission_result_s &mission_result = _mission_result_sub.get(); // if mission_result is valid for the current mission - const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp) + const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) && (mission_result.instance_count > 0); status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; @@ -1820,7 +1630,7 @@ Commander::run() if (status.mission_failure != mission_result.failure) { status.mission_failure = mission_result.failure; - status_changed = true; + _status_changed = true; if (status.mission_failure) { mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed"); @@ -1848,10 +1658,10 @@ Commander::run() } /* start geofence result check */ - geofence_result_sub.update(&geofence_result); + _geofence_result_sub.update(&_geofence_result); // Geofence actions - const bool geofence_action_enabled = geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE; + const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE; const bool not_in_battery_failsafe = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL; if (armed.armed @@ -1859,9 +1669,9 @@ Commander::run() && not_in_battery_failsafe) { // check for geofence violation transition - if (geofence_result.geofence_violated && !_geofence_violated_prev) { + if (_geofence_result.geofence_violated && !_geofence_violated_prev) { - switch (geofence_result.geofence_action) { + switch (_geofence_result.geofence_action) { case (geofence_result_s::GF_ACTION_NONE) : { // do nothing break; @@ -1874,7 +1684,7 @@ Commander::run() case (geofence_result_s::GF_ACTION_LOITER) : { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, - &internal_state)) { + &_internal_state)) { _geofence_loiter_on = true; } @@ -1883,7 +1693,7 @@ Commander::run() case (geofence_result_s::GF_ACTION_RTL) : { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, - &internal_state)) { + &_internal_state)) { _geofence_rtl_on = true; } @@ -1894,17 +1704,17 @@ Commander::run() warnx("Flight termination because of geofence"); mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated"); armed.force_failsafe = true; - status_changed = true; + _status_changed = true; break; } } } - _geofence_violated_prev = geofence_result.geofence_violated; + _geofence_violated_prev = _geofence_result.geofence_violated; // reset if no longer in LOITER or if manually switched to LOITER - const bool in_loiter_mode = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; - const bool manual_loiter_switch_on = sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON; + const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; + const bool manual_loiter_switch_on = _sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON; if (!in_loiter_mode || manual_loiter_switch_on) { _geofence_loiter_on = false; @@ -1912,8 +1722,8 @@ Commander::run() // reset if no longer in RTL or if manually switched to RTL - const bool in_rtl_mode = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; - const bool manual_return_switch_on = sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON; + const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; + const bool manual_return_switch_on = _sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON; if (!in_rtl_mode || manual_return_switch_on) { _geofence_rtl_on = false; @@ -1934,21 +1744,23 @@ Commander::run() const bool low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL; const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; const bool in_auto_mode = - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; + _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || + _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; + + if (_param_rc_override.get() && is_rotary_wing && !low_battery_reaction + && !_geofence_warning_action_on && in_auto_mode) { - if (rc_override != 0 && is_rotary_wing && !low_battery_reaction && !_geofence_warning_action_on && in_auto_mode) { // transition to previous state if sticks are touched - if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { + if ((_last_sp_man.timestamp != _sp_man.timestamp) && + ((fabsf(_sp_man.x - _last_sp_man.x) > _min_stick_change) || + (fabsf(_sp_man.y - _last_sp_man.y) > _min_stick_change) || + (fabsf(_sp_man.z - _last_sp_man.z) > _min_stick_change) || + (fabsf(_sp_man.r - _last_sp_man.r) > _min_stick_change))) { // 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_critical(&mavlink_log_pub, "Autonomy off! Returned control to pilot"); } } @@ -1958,122 +1770,123 @@ Commander::run() !status_flags.circuit_breaker_flight_termination_disabled) { armed.force_failsafe = true; - status_changed = true; - static bool flight_termination_printed = false; + _status_changed = true; - if (!flight_termination_printed) { + if (!_flight_termination_printed) { 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"); } } /* RC input check */ - if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 && - (hrt_elapsed_time(&sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) { + if (!status_flags.rc_input_blocked && _sp_man.timestamp != 0 && + (hrt_elapsed_time(&_sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) { /* handle the case where RC signal was regained */ if (!status_flags.rc_signal_found_once) { status_flags.rc_signal_found_once = true; set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status); - status_changed = true; + _status_changed = true; } else { if (status.rc_signal_lost) { mavlink_log_info(&mavlink_log_pub, "Manual control regained after %llums", - hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000); + hrt_elapsed_time(&_rc_signal_lost_timestamp) / 1000); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status); - status_changed = true; + _status_changed = true; } } status.rc_signal_lost = false; const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool arm_switch_or_button_mapped = sp_man.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE; - const bool arm_button_pressed = arm_switch_is_button == 1 - && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON; + const bool arm_switch_or_button_mapped = _sp_man.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE; + const bool arm_button_pressed = _param_arm_switch_is_button.get() + && (_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON); /* DISARM * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) * do it only for rotary wings in manual mode or fixed wing if landed. * Disable stick-disarming if arming switch or button is mapped */ - const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f - && !arm_switch_or_button_mapped; - const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 && - _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && - sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; + const bool stick_in_lower_left = _sp_man.r < -STICK_ON_OFF_LIMIT && (_sp_man.z < 0.1f) && !arm_switch_or_button_mapped; + const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() && + (_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) && + (_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF); if (in_armed_state && - status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || land_detector.landed) && + (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) && + (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { - const bool manual_thrust_mode = internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL - || internal_state.main_state == commander_state_s::MAIN_STATE_ACRO - || internal_state.main_state == commander_state_s::MAIN_STATE_STAB - || internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE; - const bool rc_wants_disarm = (stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) + const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL + || _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO + || _internal_state.main_state == commander_state_s::MAIN_STATE_STAB + || _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE; + const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition; - if (rc_wants_disarm && (land_detector.landed || manual_thrust_mode)) { - arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, + if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { + arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); } - stick_off_counter++; + _stick_off_counter++; - } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { + } else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { /* do not reset the counter when holding the arm button longer than needed */ - stick_off_counter = 0; + _stick_off_counter = 0; } /* ARM * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm * and we're in MANUAL mode. * Disable stick-arming if arming switch or button is mapped */ - const bool stick_in_lower_right = sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f + const bool stick_in_lower_right = _sp_man.r > STICK_ON_OFF_LIMIT && _sp_man.z < 0.1f && !arm_switch_or_button_mapped; /* allow a grace period for re-arming: preflight checks don't need to pass during that time, * for example for accidential in-air disarming */ - const bool in_arming_grace_period = last_disarmed_timestamp != 0 && hrt_elapsed_time(&last_disarmed_timestamp) < 5_s; - const bool arm_switch_to_arm_transition = arm_switch_is_button == 0 && - _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF && - sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && - (sp_man.z < 0.1f || in_arming_grace_period); + const bool in_arming_grace_period = (_last_disarmed_timestamp != 0) + && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s); + + const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() && + (_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) && + (_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) && + (_sp_man.z < 0.1f || in_arming_grace_period); if (!in_armed_state && - status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && + (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) && (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) { - if ((stick_on_counter == rc_arm_hyst && stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) { + + if ((_stick_on_counter == rc_arm_hyst && _stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) { /* we check outside of the transition function here because the requirement * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - - if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) - && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB) - && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) + if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) + && (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) + && (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB) + && (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) + && (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) + && (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) ) { print_reject_arm("Not arming: Switch to a manual mode first"); } else if (!status_flags.condition_home_position_valid && - geofence_action == geofence_result_s::GF_ACTION_RTL) { + (_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) { + print_reject_arm("Not arming: Geofence RTL requires valid home"); } 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_arming_grace_period /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); if (arming_ret != TRANSITION_CHANGED) { px4_usleep(100000); @@ -2082,14 +1895,14 @@ Commander::run() } } - stick_on_counter++; + _stick_on_counter++; - } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { + } else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { /* do not reset the counter when holding the arm button longer than needed */ - stick_on_counter = 0; + _stick_on_counter = 0; } - _last_sp_man_arm_switch = sp_man.arm_switch; + _last_sp_man_arm_switch = _sp_man.arm_switch; if (arming_ret == TRANSITION_DENIED) { /* @@ -2102,8 +1915,8 @@ Commander::run() } /* evaluate the main state machine according to mode switches */ - bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0); - transition_result_t main_res = set_main_state(status, &status_changed); + bool first_rc_eval = (_last_sp_man.timestamp == 0) && (_sp_man.timestamp > 0); + transition_result_t main_res = set_main_state(status, &_status_changed); /* store last position lock state */ _last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid; @@ -2113,7 +1926,7 @@ Commander::run() /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED || first_rc_eval) { tune_positive(armed.armed); - status_changed = true; + _status_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -2121,18 +1934,18 @@ Commander::run() } /* check throttle kill switch */ - if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* set lockdown flag */ if (!armed.manual_lockdown) { mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged"); - status_changed = true; + _status_changed = true; armed.manual_lockdown = true; } - } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + } else if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { if (armed.manual_lockdown) { mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged"); - status_changed = true; + _status_changed = true; armed.manual_lockdown = false; } } @@ -2143,41 +1956,41 @@ Commander::run() if (!status_flags.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(&mavlink_log_pub, "Manual control lost"); status.rc_signal_lost = true; - rc_signal_lost_timestamp = sp_man.timestamp; + _rc_signal_lost_timestamp = _sp_man.timestamp; set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status); - status_changed = true; + _status_changed = true; } } // data link checks which update the status - data_link_check(status_changed); + data_link_check(); // engine failure detection // TODO: move out of commander - if (actuator_controls_sub.updated()) { + if (_actuator_controls_sub.updated()) { /* Check engine failure * only for fixed wing for now */ if (!status_flags.circuit_breaker_engaged_enginefailure_check && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol && armed.armed) { - actuator_controls_s actuator_controls = {}; - actuator_controls_sub.copy(&actuator_controls); + actuator_controls_s actuator_controls{}; + _actuator_controls_sub.copy(&actuator_controls); const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; const float current2throttle = _battery_current / throttle; - if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres)) + if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get())) || status.engine_failure) { - const float elapsed = hrt_elapsed_time(×tamp_engine_healthy) / 1e6f; + const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f; /* potential failure, measure time */ - if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres) + if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get()) && !status.engine_failure) { status.engine_failure = true; - status_changed = true; + _status_changed = true; PX4_ERR("Engine Failure"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, status); @@ -2186,11 +1999,11 @@ Commander::run() } else { /* no failure reset flag */ - timestamp_engine_healthy = hrt_absolute_time(); + _timestamp_engine_healthy = hrt_absolute_time(); if (status.engine_failure) { status.engine_failure = false; - status_changed = true; + _status_changed = true; } } } @@ -2199,45 +2012,43 @@ Commander::run() * Sometimes, the mission result topic is outdated and the mission is still signaled * as finished even though we only just started with the takeoff. Therefore, we also * check the timestamp of the mission_result topic. */ - if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - && (_mission_result_sub.get().timestamp > internal_state.timestamp) + if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF + && (_mission_result_sub.get().timestamp > _internal_state.timestamp) && _mission_result_sub.get().finished) { - const bool mission_available = (_mission_result_sub.get().timestamp > commander_boot_timestamp) + const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp) && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; - if ((takeoff_complete_act == 1) && mission_available) { - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); + if ((_param_takeoff_finished_action.get() == 1) && mission_available) { + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &_internal_state); } else { - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state); } } /* check if we are disarmed and there is a better mode to wait in */ if (!armed.armed) { - /* if there is no radio control but GPS lock the user might want to fly using * just a tablet. Since the RC will force its mode switch setting on connecting * we can as well just wait in a hold mode which enables tablet control. */ - if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) + if (status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) && status_flags.condition_home_position_valid) { - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state); } } /* handle commands last, as the system needs to be updated to handle them */ - if (cmd_sub.updated()) { - vehicle_command_s cmd{}; - + if (_cmd_sub.updated()) { /* got command */ - cmd_sub.copy(&cmd); + vehicle_command_s cmd; - /* handle it */ - if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) { - status_changed = true; + if (_cmd_sub.copy(&cmd)) { + if (handle_command(&status, cmd, &armed, _command_ack_pub)) { + _status_changed = true; + } } } @@ -2250,7 +2061,7 @@ Commander::run() if (failure_status != status.failure_detector_status) { status.failure_detector_status = failure_status; - status_changed = true; + _status_changed = true; } } @@ -2262,7 +2073,7 @@ Commander::run() if (_failure_detector.isFailure()) { armed.force_failsafe = true; - status_changed = true; + _status_changed = true; _flight_termination_triggered = true; @@ -2279,8 +2090,8 @@ Commander::run() const vehicle_local_position_s &local_position = _local_position_sub.get(); if (armed.armed) { - if ((!was_armed || (was_landed && !land_detector.landed)) && - (hrt_elapsed_time(&commander_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + if ((!_was_armed || (_was_landed && !_land_detector.landed)) && + (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ set_home_position(); @@ -2288,7 +2099,7 @@ Commander::run() } else { if (status_flags.condition_home_position_valid) { - if (land_detector.landed && local_position.xy_valid && local_position.z_valid) { + if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) { /* distance from home */ float home_dist_xy = -1.0f; float home_dist_z = -1.0f; @@ -2318,28 +2129,30 @@ Commander::run() } // check for arming state change - if (was_armed != armed.armed) { - status_changed = true; + if (_was_armed != armed.armed) { + _status_changed = true; if (!armed.armed) { // increase the flight uuid upon disarming - ++flight_uuid; - param_set(_param_flight_uuid, &flight_uuid); - last_disarmed_timestamp = hrt_absolute_time(); + const int32_t flight_uuid = _param_flight_uuid.get() + 1; + _param_flight_uuid.set(flight_uuid); + _param_flight_uuid.commit_no_notification(); + + _last_disarmed_timestamp = hrt_absolute_time(); } } - was_armed = armed.armed; + _was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, &armed, - &internal_state, + &_internal_state, &mavlink_log_pub, (link_loss_actions_t)_param_nav_dll_act.get(), _mission_result_sub.get().finished, _mission_result_sub.get().stay_in_failsafe, status_flags, - land_detector.landed, + _land_detector.landed, (link_loss_actions_t)_param_nav_rcl_act.get(), (offboard_loss_actions_t)_param_com_obl_act.get(), (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(), @@ -2349,8 +2162,8 @@ Commander::run() status.nav_state_timestamp = hrt_absolute_time(); } - if (status.failsafe != failsafe_old) { - status_changed = true; + if (status.failsafe != _failsafe_old) { + _status_changed = true; if (status.failsafe) { mavlink_log_info(&mavlink_log_pub, "Failsafe mode activated"); @@ -2359,11 +2172,11 @@ Commander::run() mavlink_log_info(&mavlink_log_pub, "Failsafe mode deactivated"); } - failsafe_old = status.failsafe; + _failsafe_old = status.failsafe; } /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 1 Hz or immediately when changed */ - if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed || nav_state_changed) { + if (hrt_elapsed_time(&status.timestamp) >= 1_s || _status_changed || nav_state_changed) { update_control_mode(); @@ -2381,13 +2194,13 @@ Commander::run() * (all output drivers should be started / unlocked last in the boot process * when the rest of the system is fully initialized) */ - armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5_s); + armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s); break; case PrearmedMode::SAFETY_BUTTON: - if (safety.safety_switch_available) { + if (_safety.safety_switch_available) { /* safety switch is present, go into prearmed if safety is off */ - armed.prearmed = safety.safety_off; + armed.prearmed = _safety.safety_off; } else { /* safety switch is not present, do not go into prearmed */ @@ -2405,8 +2218,8 @@ Commander::run() _armed_pub.publish(armed); /* publish internal state for logging purposes */ - internal_state.timestamp = hrt_absolute_time(); - _commander_state_pub.publish(internal_state); + _internal_state.timestamp = hrt_absolute_time(); + _commander_state_pub.publish(_internal_state); /* publish vehicle_status_flags */ status_flags.timestamp = hrt_absolute_time(); @@ -2414,11 +2227,12 @@ Commander::run() } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available - && safety.safety_off))) { + if (!_arm_tune_played && armed.armed && + (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) { + /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - arm_tune_played = true; + _arm_tune_played = true; } else if (!status_flags.usb_connected && (status.hil_state != vehicle_status_s::HIL_STATE_ON) && @@ -2439,28 +2253,28 @@ Commander::run() } /* reset arm_tune_played when disarmed */ - if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { //Notify the user that it is safe to approach the vehicle - if (arm_tune_played) { + if (_arm_tune_played) { tune_neutral(true); } - arm_tune_played = false; + _arm_tune_played = false; } /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ - status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); + status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout)) { set_tune_override(TONE_GPS_WARNING_TUNE); sensor_fail_tune_played = true; - status_changed = true; + _status_changed = true; } - counter++; + _counter++; int blink_state = blink_msg_state(); @@ -2468,19 +2282,19 @@ Commander::run() /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ - control_status_leds(&status, &armed, true, _battery_warning, &cpuload); + control_status_leds(&status, &armed, true, _battery_warning); } } else { /* normal state */ - control_status_leds(&status, &armed, status_changed, _battery_warning, &cpuload); + control_status_leds(&status, &armed, _status_changed, _battery_warning); } - status_changed = false; + _status_changed = false; if (!armed.armed) { /* Reset the flag if disarmed. */ - have_taken_off_since_arming = false; + _have_taken_off_since_arming = false; } arm_auth_update(now, params_updated || param_init_forced); @@ -2539,12 +2353,12 @@ Commander::check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, } void -control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, - bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local) +Commander::control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, + const uint8_t battery_warning) { static hrt_abstime overload_start = 0; - bool overload = (cpuload_local->load > 0.95f) || (cpuload_local->ram_usage > 0.98f); + bool overload = (_cpuload.load > 0.95f) || (_cpuload.ram_usage > 0.98f); if (overload_start == 0 && overload) { overload_start = hrt_absolute_time(); @@ -2554,7 +2368,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } /* driving rgbled */ - if (changed || last_overload != overload) { + if (changed || _last_overload != overload) { uint8_t led_mode = led_control_s::MODE_OFF; uint8_t led_color = led_control_s::COLOR_WHITE; bool set_normal_color = false; @@ -2617,7 +2431,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } } - last_overload = overload; + _last_overload = overload; /* board supports HW armed indicator */ @@ -2630,7 +2444,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (status.failsafe) { BOARD_ARMED_LED_OFF(); - if (leds_counter % 5 == 0) { + if (_leds_counter % 5 == 0) { BOARD_ARMED_STATE_LED_TOGGLE(); } @@ -2645,7 +2459,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu BOARD_ARMED_LED_OFF(); /* ready to arm, blink at 1Hz */ - if (leds_counter % 20 == 0) { + if (_leds_counter % 20 == 0) { BOARD_ARMED_STATE_LED_TOGGLE(); } @@ -2653,7 +2467,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu BOARD_ARMED_LED_OFF(); /* not ready to arm, blink at 10Hz */ - if (leds_counter % 2 == 0) { + if (_leds_counter % 2 == 0) { BOARD_ARMED_STATE_LED_TOGGLE(); } } @@ -2662,7 +2476,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* give system warnings on error LED */ if (overload) { - if (leds_counter % 2 == 0) { + if (_leds_counter % 2 == 0) { BOARD_OVERLOAD_LED_TOGGLE(); } @@ -2670,13 +2484,13 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu BOARD_OVERLOAD_LED_OFF(); } - leds_counter++; + _leds_counter++; } transition_result_t Commander::set_main_state(const vehicle_status_s &status_local, bool *changed) { - if (safety.override_available && safety.override_enabled) { + if (_safety.override_available && _safety.override_enabled) { return set_main_state_override_on(status_local, changed); } else { @@ -2688,7 +2502,7 @@ transition_result_t Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed) { const transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, - &internal_state); + &_internal_state); *changed = (res == TRANSITION_CHANGED); return res; @@ -2708,18 +2522,18 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid); const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid); const bool first_time_rc = (_last_sp_man.timestamp == 0); - const bool rc_values_updated = (_last_sp_man.timestamp != sp_man.timestamp); + const bool rc_values_updated = (_last_sp_man.timestamp != _sp_man.timestamp); const bool some_switch_changed = - (_last_sp_man.offboard_switch != sp_man.offboard_switch) - || (_last_sp_man.return_switch != sp_man.return_switch) - || (_last_sp_man.mode_switch != sp_man.mode_switch) - || (_last_sp_man.acro_switch != sp_man.acro_switch) - || (_last_sp_man.rattitude_switch != sp_man.rattitude_switch) - || (_last_sp_man.posctl_switch != sp_man.posctl_switch) - || (_last_sp_man.loiter_switch != sp_man.loiter_switch) - || (_last_sp_man.mode_slot != sp_man.mode_slot) - || (_last_sp_man.stab_switch != sp_man.stab_switch) - || (_last_sp_man.man_switch != sp_man.man_switch); + (_last_sp_man.offboard_switch != _sp_man.offboard_switch) + || (_last_sp_man.return_switch != _sp_man.return_switch) + || (_last_sp_man.mode_switch != _sp_man.mode_switch) + || (_last_sp_man.acro_switch != _sp_man.acro_switch) + || (_last_sp_man.rattitude_switch != _sp_man.rattitude_switch) + || (_last_sp_man.posctl_switch != _sp_man.posctl_switch) + || (_last_sp_man.loiter_switch != _sp_man.loiter_switch) + || (_last_sp_man.mode_slot != _sp_man.mode_slot) + || (_last_sp_man.stab_switch != _sp_man.stab_switch) + || (_last_sp_man.man_switch != _sp_man.man_switch); // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink const bool should_evaluate_rc_mode_switch = first_time_rc @@ -2735,33 +2549,33 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed // the sticks to break out of the autonomous state if (!_geofence_warning_action_on - && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || - internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || - internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL || - internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || - internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || - internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) { + && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + _internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || + _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL || + _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || + _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || + _internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) { - _last_sp_man.timestamp = sp_man.timestamp; - _last_sp_man.x = sp_man.x; - _last_sp_man.y = sp_man.y; - _last_sp_man.z = sp_man.z; - _last_sp_man.r = sp_man.r; + _last_sp_man.timestamp = _sp_man.timestamp; + _last_sp_man.x = _sp_man.x; + _last_sp_man.y = _sp_man.y; + _last_sp_man.z = _sp_man.z; + _last_sp_man.r = _sp_man.r; } /* no timestamp change or no switch change -> nothing changed */ return TRANSITION_NOT_CHANGED; } - _last_sp_man = sp_man; + _last_sp_man = _sp_man; // reset the position and velocity validity calculation to give the best change of being able to select // the desired mode reset_posvel_validity(changed); /* offboard switch overrides main switch */ - if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state); + if (_sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state); if (res == TRANSITION_DENIED) { print_reject_mode("OFFBOARD"); @@ -2774,14 +2588,14 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } /* RTL switch overrides main switch */ - if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state); + if (_sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_internal_state); if (res == TRANSITION_DENIED) { print_reject_mode("AUTO RTL"); /* fallback to LOITER if home position not set */ - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state); } if (res != TRANSITION_DENIED) { @@ -2793,8 +2607,8 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } /* Loiter switch overrides main switch */ - if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); + if (_sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state); if (res == TRANSITION_DENIED) { print_reject_mode("AUTO HOLD"); @@ -2805,21 +2619,21 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } /* we know something has changed - check if we are in mode slot operation */ - if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) { + if (_sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) { - if (sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) { + if (_sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) { warnx("m slot overflow"); return TRANSITION_DENIED; } - int new_mode = _flight_mode_slots[sp_man.mode_slot - 1]; + int new_mode = _flight_mode_slots[_sp_man.mode_slot - 1]; if (new_mode < 0) { /* slot is unused */ res = TRANSITION_NOT_CHANGED; } else { - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); /* ensure that the mode selection does not get stuck here */ int maxcount = 5; @@ -2835,7 +2649,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to loiter */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; print_reject_mode("AUTO MISSION"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2847,7 +2661,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; print_reject_mode("AUTO RTL"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2859,7 +2673,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; print_reject_mode("AUTO LAND"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2871,7 +2685,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; print_reject_mode("AUTO TAKEOFF"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2883,7 +2697,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; print_reject_mode("AUTO FOLLOW"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2895,7 +2709,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_POSCTL; print_reject_mode("AUTO HOLD"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2907,7 +2721,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to altitude control */ new_mode = commander_state_s::MAIN_STATE_ALTCTL; print_reject_mode("POSITION CONTROL"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2919,7 +2733,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to stabilized */ new_mode = commander_state_s::MAIN_STATE_STAB; print_reject_mode("ALTITUDE CONTROL"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2931,7 +2745,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* fall back to manual */ new_mode = commander_state_s::MAIN_STATE_MANUAL; print_reject_mode("STABILIZED"); - res = main_state_transition(status_local, new_mode, status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; @@ -2944,44 +2758,44 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } /* offboard and RTL switches off or denied, check main mode switch */ - switch (sp_man.mode_switch) { + switch (_sp_man.mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL - if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE && - sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { + if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE && + _sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { /* * Legacy mode: * Acro switch being used as stabilized switch in FW. */ - if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* manual mode is stabilized already for multirotors, so switch to acro * for any non-manual mode */ if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &_internal_state); } else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); } - } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + } else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &_internal_state); } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); } } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); } } else { @@ -2989,25 +2803,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed * - Acro is Acro * - Manual is not default anymore when the manaul switch is assigned */ - if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); - } else if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state); + } else if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &_internal_state); - } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state); + } else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &_internal_state); - } else if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + } else if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); - } else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { + } else if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { // default to MANUAL when no manual switch is set - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); } else { // default to STAB when the manual switch is assigned (but off) - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state); } } @@ -3015,8 +2829,8 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed break; case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST - if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); + if (_sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3026,23 +2840,23 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed } // fallback to ALTCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { + if (_sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode("ALTITUDE CONTROL"); } // fallback to MANUAL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); // TRANSITION_DENIED is not possible here break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -3051,28 +2865,28 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed print_reject_mode("AUTO MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to POSCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &_internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state); // TRANSITION_DENIED is not possible here break; @@ -3116,7 +2930,7 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac bool valid = was_valid; // constrain probation times - if (land_detector.landed) { + if (_land_detector.landed) { *probation_time_us = POSVEL_PROBATION_MIN; } @@ -3334,22 +3148,22 @@ Commander::update_control_mode() } bool -stabilization_required() +Commander::stabilization_required() { return (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || // is a rotary wing, or status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or - (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND + (_vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)); // is a fixed wing, ie: transitioning back to rotary wing mode } void -print_reject_mode(const char *msg) +Commander::print_reject_mode(const char *msg) { - hrt_abstime t = hrt_absolute_time(); + const hrt_abstime t = hrt_absolute_time(); - if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { - last_print_mode_reject_time = t; + if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + _last_print_mode_reject_time = t; mavlink_log_critical(&mavlink_log_pub, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors @@ -3359,12 +3173,12 @@ print_reject_mode(const char *msg) } void -print_reject_arm(const char *msg) +Commander::print_reject_arm(const char *msg) { - hrt_abstime t = hrt_absolute_time(); + const hrt_abstime t = hrt_absolute_time(); - if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { - last_print_mode_reject_time = t; + if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + _last_print_mode_reject_time = t; mavlink_log_critical(&mavlink_log_pub, "%s", msg); tune_negative(true); } @@ -3442,64 +3256,18 @@ void *commander_low_prio_loop(void *arg) /* if we reach here, we have a valid command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - /* ignore commands the high-prio loop or the navigator handles */ - if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_MODE || - cmd.command == vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO || - cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { - - continue; - } - /* only handle low-priority commands here */ switch (cmd.command) { - - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (((int)(cmd.param1)) == 0) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - /* do nothing for autopilot */ - break; - } - - if (is_safe(safety, armed)) { - - if (((int)(cmd.param1)) == 1) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - px4_usleep(100000); - /* reboot */ - px4_shutdown_request(true, false); - - } else if (((int)(cmd.param1)) == 2) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - px4_usleep(100000); - /* shutdown */ - px4_shutdown_request(false, false); - - } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); - px4_usleep(100000); - /* reboot to bootloader */ - px4_shutdown_request(true, true); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); - } - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); - } - - break; - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { int calib_ret = PX4_ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, safety, 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, - arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { + 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 + ) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); break; @@ -3583,11 +3351,17 @@ void *commander_low_prio_loop(void *arg) if (calib_ret == OK) { tune_positive(true); - Commander::preflight_check(false); + // time since boot not relevant here + if (PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, false, false, false, 30_s)) { - arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, + status_flags.condition_system_sensors_initialized = true; + } + + arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + &mavlink_log_pub, &status_flags, + PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY + 30_s); // time since boot not relevant for switching to ARMING_STATE_STANDBY } else { tune_negative(true); @@ -3684,15 +3458,6 @@ int Commander::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int Commander::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - return 0; -} - int Commander::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("commander", @@ -3731,7 +3496,7 @@ void Commander::enable_hil() void Commander::mission_init() { /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ - mission_s mission = {}; + mission_s mission{}; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { @@ -3748,26 +3513,12 @@ void Commander::mission_init() dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } - orb_advert_t mission_pub = orb_advertise(ORB_ID(mission), &mission); - orb_unadvertise(mission_pub); + uORB::Publication mission_pub{ORB_ID(mission)}; + mission_pub.publish(mission); } } -bool Commander::preflight_check(bool report) -{ - const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT); - - const bool success = PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, - hrt_elapsed_time(&commander_boot_timestamp)); - - if (success) { - status_flags.condition_system_sensors_initialized = true; - } - - return success; -} - -void Commander::data_link_check(bool &status_changed) +void Commander::data_link_check() { if (_telemetry_status_sub.updated()) { @@ -3792,7 +3543,7 @@ void Commander::data_link_check(bool &status_changed) if (status.high_latency_data_link_lost) { if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { status.high_latency_data_link_lost = false; - status_changed = true; + _status_changed = true; } } } @@ -3809,7 +3560,7 @@ void Commander::data_link_check(bool &status_changed) if (status.data_link_lost) { if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) { status.data_link_lost = false; - status_changed = true; + _status_changed = true; if (_datalink_last_heartbeat_gcs != 0) { mavlink_log_info(&mavlink_log_pub, "Data link regained"); @@ -3831,7 +3582,7 @@ void Commander::data_link_check(bool &status_changed) if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) { mavlink_log_info(&mavlink_log_pub, "Onboard controller regained"); _onboard_controller_lost = false; - status_changed = true; + _status_changed = true; } } @@ -3848,7 +3599,7 @@ void Commander::data_link_check(bool &status_changed) if (_avoidance_system_lost) { mavlink_log_info(&mavlink_log_pub, "Avoidance system regained"); - status_changed = true; + _status_changed = true; _avoidance_system_lost = false; status_flags.avoidance_system_valid = true; } @@ -3870,7 +3621,7 @@ void Commander::data_link_check(bool &status_changed) mavlink_log_critical(&mavlink_log_pub, "Data link lost"); - status_changed = true; + _status_changed = true; } } @@ -3881,7 +3632,7 @@ void Commander::data_link_check(bool &status_changed) mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost"); _onboard_controller_lost = true; - status_changed = true; + _status_changed = true; } // AVOIDANCE SYSTEM state check (only if it is enabled) @@ -3924,7 +3675,7 @@ void Commander::data_link_check(bool &status_changed) if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) { mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected"); status_flags.avoidance_system_valid = false; - status_changed = true; + _status_changed = true; } _avoidance_system_status_change = false; @@ -3940,7 +3691,7 @@ void Commander::data_link_check(bool &status_changed) 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"); - status_changed = true; + _status_changed = true; } } } @@ -3986,7 +3737,7 @@ void Commander::battery_status_check() // execute battery failsafe if the state has gotten worse while we are 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()); } @@ -4013,7 +3764,7 @@ void Commander::battery_status_check() } } -void Commander::estimator_check(bool *status_changed) +void Commander::estimator_check() { // Check if quality checking of position accuracy and consistency is to be performed const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check; @@ -4042,7 +3793,7 @@ void Commander::estimator_check(bool *status_changed) && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS)) && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS))); - const bool operator_controlled_position = (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL); + const bool operator_controlled_position = (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL); _skip_pos_accuracy_check = reliant_on_opt_flow && operator_controlled_position; @@ -4065,7 +3816,7 @@ void Commander::estimator_check(bool *status_changed) _nav_test_failed = false; _nav_test_passed = false; - } else if (land_detector.landed) { + } else if (_land_detector.landed) { // record time of takeoff _time_at_takeoff = hrt_absolute_time(); @@ -4111,14 +3862,15 @@ void Commander::estimator_check(bool *status_changed) if (!_skip_pos_accuracy_check) { // use global position message to determine validity check_posvel_validity(true, gpos.eph, _eph_threshold_adj, gpos.timestamp, &_last_gpos_fail_time_us, - &_gpos_probation_time_us, &status_flags.condition_global_position_valid, status_changed); + &_gpos_probation_time_us, &status_flags.condition_global_position_valid, &_status_changed); } // use local position message to determine validity check_posvel_validity(lpos.xy_valid, lpos.eph, _eph_threshold_adj, lpos.timestamp, &_last_lpos_fail_time_us, - &_lpos_probation_time_us, &status_flags.condition_local_position_valid, status_changed); + &_lpos_probation_time_us, &status_flags.condition_local_position_valid, &_status_changed); + check_posvel_validity(lpos.v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, &_last_lvel_fail_time_us, - &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, status_changed); + &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, &_status_changed); } } @@ -4130,11 +3882,11 @@ void Commander::estimator_check(bool *status_changed) } check_valid(lpos.timestamp, _param_com_pos_fs_delay.get() * 1_s, lpos.z_valid, - &(status_flags.condition_local_altitude_valid), status_changed); + &(status_flags.condition_local_altitude_valid), &_status_changed); } void -Commander::offboard_control_update(bool &status_changed) +Commander::offboard_control_update() { const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); @@ -4162,7 +3914,7 @@ Commander::offboard_control_update(bool &status_changed) old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force || old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) { - status_changed = true; + _status_changed = true; } } } @@ -4172,13 +3924,13 @@ Commander::offboard_control_update(bool &status_changed) if (status_flags.offboard_control_signal_lost) { status_flags.offboard_control_signal_lost = false; status_flags.offboard_control_loss_timeout = false; - status_changed = true; + _status_changed = true; } } else { if (!status_flags.offboard_control_signal_lost) { status_flags.offboard_control_signal_lost = true; - status_changed = true; + _status_changed = true; } /* check timer if offboard was there but now lost */ @@ -4194,7 +3946,7 @@ Commander::offboard_control_update(bool &status_changed) } if (status_flags.offboard_control_loss_timeout) { - status_changed = true; + _status_changed = true; } } } @@ -4237,7 +3989,7 @@ void Commander::esc_status_check(const esc_status_s &esc_status) } } -void usage(const char *reason) +int Commander::print_usage(const char *reason) { if (reason) { PX4_INFO("%s", reason); @@ -4267,4 +4019,6 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("lockdown"); PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 1; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index b4416d57d9..b428deb982 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -34,42 +34,55 @@ #ifndef COMMANDER_HPP_ #define COMMANDER_HPP_ -#include "state_machine_helper.h" +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" +#include "state_machine_helper.h" #include +#include #include #include #include -#include // publications #include #include #include #include +#include #include #include #include #include -#include // subscriptions #include +#include #include +#include +#include +#include #include +#include #include +#include #include #include #include +#include +#include +#include +#include #include #include #include #include +#include #include -#include +#include using math::constrain; +using systemlib::Hysteresis; using namespace time_literals; @@ -95,13 +108,65 @@ public: void enable_hil(); - // TODO: only temporarily static until low priority thread is removed - static bool preflight_check(bool report); - void get_circuit_breaker_params(); private: + transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy); + + void battery_status_check(); + + void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, + bool *changed); + + bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, + const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, + bool *validity_changed); + + void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, + const uint8_t battery_warning); + + /** + * Checks the status of all available data links and handles switching between different system telemetry states. + */ + void data_link_check(); + + void esc_status_check(const esc_status_s &esc_status); + + void estimator_check(); + + bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, + uORB::PublicationQueued &command_ack_pub); + + unsigned handle_command_motor_test(const vehicle_command_s &cmd); + + void mission_init(); + + void offboard_control_update(); + + void print_reject_arm(const char *msg); + void print_reject_mode(const char *msg); + + void reset_posvel_validity(bool *changed); + + bool set_home_position(); + bool set_home_position_alt_only(); + + void update_control_mode(); + + // Set the main system state based on RC and override device inputs + transition_result_t set_main_state(const vehicle_status_s &status, bool *changed); + + // Enable override (manual reversion mode) on the system + transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed); + + // Set the system main state based on the current RC inputs + transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed); + + bool shutdown_if_allowed(); + + bool stabilization_required(); + DEFINE_PARAMETERS( (ParamInt) _param_nav_dll_act, @@ -129,28 +194,63 @@ private: (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_preflight, - (ParamInt) _param_com_obs_avoid, + (ParamBool) _param_com_obs_avoid, (ParamInt) _param_com_oa_boot_t, (ParamInt) _param_com_flt_profile, - + // Offboard (ParamFloat) _param_com_of_loss_t, (ParamInt) _param_com_obl_act, (ParamInt) _param_com_obl_rc_act, (ParamInt) _param_com_prearm_mode, - (ParamInt) _param_com_mot_test_en, + (ParamBool) _param_com_mot_test_en, (ParamFloat) _param_com_kill_disarm, + // Engine failure + (ParamFloat) _param_ef_throttle_thres, + (ParamFloat) _param_ef_current2throttle_thres, + (ParamFloat) _param_ef_time_thres, + + (ParamBool) _param_arm_without_gps, + (ParamBool) _param_arm_switch_is_button, + (ParamBool) _param_arm_mission_required, + (ParamBool) _param_arm_auth_required, + (ParamBool) _param_escs_checks_required, + + (ParamInt) _param_flight_uuid, + (ParamInt) _param_takeoff_finished_action, + + (ParamBool) _param_rc_override, + (ParamInt) _param_rc_in_off, + (ParamInt) _param_rc_arm_hyst, + (ParamFloat) _param_min_stick_change, + + (ParamInt) _param_fltmode_1, + (ParamInt) _param_fltmode_2, + (ParamInt) _param_fltmode_3, + (ParamInt) _param_fltmode_4, + (ParamInt) _param_fltmode_5, + (ParamInt) _param_fltmode_6, + + // Circuit breakers (ParamInt) _param_cbrk_supply_chk, (ParamInt) _param_cbrk_usb_chk, (ParamInt) _param_cbrk_airspd_chk, (ParamInt) _param_cbrk_enginefail, (ParamInt) _param_cbrk_gpsfail, (ParamInt) _param_cbrk_flightterm, - (ParamInt) _param_cbrk_velposerr + (ParamInt) _param_cbrk_velposerr, + + // Geofrence + (ParamInt) _param_geofence_action, + + // Mavlink + (ParamInt) _param_mav_comp_id, + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_type ) enum class PrearmedMode { @@ -159,9 +259,22 @@ private: ALWAYS = 2 }; + /* Decouple update interval and hysteresis counters, all depends on intervals */ + static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; + static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)}; + + static constexpr float STICK_ON_OFF_LIMIT{0.9f}; + + static constexpr uint64_t OFFBOARD_TIMEOUT{500_ms}; + static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */ + static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms}; + static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; + const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */ const int64_t POSVEL_PROBATION_MAX = 100_s; /**< maximum probation duration (usec) */ + PreFlightCheck::arm_requirements_t _arm_requirements{}; + hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */ hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */ hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */ @@ -177,89 +290,99 @@ private: bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */ bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */ - bool _geofence_loiter_on{false}; - bool _geofence_rtl_on{false}; - bool _geofence_warning_action_on{false}; - bool _geofence_violated_prev{false}; + bool _geofence_loiter_on{false}; + bool _geofence_rtl_on{false}; + bool _geofence_warning_action_on{false}; + bool _geofence_violated_prev{false}; - FailureDetector _failure_detector; - bool _flight_termination_triggered{false}; + FailureDetector _failure_detector; + bool _flight_termination_triggered{false}; - bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, - uORB::PublicationQueued &command_ack_pub, bool *changed); - - unsigned handle_command_motor_test(const vehicle_command_s &cmd); - - bool set_home_position(); - bool set_home_position_alt_only(); - - // Set the main system state based on RC and override device inputs - transition_result_t set_main_state(const vehicle_status_s &status, bool *changed); - - // Enable override (manual reversion mode) on the system - transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed); - - // Set the system main state based on the current RC inputs - transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed); - - void update_control_mode(); - - void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, - bool *changed); - - bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, - const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, - bool *validity_changed); - - void reset_posvel_validity(bool *changed); - - void mission_init(); - - void estimator_check(bool *status_changed); - - void offboard_control_update(bool &status_changed); - - void battery_status_check(); - - void esc_status_check(const esc_status_s &esc_status); - - /** - * Checks the status of all available data links and handles switching between different system telemetry states. - */ - void data_link_check(bool &status_changed); - - uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; hrt_abstime _datalink_last_heartbeat_gcs{0}; - - hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; - bool _onboard_controller_lost{false}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; - bool _avoidance_system_lost{false}; - + hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; + bool _onboard_controller_lost{false}; + bool _avoidance_system_lost{false}; bool _avoidance_system_status_change{false}; - uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT}; - - uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; + uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT}; hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; - int _last_esc_online_flags{-1}; + int _last_esc_online_flags{-1}; - uORB::Subscription _battery_sub{ORB_ID(battery_status)}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; - float _battery_current{0.0f}; + uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; + float _battery_current{0.0f}; - systemlib::Hysteresis _auto_disarm_landed{false}; - systemlib::Hysteresis _auto_disarm_killed{false}; + Hysteresis _auto_disarm_landed{false}; + Hysteresis _auto_disarm_killed{false}; - bool _print_avoidance_msg_once{false}; + bool _print_avoidance_msg_once{false}; + + hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent + + float _eph_threshold_adj{INFINITY}; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition + bool _skip_pos_accuracy_check{false}; + bool _last_condition_local_altitude_valid{false}; + bool _last_condition_local_position_valid{false}; + bool _last_condition_global_position_valid{false}; + + bool _last_overload{false}; + + unsigned int _leds_counter{0}; + + manual_control_setpoint_s _sp_man{}; ///< the current manual control setpoint + manual_control_setpoint_s _last_sp_man{}; ///< the manual control setpoint valid at the last mode switch + hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost + int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {}; + uint8_t _last_sp_man_arm_switch{0}; + float _min_stick_change{}; + uint32_t _stick_off_counter{0}; + uint32_t _stick_on_counter{0}; + + hrt_abstime _boot_timestamp{0}; + hrt_abstime _last_disarmed_timestamp{0}; + hrt_abstime _timestamp_engine_healthy{0}; ///< absolute time when engine was healty + + uint32_t _counter{0}; + + bool _status_changed{true}; + bool _arm_tune_played{false}; + bool _was_landed{true}; + bool _was_falling{false}; + bool _was_armed{false}; + bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag + bool _have_taken_off_since_arming{false}; + bool _flight_termination_printed{false}; + + main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL}; + + commander_state_s _internal_state{}; + cpuload_s _cpuload{}; + geofence_result_s _geofence_result{}; + vehicle_land_detected_s _land_detector{}; + safety_s _safety{}; + vtol_vehicle_status_s _vtol_status{}; // Subscriptions + uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; + uORB::Subscription _battery_sub{ORB_ID(battery_status)}; + uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; + uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; + uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _power_button_state_sub{ORB_ID(power_button_state)}; + uORB::Subscription _safety_sub{ORB_ID(safety)}; + uORB::Subscription _sp_man_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)}; + uORB::Subscription _system_power_sub{ORB_ID(system_power)}; + uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; @@ -269,15 +392,17 @@ private: uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; // Publications - uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; - uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; - uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; + uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; + uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; + }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e85d02aff6..1e0c55c4e0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -303,10 +303,7 @@ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1); * If parameter set button gets handled like stick arming. * * @group Commander - * @min 0 - * @max 1 - * @value 0 Arm switch is a switch that stays on when armed - * @value 1 Arm switch is a button that only triggers arming and disarming + * @boolean */ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 73c04af69e..bd39edc209 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -55,7 +55,6 @@ public: private: bool armingStateTransitionTest(); bool mainStateTransitionTest(); - bool isSafeTest(); }; bool StateMachineHelperTest::armingStateTransitionTest() @@ -287,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() for (size_t i = 0; i < cArmingTransitionTests; i++) { const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i]; - const bool check_gps = false; + PreFlightCheck::arm_requirements_t arm_req{}; // Setup initial machine state status.arming_state = test->current_state.arming_state; @@ -305,7 +304,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() true /* enable pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, - (check_gps ? PreFlightCheck::ARM_REQ_GPS_BIT : 0), + arm_req, 2e6 /* 2 seconds after boot, everything should be checked */ ); @@ -536,49 +535,10 @@ bool StateMachineHelperTest::mainStateTransitionTest() return true; } -bool StateMachineHelperTest::isSafeTest() -{ - struct safety_s safety = {}; - struct actuator_armed_s armed = {}; - - armed.armed = false; - armed.lockdown = false; - safety.safety_switch_available = true; - safety.safety_off = false; - ut_compare("is safe: not armed", is_safe(safety, armed), true); - - armed.armed = false; - armed.lockdown = true; - safety.safety_switch_available = true; - safety.safety_off = true; - ut_compare("is safe: software lockdown", is_safe(safety, armed), true); - - armed.armed = true; - armed.lockdown = false; - safety.safety_switch_available = true; - safety.safety_off = true; - ut_compare("not safe: safety off", is_safe(safety, armed), false); - - armed.armed = true; - armed.lockdown = false; - safety.safety_switch_available = true; - safety.safety_off = false; - ut_compare("is safe: safety off", is_safe(safety, armed), true); - - armed.armed = true; - armed.lockdown = false; - safety.safety_switch_available = false; - safety.safety_off = false; - ut_compare("not safe: no safety switch", is_safe(safety, armed), false); - - return true; -} - bool StateMachineHelperTest::run_tests() { ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); - ut_run_test(isSafeTest); return (_tests_failed == 0); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 86a03ff305..3db82124ad 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,7 +46,6 @@ #include #include -#include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "state_machine_helper.h" #include "commander_helper.h" @@ -105,7 +104,8 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, + orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, + const PreFlightCheck::arm_requirements_t &arm_requirements, const hrt_abstime &time_since_boot) { // Double check that our static arrays are still valid @@ -130,14 +130,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe */ bool preflight_check_ret = true; - const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT); - /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { - preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, - time_since_boot); + preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, + arm_requirements.global_position, true, true, time_since_boot); if (preflight_check_ret) { status_flags->condition_system_sensors_initialized = true; @@ -156,8 +154,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, - *status_flags, - checkGNSS, false, false, time_since_boot); + *status_flags, arm_requirements.global_position, false, false, time_since_boot); last_preflight_check = hrt_absolute_time(); } @@ -245,17 +242,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe return ret; } -bool is_safe(const safety_s &safety, const actuator_armed_s &armed) -{ - // System is safe if: - // 1) Not armed - // 2) Armed, but in software lockdown (HIL) - // 3) Safety switch is present AND engaged -> actuators locked - const bool lockdown = (armed.lockdown || armed.manual_lockdown); - - return !armed.armed || (armed.armed && lockdown) || (safety.safety_switch_available && !safety.safety_off); -} - 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) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 071afba6d3..9b2a42fd8a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -44,6 +44,8 @@ #include +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" + #include #include #include @@ -96,12 +98,11 @@ enum class position_nav_loss_actions_t { extern const char *const arming_state_names[]; -bool is_safe(const safety_s &safety, const actuator_armed_s &armed); - transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, - vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot); + vehicle_status_flags_s *status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, + const hrt_abstime &time_since_boot); transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,