feat(airframe): add EKF2 param and publish vehicle GPS position

- Introduce EKF2 parameter configuration for various airframes.
- Remove QEMU related checks from health and arming checks.
- Refactor failsafe conditions and add logging for mode fallback.
- Publish vehicle GPS position in ekf2_fake module.

apps/airframe/1100_sih_quad_x/1100_sih_quad_x.cpp

@@ -15,6 +15,9 @@ static param_config_s _params[] = {
  PARAM_CONFIG_INT32(SYS_HITL, 2),
  PARAM_CONFIG_INT32(SIH_VEHICLE_TYPE, 0),
 
+ // EKF2
+ PARAM_CONFIG_INT32(SENS_IMU_MODE, 1),
+
  // MAVLINK
  PARAM_CONFIG_INT32(MAV_TYPE, 2),
  PARAM_CONFIG_INT32(MAV_0_CONFIG, 0),

apps/airframe/1101_sih_plane_aert/1101_sih_plane_aert.cpp

@@ -15,6 +15,9 @@ static param_config_s _params[] = {
  PARAM_CONFIG_INT32(SYS_HITL, 2),
  PARAM_CONFIG_INT32(SIH_VEHICLE_TYPE, 1),
 
+ // EKF2
+ PARAM_CONFIG_INT32(SENS_IMU_MODE, 1),
+
  // MAVLINK
  PARAM_CONFIG_INT32(MAV_TYPE, 1),
  PARAM_CONFIG_INT32(MAV_0_CONFIG, 0),

apps/airframe/1102_sih_tailsitter_duo/1102_sih_tailsitter_duo.cpp

@@ -15,6 +15,9 @@ static param_config_t _params[] = {
  PARAM_CONFIG_INT32(SYS_HITL, 2),
  PARAM_CONFIG_INT32(SIH_VEHICLE_TYPE, 2),
 
+ // EKF2
+ PARAM_CONFIG_INT32(SENS_IMU_MODE, 1),
+
  // MAVLINK
  PARAM_CONFIG_INT32(MAV_TYPE, 19),
  PARAM_CONFIG_INT32(MAV_0_CONFIG, 0),

apps/airframe/1103_sih_standard_vtol/1103_sih_standard_vtol.cpp

@@ -15,6 +15,9 @@ static param_config_t _params[] = {
  PARAM_CONFIG_INT32(SYS_HITL, 2),
  PARAM_CONFIG_INT32(SIH_VEHICLE_TYPE, 3),
 
+ // EKF2
+ PARAM_CONFIG_INT32(SENS_IMU_MODE, 1),
+
  // MAVLINK
  PARAM_CONFIG_INT32(MAV_TYPE, 22),
  PARAM_CONFIG_INT32(MAV_0_CONFIG, 0),

apps/controller/commander/Commander.cpp

@@ -1636,6 +1636,7 @@ void Commander::Run() {
 
  const hrt_abstime now = hrt_absolute_time();
 
+ // 处理模式切换和失控保护
  const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
 
  // Run arming checks @ 10Hz

apps/controller/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -36,9 +36,7 @@ bool HealthAndArmingChecks::update(bool force_reporting) {
  break;
  }
 
-#ifndef BSP_USING_QEMU
  _checks[i]->checkAndReport(_context, _reporter);
-#endif /* BSP_USING_QEMU */
  }
 
  const bool results_changed = _reporter.finalize();
@@ -59,9 +57,7 @@ bool HealthAndArmingChecks::update(bool force_reporting) {
  break;
  }
 
-#ifndef BSP_USING_QEMU
  _checks[i]->checkAndReport(_context, _reporter);
-#endif /* BSP_USING_QEMU */
  }
 
  _reporter.finalize();

apps/controller/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp

@@ -17,6 +17,10 @@
 using namespace time_literals;
 
 void AccelerometerChecks::checkAndReport(const Context &context, Report &reporter) {
+#ifdef BSP_USING_QEMU
+ return;
+#endif /* BSP_USING_QEMU */
+
  for (int instance = 0; instance < _sensor_accel_sub.size(); instance++) {
  const bool is_required = instance == 0 || isAccelRequired(instance);
 

apps/controller/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp

@@ -16,6 +16,10 @@
 using namespace time_literals;
 
 void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) {
+#ifdef BSP_USING_QEMU
+ return;
+#endif /* BSP_USING_QEMU */
+
  if (_param_com_cpu_max.get() < FLT_EPSILON) {
  return;
  }

apps/controller/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -49,7 +49,9 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) {
  estimator_selector_status_s estimator_selector_status;
 
  if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
- bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance) && _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance) && _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
+ bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance)
+ && _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance)
+ && _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
 
  if (!instance_changed) {
  missing_data = true;

apps/controller/commander/HealthAndArmingChecks/checks/gyroCheck.cpp

@@ -17,6 +17,10 @@
 using namespace time_literals;
 
 void GyroChecks::checkAndReport(const Context &context, Report &reporter) {
+#ifdef BSP_USING_QEMU
+ return;
+#endif /* BSP_USING_QEMU */
+
  for (int instance = 0; instance < _sensor_gyro_sub.size(); instance++) {
  const bool is_required = instance == 0 || isGyroRequired(instance);
 

apps/controller/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp

@@ -17,6 +17,10 @@
 using namespace time_literals;
 
 void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter) {
+#ifdef BSP_USING_QEMU
+ return;
+#endif /* BSP_USING_QEMU */
+
  if (_param_sys_has_mag.get() == 0) {
  return;
  }

apps/controller/commander/HealthAndArmingChecks/checks/powerCheck.cpp

@@ -17,6 +17,10 @@
 using namespace time_literals;
 
 void PowerChecks::checkAndReport(const Context &context, Report &reporter) {
+#ifdef BSP_USING_QEMU
+ return;
+#endif /* BSP_USING_QEMU */
+
  if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) {
  return;
  }

apps/controller/commander/failsafe/failsafe.cpp

@@ -8,6 +8,9 @@
  * Copyright All Reserved © 2015-2024 NextPilot Development Team
  ******************************************************************/
 
+#define LOG_TAG "failsafe"
+#define LOG_LVL LOG_LVL_INFO
+
 #include "failsafe.h"
 #include <defines.h>
 #include <ulog/log.h>
@@ -312,31 +315,24 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
  const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
  const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
  const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
- const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) &&
- (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
- const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
- rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
+ const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
+ const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
 
  if (_param_com_rc_in_mode.get() != int32_t(offboard_loss_failsafe_mode::Land_mode) && !rc_loss_ignored) {
  CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
  fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
  }
 
  // GCS connection loss
- const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
+ const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
 
  if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !gcs_connection_loss_ignored) {
  CHECK_FAILSAFE(status_flags, gcs_connection_lost,
  fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
  }
 
  // VTOL transition failure (quadchute)
- if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
+ if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
  CHECK_FAILSAFE(status_flags, vtol_fixed_wing_system_failure, fromQuadchuteActParam(_param_com_qc_act.get()));
  }
 
@@ -357,8 +353,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
  CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());
 
  // trigger RTL if low position accurancy is detected
- if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
- state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+ if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
  CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
  }
 
@@ -411,9 +406,16 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
  // Mode fallback (last)
  Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode);
