From e4bb219d10361816ed2c0b1be2aeaebf17522070 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Sep 2022 07:56:39 +0200 Subject: [PATCH] vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status --- msg/vehicle_status.msg | 10 +- msg/vehicle_status_flags.msg | 63 +++++----- src/modules/commander/Commander.cpp | 112 +++++------------- src/modules/commander/Commander.hpp | 6 - .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.cpp | 12 ++ .../HealthAndArmingChecks.hpp | 7 +- .../checks/estimatorCheck.cpp | 54 ++++----- .../checks/geofenceCheck.cpp | 2 +- .../checks/homePositionCheck.cpp | 4 +- .../checks/missionCheck.cpp | 4 + .../checks/modeCheck.cpp | 30 +++-- .../checks/modeCheck.hpp | 3 + .../checks/offboardCheck.cpp | 70 +++++++++++ .../checks/offboardCheck.hpp | 57 +++++++++ .../checks/systemCheck.cpp | 24 +--- .../checks/systemCheck.hpp | 1 - src/modules/commander/HomePosition.cpp | 18 +-- src/modules/commander/failsafe/framework.cpp | 16 +-- src/modules/events/rc_loss_alarm.cpp | 8 +- src/modules/events/rc_loss_alarm.h | 2 + src/modules/events/set_leds.cpp | 4 +- .../tasks/Utility/Sticks.cpp | 6 +- .../tasks/Utility/Sticks.hpp | 4 +- .../gyro_calibration/GyroCalibration.cpp | 18 +-- .../gyro_calibration/GyroCalibration.hpp | 2 - .../mag_bias_estimator/MagBiasEstimator.cpp | 26 ++-- .../mag_bias_estimator/MagBiasEstimator.hpp | 2 - src/modules/mavlink/streams/HEARTBEAT.hpp | 7 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 11 +- 30 files changed, 325 insertions(+), 259 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 599dca6c1b..a1b9e80370 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -85,10 +85,8 @@ uint8 VEHICLE_TYPE_AIRSHIP = 4 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control -uint64 failsafe_timestamp # time when failsafe was activated # Link loss -bool rc_signal_lost # true if RC reception lost bool data_link_lost # datalink to GCS lost uint8 data_link_lost_counter # counts unique data link lost events bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost @@ -99,7 +97,6 @@ bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotatio bool in_transition_mode # True if VTOL is doing a transition bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW - # MAVLink identification uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field @@ -107,7 +104,6 @@ uint8 component_id # subsystem / component id, contains MAVLink's component ID f bool safety_button_available # Set to true if a safety button is connected bool safety_off # Set to true if safety is off -bool auto_mission_available bool power_input_valid # set if input power is valid bool usb_connected # set to true (never cleared) once telemetry received from usb link @@ -120,3 +116,9 @@ bool parachute_system_healthy bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass + diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 5d9e32807a..80f6897cea 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -1,6 +1,5 @@ -# TODO: rename to failsafe_flags (will be input to failsafe state machine) +# Input flags for the failsafe state machine set by the arming & health checks. # -# Input flags for the failsafe state machine. # Flags must be named such that false == no failure (e.g. _invalid, _unhealthy, _lost) # The flag comments are used as label for the failsafe state machine simulation @@ -9,10 +8,10 @@ uint64 timestamp # time since system start (microseconds) # Per-mode requirements uint32 mode_req_angular_velocity uint32 mode_req_attitude +uint32 mode_req_local_alt uint32 mode_req_local_position uint32 mode_req_local_position_relaxed uint32 mode_req_global_position -uint32 mode_req_local_alt uint32 mode_req_mission uint32 mode_req_offboard_signal uint32 mode_req_home_position @@ -20,39 +19,39 @@ uint32 mode_req_prevent_arming # if set, cannot arm while in this mode uint32 mode_req_other # other requirements, not covered above (for external modes) -bool calibration_enabled -bool pre_flight_checks_pass # true if all checks necessary to arm pass -bool auto_mission_available # Mission available -bool angular_velocity_valid # Angular velocity valid -bool attitude_valid # Attitude valid -bool local_altitude_valid # Local altitude valid -bool local_position_valid # Local position estimate valid -bool local_position_valid_relaxed # Local position with reduced accuracy requirements (e.g. flying with optical flow) -bool local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation -bool global_position_valid # Global position estimate valid -bool gps_position_valid -bool home_position_valid # Home position valid +# Mode requirements +bool angular_velocity_invalid # Angular velocity invalid +bool attitude_invalid # Attitude invalid +bool local_altitude_invalid # Local altitude invalid +bool local_position_invalid # Local position estimate invalid +bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow) +bool local_velocity_invalid # Local velocity estimate invalid +bool global_position_invalid # Global position estimate invalid +bool gps_position_invalid # GPS position invalid +bool auto_mission_missing # No mission available +bool offboard_control_signal_lost # Offboard signal lost +bool home_position_invalid # No home position available -bool offboard_control_signal_lost # Offboard signal lost -bool geofence_violated # Geofence violated +# Control links +bool rc_signal_lost # RC signal lost +bool data_link_lost # Data link lost -bool rc_signal_lost # RC signal lost -bool data_link_lost # Data link lost -bool mission_failure # Mission failure -bool rc_calibration_in_progress -bool vtol_transition_failure # VTOL transition failed +# Battery +uint8 battery_warning # Battery warning level +bool battery_low_remaining_time # Low battery based on remaining flight time +bool battery_unhealthy # Battery unhealthy -bool battery_low_remaining_time -bool battery_unhealthy +bool geofence_violated # Geofence violated +bool mission_failure # Mission failure +bool vtol_transition_failure # VTOL transition failed (quadchute) -bool wind_limit_exceeded -bool flight_time_limit_exceeded +bool wind_limit_exceeded # Wind limit exceeded +bool flight_time_limit_exceeded # Maximum flight time exceeded -# failure detector -bool fd_esc_arming_failure -bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) -bool fd_imbalanced_prop # Imbalanced propeller detected -bool fd_motor_failure +# Failure detector +bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) +bool fd_esc_arming_failure # ESC failed to arm +bool fd_imbalanced_prop # Imbalanced propeller detected +bool fd_motor_failure # Motor failure -uint8 battery_warning # Battery warning level diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index add9053f85..7bd61084d5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -292,11 +292,10 @@ int Commander::custom_command(int argc, char *argv[]) } if (!strcmp(argv[0], "check")) { - uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS); - uORB::SubscriptionData vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; - PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED"); + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + PX4_INFO("Preflight check: %s", vehicle_status_sub.get().pre_flight_checks_pass ? "OK" : "FAILED"); return 0; } @@ -657,8 +656,6 @@ Commander::Commander() : /* mark all signals lost as long as they haven't been found */ _vehicle_status.data_link_lost = true; - _vehicle_status_flags.offboard_control_signal_lost = true; - _vehicle_status.power_input_valid = true; // default for vtol is rotary wing @@ -1051,7 +1048,7 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; // check if current mission and first item are valid - if (_vehicle_status.auto_mission_available) { + if (!_vehicle_status_flags.auto_mission_missing) { // requested first mission item valid if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { @@ -1201,7 +1198,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if ((int)(cmd.param1) == 1) { /* gyro calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::GyroCalibration); } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || @@ -1213,20 +1210,20 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::MagCalibration); } else if ((int)(cmd.param3) == 1) { /* baro calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::BaroCalibration); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); /* disable RC control input completely */ - _vehicle_status_flags.rc_calibration_in_progress = true; + _vehicle_status.rc_calibration_in_progress = true; mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t"); events::send(events::ID("commander_calib_rc_off"), events::Log::Info, "Calibration: Disabling RC input"); @@ -1234,32 +1231,32 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AccelCalibration); } else if ((int)(cmd.param5) == 2) { // board offset calibration answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::LevelCalibration); } else if ((int)(cmd.param5) == 4) { // accelerometer quick calibration answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) /* airspeed calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); } else if ((int)(cmd.param7) == 1) { @@ -1273,7 +1270,7 @@ Commander::handle_command(const vehicle_command_s &cmd) "ESCs calibration denied"); } else { - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _actuator_armed.in_esc_calibration_mode = true; _worker_thread.startTask(WorkerThread::Request::ESCCalibration); } @@ -1284,9 +1281,9 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ - if (_vehicle_status_flags.rc_calibration_in_progress) { + if (_vehicle_status.rc_calibration_in_progress) { /* enable RC control input */ - _vehicle_status_flags.rc_calibration_in_progress = false; + _vehicle_status.rc_calibration_in_progress = false; mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t"); events::send(events::ID("commander_calib_rc_on"), events::Log::Info, "Calibration: Restoring RC input"); @@ -1329,7 +1326,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } } - _vehicle_status_flags.calibration_enabled = true; + _vehicle_status.calibration_enabled = true; _worker_thread.setMagQuickData(heading_radians, latitude, longitude); _worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick); } @@ -1495,7 +1492,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) arm_disarm_reason_t arm_disarm_reason{}; // Silently ignore RC actions during RC calibration - if (_vehicle_status_flags.rc_calibration_in_progress + if (_vehicle_status.rc_calibration_in_progress && (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE || action_request.source == action_request_s::SOURCE_RC_SWITCH || action_request.source == action_request_s::SOURCE_RC_BUTTON @@ -1595,7 +1592,6 @@ void Commander::updateParameters() _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); - _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); @@ -1684,8 +1680,6 @@ Commander::run() handlePowerButtonState(); - offboard_control_update(); - systemPowerUpdate(); landDetectorUpdate(); @@ -1701,7 +1695,7 @@ Commander::run() battery_status_check(); /* If in INIT state, try to proceed to STANDBY state */ - if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) { + if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) { _arm_state_machine.arming_state_transition(_vehicle_status, vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, @@ -1784,11 +1778,10 @@ Commander::run() || !(_actuator_armed == actuator_armed_prev)) { // Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already) - if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) { + if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status.calibration_enabled) { perf_begin(_preflight_check_perf); _health_and_arming_checks.update(); - _vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm( - _vehicle_status.nav_state); + _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); perf_end(_preflight_check_perf); check_and_inform_ready_for_takeoff(); @@ -1868,14 +1861,14 @@ void Commander::checkForMissionUpdate() const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) && (mission_result.instance_count > 0); - _vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid; + bool auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { /* Only evaluate mission state if home is set */ - if (_vehicle_status_flags.home_position_valid && + if (!_vehicle_status_flags.home_position_invalid && (prev_mission_instance_count != mission_result.instance_count)) { - if (!_vehicle_status.auto_mission_available) { + if (!auto_mission_available) { /* the mission is invalid */ tune_mission_fail(true); @@ -1897,7 +1890,7 @@ void Commander::checkForMissionUpdate() && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) { - if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) { + if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) { _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); } else { @@ -2112,8 +2105,8 @@ void Commander::checkWorkerThread() int ret = _worker_thread.getResultAndReset(); _actuator_armed.in_esc_calibration_mode = false; - if (_vehicle_status_flags.calibration_enabled) { // did we do a calibration? - _vehicle_status_flags.calibration_enabled = false; + if (_vehicle_status.calibration_enabled) { // did we do a calibration? + _vehicle_status.calibration_enabled = false; if (ret == 0) { tune_positive(true); @@ -2312,7 +2305,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_ON; set_normal_color = true; - } else if (!_vehicle_status_flags.pre_flight_checks_pass) { + } else if (!_vehicle_status.pre_flight_checks_pass) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; @@ -2342,7 +2335,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_color = led_control_s::COLOR_RED; } else { - if (_vehicle_status_flags.home_position_valid && _vehicle_status_flags.global_position_valid) { + if (!_vehicle_status_flags.home_position_invalid && !_vehicle_status_flags.global_position_invalid) { led_color = led_control_s::COLOR_GREEN; } else { @@ -2413,6 +2406,7 @@ void Commander::update_control_mode() { _vehicle_control_mode = {}; + _offboard_control_mode_sub.update(); mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state, _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); _vehicle_control_mode.timestamp = hrt_absolute_time(); @@ -2786,54 +2780,6 @@ void Commander::manual_control_check() } } -void -Commander::offboard_control_update() -{ - bool offboard_available = false; - - if (_offboard_control_mode_sub.updated()) { - const offboard_control_mode_s old = _offboard_control_mode_sub.get(); - - if (_offboard_control_mode_sub.update()) { - const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get(); - - if (old.position != ocm.position || - old.velocity != ocm.velocity || - old.acceleration != ocm.acceleration || - old.attitude != ocm.attitude || - old.body_rate != ocm.body_rate || - old.actuator != ocm.actuator) { - - _status_changed = true; - } - - if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate || ocm.actuator) { - offboard_available = true; - } - } - } - - if (_offboard_control_mode_sub.get().position && !_vehicle_status_flags.local_position_valid) { - offboard_available = false; - - } else if (_offboard_control_mode_sub.get().velocity && !_vehicle_status_flags.local_velocity_valid) { - offboard_available = false; - - } else if (_offboard_control_mode_sub.get().acceleration && !_vehicle_status_flags.local_velocity_valid) { - // OFFBOARD acceleration handled by position controller - offboard_available = false; - } - - _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); - - const bool offboard_lost = !_offboard_available.get_state(); - - if (_vehicle_status_flags.offboard_control_signal_lost != offboard_lost) { - _vehicle_status_flags.offboard_control_signal_lost = offboard_lost; - _status_changed = true; - } -} - void Commander::send_parachute_command() { vehicle_command_s vcmd{}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 06c6550bd1..d0227f891c 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -147,8 +147,6 @@ private: void executeActionRequest(const action_request_s &action_request); - void offboard_control_update(); - void print_reject_mode(uint8_t nav_state); void update_control_mode(); @@ -195,9 +193,6 @@ private: (ParamFloat) _param_com_obc_loss_t, - // Offboard - (ParamFloat) _param_com_of_loss_t, - (ParamInt) _param_com_prearm_mode, (ParamBool) _param_com_force_safety, (ParamBool) _param_com_mot_test_en, @@ -252,7 +247,6 @@ private: uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_killed{false}; - Hysteresis _offboard_available{false}; hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent bool _mode_switch_mapped{false}; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 6daf570504..fb5bcc8025 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -60,6 +60,7 @@ px4_add_library(health_and_arming_checks checks/missionCheck.cpp checks/rcAndDataLinkCheck.cpp checks/vtolCheck.cpp + checks/offboardCheck.cpp ) add_dependencies(health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index ada699a2e9..c4088173ea 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -39,6 +39,18 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu _context(status), _reporter(status_flags) { + // Initialize mode requirements to invalid + _failsafe_flags.angular_velocity_invalid = true; + _failsafe_flags.attitude_invalid = true; + _failsafe_flags.local_altitude_invalid = true; + _failsafe_flags.local_position_invalid = true; + _failsafe_flags.local_position_invalid_relaxed = true; + _failsafe_flags.local_velocity_invalid = true; + _failsafe_flags.global_position_invalid = true; + _failsafe_flags.gps_position_invalid = true; + _failsafe_flags.auto_mission_missing = true; + _failsafe_flags.offboard_control_signal_lost = true; + _failsafe_flags.home_position_invalid = true; } bool HealthAndArmingChecks::update(bool force_reporting) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index fcefe8d5bd..3ef959ccb4 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -65,6 +65,7 @@ #include "checks/missionCheck.hpp" #include "checks/rcAndDataLinkCheck.hpp" #include "checks/vtolCheck.hpp" +#include "checks/offboardCheck.hpp" class HealthAndArmingChecks : public ModuleParams @@ -132,6 +133,7 @@ private: MissionChecks _mission_checks; RcAndDataLinkChecks _rc_and_data_link_checks; VtolChecks _vtol_checks; + OffboardChecks _offboard_checks; HealthAndArmingCheckBase *_checks[30] = { &_accelerometer_checks, @@ -147,7 +149,9 @@ private: &_magnetometer_checks, &_manual_control_checks, &_home_position_checks, - &_mode_checks, // must be after _estimator_checks & _home_position_checks + &_mission_checks, + &_offboard_checks, // must be after _estimator_checks + &_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks &_parachute_checks, &_power_checks, &_rc_calibration_checks, @@ -157,7 +161,6 @@ private: &_wind_checks, &_geofence_checks, // must be after _home_position_checks &_flight_time_checks, - &_mission_checks, &_rc_and_data_link_checks, &_vtol_checks, }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 71e07d602a..f1c7e4c832 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -119,11 +119,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } // set mode requirements - const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid; + const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid; setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, reporter.failsafeFlags()); - if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) { + if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) { gpsNoLongerValid(context, reporter); } } @@ -658,7 +658,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) PX4_DEBUG("GPS no longer valid"); // report GPS failure if armed and the global position estimate is still valid - if (context.isArmed() && reporter.failsafeFlags().global_position_valid) { + if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) { if (reporter.mavlink_log_pub()) { mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t"); } @@ -706,26 +706,26 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } } - failsafe_flags.global_position_valid = - checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, - _last_gpos_fail_time_us, failsafe_flags.global_position_valid); + failsafe_flags.global_position_invalid = + !checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, + _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); - failsafe_flags.local_position_valid = - checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp, - _last_lpos_fail_time_us, failsafe_flags.local_position_valid); + failsafe_flags.local_position_invalid = + !checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp, + _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); - failsafe_flags.local_position_valid_relaxed = - checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, - _last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid); + failsafe_flags.local_position_invalid_relaxed = + !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, + _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed); - failsafe_flags.local_velocity_valid = - checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, - _last_lvel_fail_time_us, failsafe_flags.local_velocity_valid); + failsafe_flags.local_velocity_invalid = + !checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, + _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid); // altitude - failsafe_flags.local_altitude_valid = lpos.z_valid - && (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s)); + failsafe_flags.local_altitude_invalid = !lpos.z_valid + || (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s)); // attitude @@ -740,11 +740,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f && (fabsf(q(3)) <= 1.f + eps); const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps; - failsafe_flags.attitude_valid = now < attitude.timestamp + 1_s && norm_in_tolerance - && no_element_larger_than_one; + failsafe_flags.attitude_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance + || !no_element_larger_than_one; } else { - failsafe_flags.attitude_valid = false; + failsafe_flags.attitude_invalid = true; } // angular velocity @@ -754,13 +754,13 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f && now < angular_velocity.timestamp + 1_s; const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); - const bool angular_velocity_valid = condition_angular_velocity_time_valid - && condition_angular_velocity_finite; + const bool angular_velocity_invalid = !condition_angular_velocity_time_valid + || !condition_angular_velocity_finite; - if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) { + if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) { const char err_str[] {"angular velocity no longer valid"}; - if (!condition_angular_velocity_time_valid) { + if (!condition_angular_velocity_time_valid && angular_velocity.timestamp != 0) { PX4_ERR("%s (timeout)", err_str); } else if (!condition_angular_velocity_finite) { @@ -768,7 +768,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } } - failsafe_flags.angular_velocity_valid = angular_velocity_valid; + failsafe_flags.angular_velocity_invalid = angular_velocity_invalid; // gps @@ -782,10 +782,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL); _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now); - failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state(); + failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state(); } else { - failsafe_flags.gps_position_valid = false; + failsafe_flags.gps_position_invalid = true; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp index b1a36541a2..3fcd2c6644 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp @@ -59,7 +59,7 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) } if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL - && !reporter.failsafeFlags().home_position_valid) { + && reporter.failsafeFlags().home_position_invalid) { /* EVENT * @description * diff --git a/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp index ac86fa0b44..d3df02328c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp @@ -38,10 +38,10 @@ void HomePositionChecks::checkAndReport(const Context &context, Report &reporter home_position_s home_position; if (_home_position_sub.copy(&home_position)) { - reporter.failsafeFlags().home_position_valid = home_position.valid_alt && home_position.valid_hpos; + reporter.failsafeFlags().home_position_invalid = !home_position.valid_alt || !home_position.valid_hpos; } else { - reporter.failsafeFlags().home_position_valid = false; + reporter.failsafeFlags().home_position_invalid = true; } // No need to report, as it's a mode requirement diff --git a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp index 39abe81401..fd94fdbf58 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp @@ -36,6 +36,7 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter) { reporter.failsafeFlags().mission_failure = false; + reporter.failsafeFlags().auto_mission_missing = true; mission_result_s mission_result; if (_mission_result_sub.copy(&mission_result) && mission_result.valid) { @@ -53,5 +54,8 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter) mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t"); } } + + // This is a mode requirement, no need to report + reporter.failsafeFlags().auto_mission_missing = mission_result.instance_count <= 0; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 0f1fdfc1df..820d2e27d8 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -42,7 +42,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) // Failing mode requirements generally also clear the can_run bits which prevents mode switching and // might trigger a failsafe if already in that mode. - if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) { + if (reporter.failsafeFlags().angular_velocity_invalid && reporter.failsafeFlags().mode_req_angular_velocity != 0) { /* EVENT * @description * Make sure the gyroscope is providing valid data. @@ -53,7 +53,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity); } - if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) { + if (reporter.failsafeFlags().attitude_invalid && reporter.failsafeFlags().mode_req_attitude != 0) { /* EVENT * @description * Wait until the estimator initialized @@ -66,11 +66,11 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) NavModes local_position_modes = NavModes::None; - if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) { + if (reporter.failsafeFlags().local_position_invalid && reporter.failsafeFlags().mode_req_local_position != 0) { local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position; } - if (!reporter.failsafeFlags().local_position_valid_relaxed + if (reporter.failsafeFlags().local_position_invalid_relaxed && reporter.failsafeFlags().mode_req_local_position_relaxed != 0) { local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed; } @@ -84,7 +84,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits(local_position_modes); } - if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) { + if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) { /* EVENT */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system, @@ -93,7 +93,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position); } - if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) { + if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) { /* EVENT */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system, @@ -102,12 +102,22 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt); } - if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) { + NavModes mission_required_modes = (NavModes)reporter.failsafeFlags().mode_req_mission; + + if (_param_com_arm_mis_req.get()) { + mission_required_modes = NavModes::All; + } + + if (reporter.failsafeFlags().auto_mission_missing && mission_required_modes != NavModes::None) { /* EVENT * @description * Upload a mission first. + * + * + * This check can be configured via COM_ARM_MIS_REQ parameter. + * */ - reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system, + reporter.armingCheckFailure(mission_required_modes, health_component_t::system, events::ID("check_modes_mission"), events::Log::Info, "No valid mission available"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission); @@ -116,7 +126,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) { /* EVENT * @description - * The offboard component is not sending setpoints. + * The offboard component is not sending setpoints or the required estimate (e.g. position) is missing. */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system, events::ID("check_modes_offboard_signal"), @@ -124,7 +134,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal); } - if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) { + if (reporter.failsafeFlags().home_position_invalid && reporter.failsafeFlags().mode_req_home_position != 0) { /* EVENT */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp index dd7df427e8..6ab8db25d3 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp @@ -48,4 +48,7 @@ public: private: void checkArmingRequirement(const Context &context, Report &reporter); + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_arm_mis_req + ); }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp new file mode 100644 index 0000000000..1609da7af4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "offboardCheck.hpp" + +using namespace time_literals; + +OffboardChecks::OffboardChecks() +{ + _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); +} + +void OffboardChecks::checkAndReport(const Context &context, Report &reporter) +{ + reporter.failsafeFlags().offboard_control_signal_lost = true; + + offboard_control_mode_s offboard_control_mode; + + if (_offboard_control_mode_sub.copy(&offboard_control_mode)) { + bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity + || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate + || offboard_control_mode.actuator; + + if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) { + offboard_available = false; + + } else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) { + offboard_available = false; + + } else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) { + // OFFBOARD acceleration handled by position controller + offboard_available = false; + } + + _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); + + // This is a mode requirement, no need to report + reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state(); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp new file mode 100644 index 0000000000..5d29bc75c2 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include +#include + +class OffboardChecks : public HealthAndArmingCheckBase +{ +public: + OffboardChecks(); + ~OffboardChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + systemlib::Hysteresis _offboard_available{false}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_com_of_loss_t + ); +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index f827d0b229..5d3a26d95b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -72,29 +72,9 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // Arm Requirements: mission - if (_param_com_arm_mis_req.get() && !context.isArmed()) { - if (!context.status().auto_mission_available) { - /* EVENT - * @description - * - * This check can be configured via COM_ARM_MIS_REQ parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"), - events::Log::Error, "No valid mission"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission"); - } - } - } - - reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available; - // Global position required if (!_param_com_arm_wo_gps.get() && !context.isArmed()) { - if (!reporter.failsafeFlags().global_position_valid) { + if (reporter.failsafeFlags().global_position_invalid) { /* EVENT * @description * @@ -108,7 +88,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required"); } - } else if (!reporter.failsafeFlags().home_position_valid) { + } else if (reporter.failsafeFlags().home_position_invalid) { /* EVENT * @description * diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp index e4dd148912..e7b8dafb40 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp @@ -52,7 +52,6 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_cbrk_vtolarming, (ParamInt) _param_cbrk_usb_chk, - (ParamBool) _param_com_arm_mis_req, (ParamBool) _param_com_arm_wo_gps, (ParamInt) _param_com_arm_auth_req, (ParamInt) _param_gf_action diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index ee23ad3994..bd4e2887bd 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation() eph = gpos.eph; epv = gpos.epv; - } else if (_vehicle_status_flags.gps_position_valid) { + } else if (!_vehicle_status_flags.gps_position_invalid) { sensor_gps_s gps; _vehicle_gps_position_sub.copy(&gps); const double lat = static_cast(gps.lat) * 1e-7; @@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force) bool updated = false; home_position_s home{}; - if (_vehicle_status_flags.local_position_valid) { + if (!_vehicle_status_flags.local_position_invalid) { // Set home position in local coordinates const vehicle_local_position_s &lpos = _local_position_sub.get(); _heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here @@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force) updated = true; } - if (_vehicle_status_flags.global_position_valid) { + if (!_vehicle_status_flags.global_position_invalid) { // Set home using the global position estimate (fused INS/GNSS) const vehicle_global_position_s &gpos = _global_position_sub.get(); fillGlobalHomePos(home, gpos); setHomePosValid(); updated = true; - } else if (_vehicle_status_flags.gps_position_valid) { + } else if (!_vehicle_status_flags.gps_position_invalid) { // Set home using GNSS position sensor_gps_s gps_pos; _vehicle_gps_position_sub.copy(&gps_pos); @@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition() const bool local_home_valid = home.valid_lpos; if (local_home_valid && !global_home_valid) { - if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) { + if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.global_position_invalid) { // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS fused) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); @@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition() home.timestamp = hrt_absolute_time(); _home_position_pub.update(); - } else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) { + } else if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.gps_position_invalid) { // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS raw) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); @@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition() } else if (!local_home_valid && global_home_valid) { const vehicle_local_position_s &lpos = _local_position_sub.get(); - if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) { + if (!_vehicle_status_flags.local_position_invalid && lpos.xy_global && lpos.z_global) { // Back-compute x, y and z of home position given the global home position // and the global reference of the local frame MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon}; @@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) } if (check_if_changed && set_automatically) { - const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid; + const bool can_set_home_lpos_first_time = !home.valid_lpos && !_vehicle_status_flags.local_position_invalid; const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt) - && (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid)); + && (!_vehicle_status_flags.global_position_invalid || !_vehicle_status_flags.gps_position_invalid)); const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global); if (can_set_home_lpos_first_time diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 5a1b6e0e7b..c50ca11000 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -615,14 +615,14 @@ bool FailsafeBase::modeCanRun(const vehicle_status_flags_s &status_flags, uint8_ { uint32_t mode_mask = 1u << mode; return - (status_flags.angular_velocity_valid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) && - (status_flags.attitude_valid || ((status_flags.mode_req_attitude & mode_mask) == 0)) && - (status_flags.local_position_valid || ((status_flags.mode_req_local_position & mode_mask) == 0)) && - (status_flags.local_position_valid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) && - (status_flags.global_position_valid || ((status_flags.mode_req_global_position & mode_mask) == 0)) && - (status_flags.local_altitude_valid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) && - (status_flags.auto_mission_available || ((status_flags.mode_req_mission & mode_mask) == 0)) && + (!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) && + (!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) && + (!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) && + (!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) && + (!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) && + (!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) && + (!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) && (!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) && - (status_flags.home_position_valid || ((status_flags.mode_req_home_position & mode_mask) == 0)) && + (!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) && ((status_flags.mode_req_other & mode_mask) == 0); } diff --git a/src/modules/events/rc_loss_alarm.cpp b/src/modules/events/rc_loss_alarm.cpp index a1df1c38ba..128f4fb044 100644 --- a/src/modules/events/rc_loss_alarm.cpp +++ b/src/modules/events/rc_loss_alarm.cpp @@ -58,18 +58,22 @@ void RC_Loss_Alarm::process() return; } + vehicle_status_flags_s status_flags{}; + + _vehicle_status_flags_sub.copy(&status_flags); + if (!_was_armed && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { _was_armed = true; // Once true, impossible to go back to false } - if (!_had_rc && !status.rc_signal_lost) { + if (!_had_rc && !status_flags.rc_signal_lost) { _had_rc = true; } - if (_was_armed && _had_rc && status.rc_signal_lost && + if (_was_armed && _had_rc && status_flags.rc_signal_lost && status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { play_tune(); _alarm_playing = true; diff --git a/src/modules/events/rc_loss_alarm.h b/src/modules/events/rc_loss_alarm.h index 861ced0511..38cdf876fd 100644 --- a/src/modules/events/rc_loss_alarm.h +++ b/src/modules/events/rc_loss_alarm.h @@ -42,6 +42,7 @@ #include #include #include +#include #include namespace events @@ -67,6 +68,7 @@ private: uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; bool _was_armed = false; bool _had_rc = false; // Don't trigger alarm for systems without RC diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index b0711fa30d..f272a87b92 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -54,8 +54,8 @@ namespace status void StatusDisplay::set_leds() { - bool gps_lock_valid = _vehicle_status_flags_sub.get().global_position_valid; - bool home_position_valid = _vehicle_status_flags_sub.get().home_position_valid; + bool gps_lock_valid = !_vehicle_status_flags_sub.get().global_position_invalid; + bool home_position_valid = !_vehicle_status_flags_sub.get().home_position_invalid; int nav_state = _vehicle_status_sub.get().nav_state; #if defined(BOARD_FRONT_LED_MASK) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index f0104a7612..5988acf2fc 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -72,10 +72,10 @@ bool Sticks::checkAndUpdateStickInputs() _input_available = valid_sticks; } else { - vehicle_status_s vehicle_status; + vehicle_status_flags_s vehicle_status_flags; - if (_vehicle_status_sub.update(&vehicle_status)) { - if (vehicle_status.rc_signal_lost) { + if (_vehicle_status_flags_sub.update(&vehicle_status_flags)) { + if (vehicle_status_flags.rc_signal_lost) { _input_available = false; } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index 723375f379..32ce1b6196 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include class Sticks : public ModuleParams { @@ -92,7 +92,7 @@ private: matrix::Vector _positions_expo; ///< modified manual sticks using expo function uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_hold_dz, diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index a832b3bfba..fc300bc4cc 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -86,6 +86,12 @@ void GyroCalibration::Run() Reset(); return; } + + if (_system_calibrating != vehicle_status.calibration_enabled) { + _system_calibrating = vehicle_status.calibration_enabled; + Reset(); + return; + } } } @@ -94,18 +100,6 @@ void GyroCalibration::Run() return; } - if (_vehicle_status_flags_sub.updated()) { - vehicle_status_flags_s vehicle_status_flags; - - if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { - if (_system_calibrating != vehicle_status_flags.calibration_enabled) { - _system_calibrating = vehicle_status_flags.calibration_enabled; - Reset(); - return; - } - } - } - if (_system_calibrating) { // do nothing if system is calibrating Reset(); diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp index a16e83c4a2..10d922bb50 100644 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -49,7 +49,6 @@ #include #include #include -#include using namespace time_literals; @@ -87,7 +86,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status}; - uORB::Subscription _vehicle_status_flags_sub{ORB_ID::vehicle_status_flags}; uORB::SubscriptionMultiArray _sensor_accel_subs{ORB_ID::sensor_accel}; uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index f980c0bbd7..d8fc118b0e 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -101,6 +101,16 @@ void MagBiasEstimator::Run() ScheduleOnInterval(20_ms); } } + + bool system_calibrating = vehicle_status.calibration_enabled; + + if (system_calibrating != _system_calibrating) { + _system_calibrating = system_calibrating; + + for (auto &reset : _reset_field_estimator) { + reset = true; + } + } } } @@ -130,22 +140,6 @@ void MagBiasEstimator::Run() } } - if (_vehicle_status_flags_sub.updated()) { - vehicle_status_flags_s vehicle_status_flags; - - if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { - bool system_calibrating = vehicle_status_flags.calibration_enabled; - - if (system_calibrating != _system_calibrating) { - _system_calibrating = system_calibrating; - - for (auto &reset : _reset_field_estimator) { - reset = true; - } - } - } - } - // do nothing during regular sensor calibration if (_system_calibrating) { return; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp index 3d0723ac0d..d47e7d0a8f 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.hpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.hpp @@ -51,7 +51,6 @@ #include #include #include -#include namespace mag_bias_estimator { @@ -91,7 +90,6 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; uORB::Publication _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)}; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 4da54af5a9..26d966a85a 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -37,7 +37,6 @@ #include #include #include -#include class MavlinkStreamHeartbeat : public MavlinkStream { @@ -63,7 +62,6 @@ private: uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; bool send() override { @@ -72,9 +70,6 @@ private: vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); - vehicle_status_flags_s vehicle_status_flags{}; - _vehicle_status_flags_sub.copy(&vehicle_status_flags); - vehicle_control_mode_s vehicle_control_mode{}; _vehicle_control_mode_sub.copy(&vehicle_control_mode); @@ -131,7 +126,7 @@ private: || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { system_status = MAV_STATE_FLIGHT_TERMINATION; - } else if (vehicle_status_flags.calibration_enabled) { + } else if (vehicle_status.calibration_enabled) { system_status = MAV_STATE_CALIBRATING; } diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 77f7b96902..e5863f585d 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -429,10 +429,6 @@ private: } } - if (status.rc_signal_lost) { - msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; - } - if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) { msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; } @@ -452,7 +448,7 @@ private: vehicle_status_flags_s status_flags; if (_status_flags_sub.update(&status_flags)) { - if (!status_flags.global_position_valid) { //TODO check if there is a better way to get only GPS failure + if (status_flags.gps_position_invalid) { msg->failure_flags |= HL_FAILURE_FLAG_GPS; } @@ -464,6 +460,11 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_MISSION; } + if (status_flags.rc_signal_lost) { + msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + return true; }