- _last_state_mode_fallback = checkFailsafe(_caller_id_mode_fallback, _last_state_mode_fallback,
- mode_fallback_action != Action::None,
- ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always).cannotBeDeferred());
+
+#if 0
+ if (mode_fallback_action != Action::None) {
+ LOG_W("mode_fallback_action: %d, state.user_intended_mode: %d", mode_fallback_action, state.user_intended_mode);
+ }
+#endif /* 0 */
+
+ _last_state_mode_fallback = checkFailsafe(_caller_id_mode_fallback, _last_state_mode_fallback,
+ mode_fallback_action != Action::None,
+ ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always).cannotBeDeferred());
 }
 
 void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags) {

apps/controller/commander/failsafe/framework.cpp

@@ -9,7 +9,7 @@
  ******************************************************************/
 
 #define LOG_TAG "cmder.framework"
-#define LOG_LVL LOG_LVL_INFO
+#define LOG_LVL LOG_LVL_DBG
 
 #include "framework.h"
 #define DEFINE_GET_CUSTOM_MODE
@@ -37,7 +37,8 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
  _last_update = time_us;
  }
 
- if ((_last_armed && !state.armed) || (!_last_armed && state.armed)) { // Disarming or Arming
+ // Disarming or Arming
+ if ((_last_armed && !state.armed) || (!_last_armed && state.armed)) {
  removeActions(ClearCondition::OnDisarm);
  removeActions(ClearCondition::OnModeChangeOrDisarm);
  _user_takeover_active = false;
@@ -49,7 +50,10 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
  removeActions(ClearCondition::OnModeChangeOrDisarm);
  }
 
- if (_defer_failsafes && _failsafe_defer_started != 0 && _defer_timeout > 0 && time_us > _failsafe_defer_started + _defer_timeout) {
+ if (_defer_failsafes
+ && _failsafe_defer_started != 0
+ && _defer_timeout > 0
+ && time_us > _failsafe_defer_started + _defer_timeout) {
  _defer_failsafes = false;
  }
 
@@ -69,6 +73,8 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
 
  // Notify user if the action is worse than before, or a new action got added
  if (action_state.action > _selected_action || (action_state.action != Action::None && _notification_required)) {
+ // LOG_W("a: %d, %d, %d", action_state.action, _selected_action, _notification_required);
+ // LOG_W("b: %d, %d, %d, %d", state.user_intended_mode, action_state.action, action_state.delayed_action, action_state.cause);
  notifyUser(state.user_intended_mode, action_state.action, action_state.delayed_action, action_state.cause);
  }
 
@@ -428,7 +434,11 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
  }
 
  // Check if we should enter delayed Hold
- if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly && selected_action != Action::Disarm && selected_action != Action::Terminate) {
+ if (_current_delay > 0
+ && !_user_takeover_active
+ && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly
+ && selected_action != Action::Disarm
+ && selected_action != Action::Terminate) {
  returned_state.delayed_action = selected_action;
  selected_action = Action::Hold;
  allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
@@ -608,7 +618,45 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
  uint32_t mode_mask = 1u << mode;
  // mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers)
  // mode_req_manual_control: is handled separately
- return (!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_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) && ((status_flags.mode_req_other & mode_mask) == 0);
+
+#if 0
+ bool ret_sets[10] = {};
+ ret_sets[0] = (!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0));
+ ret_sets[1] = (!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0));
+ ret_sets[2] = (!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0));
+ ret_sets[3] = (!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0));
+ ret_sets[4] = (!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0));
+ ret_sets[5] = (!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0));
+ ret_sets[6] = (!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0));
+ ret_sets[7] = (!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0));
+ ret_sets[8] = (!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0));
+ ret_sets[9] = ((status_flags.mode_req_other & mode_mask) == 0);
+ // 0 1 2 4 5
+ for (int ret_index = 0; ret_index < 10; ret_index++) {
+ // LOG_D("ret_sets[%d]: %d", ret_index, ret_sets[ret_index]);
+ if (ret_sets[ret_index] != 1) {
+ LOG_D("ret error index: %d", ret_index);
+ }
+ }
+ LOG_D("*******************************************");
+ LOG_D("mode_mask: 0x%x", mode_mask);
+ LOG_D("ret[0]: %d, %d", status_flags.angular_velocity_invalid, (status_flags.mode_req_angular_velocity & mode_mask) == 0);
+ LOG_D("ret[1]: %d, %d", status_flags.attitude_invalid, (status_flags.mode_req_attitude & mode_mask) == 0);
+ LOG_D("ret[2]: %d, %d", status_flags.local_position_invalid, (status_flags.mode_req_local_position & mode_mask) == 0);
+ LOG_D("ret[4]: %d, %d", status_flags.global_position_invalid, (status_flags.mode_req_global_position & mode_mask) == 0);
+ LOG_D("ret[5]: %d, %d", status_flags.local_altitude_invalid, (status_flags.mode_req_local_alt & mode_mask) == 0);
+#endif /* 0 */
+
+ return (!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_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0))
+ && ((status_flags.mode_req_other & mode_mask) == 0);
 }
 
 bool FailsafeBase::deferFailsafes(bool enabled, int timeout_s) {