ekf2: gravity fusion updates

- only fuse gravity if no horizontal aiding source is enabled
 - add estimator_aid_src_gravity for inspection and logging
 - estimator_aid_src_gravity is always updated and published, even if not fused
 - add innovation gate with conservative default (could be made configurable)
 - use new EKF2_IMU_CTRL parameter to enable/disable gravity vector fusion

.vscode/settings.json

@@ -2,6 +2,7 @@
         "astyle.astylerc": "${workspaceFolder}/Tools/astyle/astylerc",
         "astyle.c.enable": true,
         "astyle.cpp.enable": true,
+        "breadcrumbs.enabled": true,
         "C_Cpp.autoAddFileAssociations": false,
         "C_Cpp.clang_format_fallbackStyle": "none",
         "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": true,
@@ -19,6 +20,7 @@
         "cmakeExplorer.buildDir": "${workspaceFolder}/build/px4_sitl_test",
         "cmakeExplorer.parallelJobs": 1,
         "cmakeExplorer.suiteDelimiter": "-",
+        "cortex-debug.enableTelemetry": false,
         "cSpell.allowCompoundWords": true,
         "cSpell.diagnosticLevel": "Hint",
         "cSpell.showStatus": false,
@@ -29,6 +31,7 @@
         ],
         "debug.toolBarLocation": "docked",
         "editor.defaultFormatter": "chiehyu.vscode-astyle",
+        "editor.dragAndDrop": false,
         "editor.insertSpaces": false,
         "editor.minimap.maxColumn": 120,
         "editor.minimap.renderCharacters": false,
@@ -124,7 +127,12 @@
                 "${workspaceFolder}/build": true
         },
         "search.showLineNumbers": true,
-        "terminal.integrated.scrollback": 15000,
+        "telemetry.enableTelemetry": false,
+        "terminal.integrated.scrollback": 5000,
+        "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}",
+        "workbench.editor.highlightModifiedTabs": true,
+        "workbench.enableExperiments": false,
+        "workbench.settings.enableNaturalLanguageSearch": false,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         }

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -49,7 +49,7 @@ param set-default CA_AIRFRAME 4
 
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_PX 0
-param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_PY 2
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX 0
 param set-default CA_ROTOR1_PY -1

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -13,7 +13,7 @@ param set-default CA_AIRFRAME 4
 
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1
-param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_PY 2
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -54,9 +54,6 @@ param set-default RWTO_TKOFF 1
 
 param set-default CA_AIRFRAME 1
 
-param set-default COM_PREARM_MODE 2
-param set-default CBRK_AIRSPD_CHK 162128
-
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
 

Tools/simulation/gz/models/rc_cessna/model.sdf

@@ -685,7 +685,6 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_0</joint_name>
       <sub_topic>servo_0</sub_topic>
-      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.05984281113</a0>
@@ -709,7 +708,6 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_1</joint_name>
       <sub_topic>servo_1</sub_topic>
-      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.05984281113</a0>
@@ -769,7 +767,6 @@
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>servo_2</joint_name>
       <sub_topic>servo_2</sub_topic>
-      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
       <a0>0.0</a0>
@@ -792,8 +789,6 @@
     <plugin
       filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
       <joint_name>rudder_joint</joint_name>
-      <sub_topic>servo_3</sub_topic>
-      <p_gain>10.0</p_gain>
     </plugin>
     <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
       <jointName>rotor_puller_joint</jointName>

msg/VehicleCommand.msg

@@ -84,7 +84,6 @@ uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    		# Request to send a single instanc
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530		# Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531		# Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
-uint16 VEHICLE_CMD_SET_AT_PARAM = 550			# Set AT command for SiK radio
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 	# Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 	# Gimbal configuration to set which sysid/compid is in primary and secondary control
 uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   	# Start image capture sequence.

platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 add_subdirectory(../stm32_common/io_pins io_pins)
-add_subdirectory(../stm32_common/spi spi)
 add_subdirectory(../stm32_common/hrt hrt)
 add_subdirectory(../stm32_common/board_critmon board_critmon)
 add_subdirectory(../stm32_common/board_reset board_reset)

src/lib/sensor_calibration/Accelerometer.hpp

@@ -87,8 +87,7 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
-		return _offset + (_rotation.I() * bias).edivide(_scale);
+		return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
 	}
 
 	bool ParametersLoad();

src/lib/sensor_calibration/Gyroscope.hpp

@@ -91,8 +91,7 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		// updated calibration offset = existing offset + bias rotated to sensor frame
-		return _offset + (_rotation.I() * bias);
+		return (_rotation.I() * bias) + _thermal_offset + _offset;
 	}
 
 	bool ParametersLoad();

src/lib/sensor_calibration/Magnetometer.hpp

@@ -89,8 +89,7 @@ public:
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
-		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
-		return _offset + (_scale.I() * _rotation.I() * bias);
+		return _scale.I() * _rotation.I() * bias + _offset;
 	}
 
 	bool ParametersLoad();

src/modules/commander/Commander.cpp

@@ -1713,14 +1713,9 @@ void Commander::run()
 
 			perf_begin(_preflight_check_perf);
 			_health_and_arming_checks.update();
-			bool pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
-
-			if (_vehicle_status.pre_flight_checks_pass != pre_flight_checks_pass) {
-				_vehicle_status.pre_flight_checks_pass = pre_flight_checks_pass;
-				_status_changed = true;
-			}
-
+			_vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
 			perf_end(_preflight_check_perf);
+
 			checkAndInformReadyForTakeoff();
 		}
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/computy_gravity_innov_var_and_k_and_h.h

@@ -0,0 +1,276 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: computy_gravity_innov_var_and_k_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     meas: Matrix31
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     innov: Matrix31
+ *     innov_var: Matrix31
+ *     Kx: Matrix24_1
+ *     Ky: Matrix24_1
+ *     Kz: Matrix24_1
+ */
+template <typename Scalar>
+void ComputyGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
+                                    const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
+                                    const Scalar epsilon,
+                                    matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
+                                    matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
+  // Total ops: 734
+
+  // Input arrays
+
+  // Intermediate terms (55)
+  const Scalar _tmp0 =
+      -Scalar(9.8066499999999994) *
+      std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
+                       std::pow(meas(2, 0), Scalar(2))));
+  const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0);
+  const Scalar _tmp2 = -P(3, 0) * _tmp1;
+  const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0);
+  const Scalar _tmp4 = P(0, 0) * _tmp3;
+  const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0);
+  const Scalar _tmp6 = P(2, 0) * _tmp5;
+  const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0);
+  const Scalar _tmp8 = P(3, 1) * _tmp1;
+  const Scalar _tmp9 = P(2, 1) * _tmp5;
+  const Scalar _tmp10 = -P(1, 1) * _tmp7;
+  const Scalar _tmp11 = P(0, 2) * _tmp3;
+  const Scalar _tmp12 = P(2, 2) * _tmp5;
+  const Scalar _tmp13 = -P(1, 2) * _tmp7;
+  const Scalar _tmp14 = -P(3, 3) * _tmp1;
+  const Scalar _tmp15 = P(0, 3) * _tmp3;
+  const Scalar _tmp16 = -P(1, 3) * _tmp7;
+  const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) +
+                        _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) +
+                        _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) -
+                        _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9);
+  const Scalar _tmp18 = P(3, 0) * _tmp3;
+  const Scalar _tmp19 = -P(0, 0) * _tmp1;
+  const Scalar _tmp20 = -P(1, 0) * _tmp5;
+  const Scalar _tmp21 = P(3, 2) * _tmp3;
+  const Scalar _tmp22 = -P(2, 2) * _tmp7;
+  const Scalar _tmp23 = P(1, 2) * _tmp5;
+  const Scalar _tmp24 = P(0, 1) * _tmp1;
+  const Scalar _tmp25 = -P(2, 1) * _tmp7;
+  const Scalar _tmp26 = -P(1, 1) * _tmp5;
+  const Scalar _tmp27 = -P(3, 3) * _tmp3;
+  const Scalar _tmp28 = -P(0, 3) * _tmp1;
+  const Scalar _tmp29 = -P(2, 3) * _tmp7;
+  const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) -
+                        _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) -
+                        _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) -
+                        _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23);
+  const Scalar _tmp31 = -P(0, 0) * _tmp5;
+  const Scalar _tmp32 = P(2, 0) * _tmp3;
+  const Scalar _tmp33 = P(1, 0) * _tmp1;
+  const Scalar _tmp34 = -P(3, 2) * _tmp7;
+  const Scalar _tmp35 = P(0, 2) * _tmp5;
+  const Scalar _tmp36 = P(2, 2) * _tmp3;
+  const Scalar _tmp37 = -P(3, 1) * _tmp7;
+  const Scalar _tmp38 = -P(0, 1) * _tmp5;
+  const Scalar _tmp39 = P(1, 1) * _tmp1;
+  const Scalar _tmp40 = -P(3, 3) * _tmp7;
+  const Scalar _tmp41 = P(2, 3) * _tmp3;
+  const Scalar _tmp42 = P(1, 3) * _tmp1;
+  const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) +
+                        _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) -
+                        _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) -
+                        _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42);
+  const Scalar _tmp44 = Scalar(1.0) / (_tmp17);
+  const Scalar _tmp45 = Scalar(19.613299999999999) * P(4, 0);
+  const Scalar _tmp46 = Scalar(19.613299999999999) * P(4, 2);
+  const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 3);
+  const Scalar _tmp48 = Scalar(19.613299999999999) * P(8, 0);
+  const Scalar _tmp49 = Scalar(19.613299999999999) * P(8, 1);
+  const Scalar _tmp50 = Scalar(19.613299999999999) * P(8, 2);
+  const Scalar _tmp51 = Scalar(19.613299999999999) * P(9, 2);
+  const Scalar _tmp52 = Scalar(19.613299999999999) * P(9, 0);
+  const Scalar _tmp53 = Scalar(1.0) / (_tmp30);
+  const Scalar _tmp54 = Scalar(1.0) / (_tmp43);
+
+  // Output terms (5)
+  if (innov != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
+
+    _innov(0, 0) = _tmp0 + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) -
+                   Scalar(19.613299999999999) * state(1, 0) * state(3, 0);
+    _innov(1, 0) = _tmp0 - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) -
+                   Scalar(19.613299999999999) * state(2, 0) * state(3, 0);
+    _innov(2, 0) = _tmp0 - Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) +
+                   Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) +
+                   Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) -
+                   Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2));
+  }
+
+  if (innov_var != nullptr) {
+    matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
+
+    _innov_var(0, 0) = _tmp17;
+    _innov_var(1, 0) = _tmp30;
+    _innov_var(2, 0) = _tmp43;
+  }
+
+  if (Kx != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
+
+    _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4);
+    _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42);
+    _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32);
+    _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37);
+    _kx(4, 0) =
+        _tmp44 * (-P(4, 1) * _tmp7 - P(4, 3) * _tmp1 + _tmp45 * state(2, 0) + _tmp46 * state(0, 0));
+    _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1);
+    _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1);
+    _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1);
+    _kx(8, 0) = _tmp44 * (-_tmp47 * state(1, 0) + _tmp48 * state(2, 0) - _tmp49 * state(3, 0) +
+                          _tmp50 * state(0, 0));
+    _kx(9, 0) =
+        _tmp44 * (-P(9, 1) * _tmp7 - P(9, 3) * _tmp1 + _tmp51 * state(0, 0) + _tmp52 * state(2, 0));
+    _kx(10, 0) =
+        _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1);
+    _kx(11, 0) =
+        _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1);
+    _kx(12, 0) =
+        _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1);
+    _kx(13, 0) =
+        _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1);
+    _kx(14, 0) =
+        _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1);
+    _kx(15, 0) =
+        _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1);
+    _kx(16, 0) =
+        _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1);
+    _kx(17, 0) =
+        _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1);
+    _kx(18, 0) =
+        _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1);
+    _kx(19, 0) =
+        _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1);
+    _kx(20, 0) =
+        _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1);
+    _kx(21, 0) =
+        _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1);
+    _kx(22, 0) =
+        _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1);
+    _kx(23, 0) =
+        _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1);
+  }
+
+  if (Ky != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
+
+    _ky(0, 0) = _tmp53 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38);
+    _ky(1, 0) = _tmp53 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33);
+    _ky(2, 0) = _tmp53 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9);
+    _ky(3, 0) = _tmp53 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34);
+    _ky(4, 0) = _tmp53 * (-P(4, 0) * _tmp1 - P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3);
+    _ky(5, 0) = _tmp53 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
+    _ky(6, 0) = _tmp53 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3);
+    _ky(7, 0) = _tmp53 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
+    _ky(8, 0) = _tmp53 * (-P(8, 2) * _tmp7 - _tmp47 * state(2, 0) - _tmp48 * state(1, 0) -
+                          _tmp49 * state(0, 0));
+    _ky(9, 0) =
+        _tmp53 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - P(9, 3) * _tmp3 - _tmp52 * state(1, 0));
+    _ky(10, 0) =
+        _tmp53 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
+    _ky(11, 0) =
+        _tmp53 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
+    _ky(12, 0) =
+        _tmp53 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
+    _ky(13, 0) =
+        _tmp53 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
+    _ky(14, 0) =
+        _tmp53 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
+    _ky(15, 0) =
+        _tmp53 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
+    _ky(16, 0) =
+        _tmp53 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
+    _ky(17, 0) =
+        _tmp53 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
+    _ky(18, 0) =
+        _tmp53 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
+    _ky(19, 0) =
+        _tmp53 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
+    _ky(20, 0) =
+        _tmp53 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
+    _ky(21, 0) =
+        _tmp53 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
+    _ky(22, 0) =
+        _tmp53 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
+    _ky(23, 0) =
+        _tmp53 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
+  }
+
+  if (Kz != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
+
+    _kz(0, 0) = _tmp54 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31);
+    _kz(1, 0) = _tmp54 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39);
+    _kz(2, 0) = _tmp54 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6);
+    _kz(3, 0) = _tmp54 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8);
+    _kz(4, 0) =
+        _tmp54 * (P(4, 1) * _tmp1 - P(4, 3) * _tmp7 - _tmp45 * state(0, 0) + _tmp46 * state(2, 0));
+    _kz(5, 0) = _tmp54 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7);
+    _kz(6, 0) = _tmp54 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7);
+    _kz(7, 0) = _tmp54 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7);
+    _kz(8, 0) = _tmp54 * (-P(8, 3) * _tmp7 - _tmp48 * state(0, 0) + _tmp49 * state(1, 0) +
+                          _tmp50 * state(2, 0));
+    _kz(9, 0) =
+        _tmp54 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp51 * state(2, 0) - _tmp52 * state(0, 0));
+    _kz(10, 0) =
+        _tmp54 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7);
+    _kz(11, 0) =
+        _tmp54 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7);
+    _kz(12, 0) =
+        _tmp54 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7);
+    _kz(13, 0) =
+        _tmp54 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7);
+    _kz(14, 0) =
+        _tmp54 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7);
+    _kz(15, 0) =
+        _tmp54 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7);
+    _kz(16, 0) =
+        _tmp54 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7);
+    _kz(17, 0) =
+        _tmp54 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7);
+    _kz(18, 0) =
+        _tmp54 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7);
+    _kz(19, 0) =
+        _tmp54 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7);
+    _kz(20, 0) =
+        _tmp54 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7);
+    _kz(21, 0) =
+        _tmp54 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7);
+    _kz(22, 0) =
+        _tmp54 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7);
+    _kz(23, 0) =
+        _tmp54 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7);
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/ekf2_params.c

@@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
  * @bit 1 Accel Bias
  * @bit 2 Gravity vector fusion
  */
-PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3);
+PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7);
 
 /**
  * Magnetometer measurement delay relative to IMU measurements

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -229,14 +229,12 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	}
 
 	if (_type_previous != WaypointType::land) {
-		// initialize yaw
+		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
+		_land_position = Vector3f(_target(0), _target(1), NAN);
 		_land_heading = _yaw_setpoint;
 		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
 	}
 
-	// Update xy-position in case of landing position changes (etc. precision landing)
-	_land_position = Vector3f(_target(0), _target(1), NAN);
-
 	// User input assisted landing
 	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed

src/modules/mavlink/mavlink_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 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
@@ -2299,14 +2299,7 @@ Mavlink::task_main(int argc, char *argv[])
 			mavlink_update_parameters();
 		}
 
-		if (_param_sik_radio_id.get() != 0) {
-			const uint8_t ret = configure_sik_radio((uint16_t)_param_sik_radio_id.get());
-
-			if (ret == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
-				_param_sik_radio_id.set(0);
-				_param_sik_radio_id.commit_no_notification();
-			}
-		}
+		configure_sik_radio();
 
 		if (_vehicle_status_sub.updated()) {
 			vehicle_status_s vehicle_status;
@@ -2336,77 +2329,47 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 
-		int vehicle_command_updates = 0;
+		// MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY
+		if (_mode == MAVLINK_MODE_IRIDIUM) {
+			int vehicle_command_updates = 0;
 
-		while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
-			vehicle_command_updates++;
-			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
-			vehicle_command_s vehicle_cmd;
+			while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
+				vehicle_command_updates++;
+				const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+				vehicle_command_s vehicle_cmd;
 
-			if (_vehicle_command_sub.update(&vehicle_cmd)) {
-				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
-					PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
-				}
-
-				if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
-				    _mode == MAVLINK_MODE_IRIDIUM) {
-
-					if (vehicle_cmd.param1 > 0.5f) {
-						if (!_transmitting_enabled) {
-							mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t",
-									 _device_name);
-							events::send<int8_t>(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info,
-									     "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
-						}
-
-						_transmitting_enabled = true;
-						_transmitting_enabled_commanded = true;
-
-					} else {
-						if (_transmitting_enabled) {
-							mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t",
-									 _device_name);
-							events::send<int8_t>(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info,
-									     "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
-						}
-
-						_transmitting_enabled = false;
-						_transmitting_enabled_commanded = false;
+				if (_vehicle_command_sub.update(&vehicle_cmd)) {
+					if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+						PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
 					}
 
-					// send positive command ack
-					vehicle_command_ack_s command_ack{};
-					command_ack.command = vehicle_cmd.command;
-					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-					command_ack.from_external = !vehicle_cmd.from_external;
-					command_ack.target_system = vehicle_cmd.source_system;
-					command_ack.target_component = vehicle_cmd.source_component;
-					command_ack.timestamp = vehicle_cmd.timestamp;
-					_vehicle_command_ack_pub.publish(command_ack);
+					if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
+					    _mode == MAVLINK_MODE_IRIDIUM) {
 
-				} else if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_SET_AT_PARAM) {
+						if (vehicle_cmd.param1 > 0.5f) {
+							if (!_transmitting_enabled) {
+								mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t",
+										 _device_name);
+								events::send<int8_t>(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info,
+										     "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
+							}
 
+							_transmitting_enabled = true;
+							_transmitting_enabled_commanded = true;
 
-					// We only support ATS3 to set NET_ID and setting it for all radios.
-					if ((int)(vehicle_cmd.param2 + 0.5f) != 3 && (int)(vehicle_cmd.param1 + 0.5f) == 0) {
-						vehicle_command_ack_s command_ack{};
-						command_ack.command = vehicle_cmd.command;
-						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-						command_ack.from_external = !vehicle_cmd.from_external;
-						command_ack.target_system = vehicle_cmd.source_system;
-						command_ack.target_component = vehicle_cmd.source_component;
-						command_ack.timestamp = vehicle_cmd.timestamp;
-						_vehicle_command_ack_pub.publish(command_ack);
+						} else {
+							if (_transmitting_enabled) {
+								mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t",
+										 _device_name);
+								events::send<int8_t>(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info,
+										     "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
+							}
 
-					} else if (!_radio_status_available) {
-						// Only respond if we have an SiK radio detected, otherwise just stay silent.
-						// If we nacked here with DENIED or UNSUPPORTED we would cause multiple
-						// acks/nacks to be sent back which would be confusing.
-
-					} else {
-						// Since this command might take several seconds, we need to send an
-						// IN_PROGRESS ack immediately, and the final result later.
+							_transmitting_enabled = false;
+							_transmitting_enabled_commanded = false;
+						}
 
+						// send positive command ack
 						vehicle_command_ack_s command_ack{};
 						command_ack.command = vehicle_cmd.command;
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -2415,10 +2378,6 @@ Mavlink::task_main(int argc, char *argv[])
 						command_ack.target_component = vehicle_cmd.source_component;
 						command_ack.timestamp = vehicle_cmd.timestamp;
 						_vehicle_command_ack_pub.publish(command_ack);
-
-						command_ack.result = configure_sik_radio((uint16_t)(vehicle_cmd.param3 + 0.5f));
-						command_ack.timestamp = vehicle_cmd.timestamp;
-						_vehicle_command_ack_pub.publish(command_ack);
 					}
 				}
 			}
@@ -2785,56 +2744,56 @@ void Mavlink::publish_telemetry_status()
 	_tstatus_updated = false;
 }
 
-uint8_t Mavlink::configure_sik_radio(uint16_t netid)
+void Mavlink::configure_sik_radio()
 {
-	if (_uart_fd < 0) {
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+	/* radio config check */
+	if (_uart_fd >= 0 && _param_sik_radio_id.get() != 0) {
+		/* request to configure radio and radio is present */
+		FILE *fs = fdopen(_uart_fd, "w");
+
+		if (fs) {
+			/* switch to AT command mode */
+			px4_usleep(1200000);
+			fprintf(fs, "+++");
+			fflush(fs);
+			px4_usleep(1200000);
+
+			if (_param_sik_radio_id.get() > 0) {
+				/* set channel */
+				fprintf(fs, "ATS3=%" PRIu32 "\r\n", _param_sik_radio_id.get());
+				px4_usleep(200000);
+
+			} else {
+				/* reset to factory defaults */
+				fprintf(fs, "AT&F\r\n");
+				px4_usleep(200000);
+			}
+
+			/* write config */
+			fprintf(fs, "AT&W\r\n");
+			px4_usleep(200000);
+
+			/* reboot */
+			fprintf(fs, "ATZ\r\n");
+			px4_usleep(200000);
+
+			// XXX NuttX suffers from a bug where
+			// fclose() also closes the fd, not just
+			// the file stream. Since this is a one-time
+			// config thing, we leave the file struct
+			// allocated.
+#ifndef __PX4_NUTTX
+			fclose(fs);
+#endif
+
+		} else {
+			PX4_WARN("open fd %d failed", _uart_fd);
+		}
+
+		/* reset param and save */
+		_param_sik_radio_id.set(0);
+		_param_sik_radio_id.commit_no_notification();
 	}
-
-	LockGuard lg{_send_mutex};
-
-	// it doesn't seem to switch without this wait
-	px4_usleep(1200000);
-	// switch to AT command mode
-	const char at_command_mode[] = "+++";
-	_receiver.wait_for_ok();
-	// we don't write the 0 termination, otherwise it doesn't seem to work.
-	write(_uart_fd, at_command_mode, 3);
-	fsync(_uart_fd);
-
-	if (!wait_for_ok(2000000)) {
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
-	}
-
-	// set channel
-	char id_setting[20] {};
-	snprintf(id_setting, sizeof(id_setting), "ATS3=%" PRIu16 "\r\n", netid);
-	_receiver.wait_for_ok();
-	write(_uart_fd, id_setting, sizeof(id_setting));
-	fsync(_uart_fd);
-
-	if (!wait_for_ok(500000)) {
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
-	}
-
-	// write config
-	const char write_config[] = "AT&W\r\n\0";
-	_receiver.wait_for_ok();
-	write(_uart_fd, write_config, sizeof(write_config));
-	fsync(_uart_fd);
-	px4_usleep(200000);
-
-	if (!wait_for_ok(200000)) {
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
-	}
-
-	// reboot
-	const char reboot[] = "ATZ\r\n\0";
-	_receiver.wait_for_ok();
-	write(_uart_fd, reboot, sizeof(reboot));
-	fsync(_uart_fd);
-
-	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 }
 
 int Mavlink::start_helper(int argc, char *argv[])
@@ -3323,22 +3282,6 @@ Mavlink::set_boot_complete()
 
 }
 
-bool Mavlink::wait_for_ok(unsigned timeout_us)
-{
-	hrt_abstime now = hrt_absolute_time();
-
-	while (hrt_elapsed_time(&now) < timeout_us) {
-
-		if (_receiver.got_ok()) {
-			return true;
-		}
-
-		px4_usleep(10000);
-	}
-
-	return false;
-}
-
 static void usage()
 {
 

src/modules/mavlink/mavlink_main.h

@@ -729,8 +729,13 @@ private:
 
 	void check_requested_subscriptions();
 
-	uint8_t configure_sik_radio(uint16_t netid);
-	bool wait_for_ok(unsigned timeout_us);
+	/**
+	 * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID
+	 *
+	 * This convenience function allows to re-configure a connected
+	 * SiK radio without removing it from the main system harness.
+	 */
+	void configure_sik_radio();
 
 	/**
 	 * Update rate mult so total bitrate will be equal to _datarate.

src/modules/mavlink/mavlink_params.c

@@ -62,10 +62,12 @@ PARAM_DEFINE_INT32(MAV_PROTO_VER, 0);
  * MAVLink SiK Radio ID
  *
  * When non-zero the MAVLink app will attempt to configure the
- * SiK radio to this ID and re-set the parameter to 0. Only applies if this mavlink instance is going through a SiK radio
+ * SiK radio to this ID and re-set the parameter to 0. If the value
+ * is negative it will reset the complete radio config to
+ * factory defaults. Only applies if this mavlink instance is going through a SiK radio
  *
  * @group MAVLink
- * @min 0
+ * @min -1
  * @max 240
  */
 PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0);

src/modules/mavlink/mavlink_receiver.cpp

@@ -3213,13 +3213,6 @@ MavlinkReceiver::run()
 				/* non-blocking read. read may return negative values */
 				nread = ::read(fds[0].fd, buf, sizeof(buf));
 
-				parse_for_ok(nread, (const char *)buf);
-
-				if (waiting_for_ok()) {
-					// Don't bother with the rest at the moment, we only wait for an ok.
-					continue;
-				}
-
 				if (nread == -1 && errno == ENOTCONN) { // Not connected (can happen for USB)
 					usleep(100000);
 				}
@@ -3586,62 +3579,3 @@ void MavlinkReceiver::stop()
 	_should_exit.store(true);
 	pthread_join(_thread, nullptr);
 }
-
-void MavlinkReceiver::parse_for_ok(int nread, const char *buf)
-{
-	if (nread < 1) {
-		return;
-	}
-
-	for (int i = 0; i < nread; ++i) {
-		switch (_parse_state) {
-		case OkParseState::None:
-			// Nothing to do.
-			return;
-
-		case OkParseState::Waiting:
-			if (buf[i] == 'O') {
-				_parse_state = OkParseState::GotO;
-			}
-
-			break;
-
-		case OkParseState::GotO:
-			if (buf[i] == 'K') {
-				_parse_state = OkParseState::GotK;
-
-			} else {
-				_parse_state = OkParseState::None;
-			}
-
-			break;
-
-		case OkParseState::GotK:
-			break;
-
-		default:
-			break;
-
-
-		};
-	}
-}
-
-void MavlinkReceiver::wait_for_ok()
-{
-	_parse_state = OkParseState::Waiting;
-}
-
-bool MavlinkReceiver::got_ok()
-{
-	if (_parse_state == OkParseState::GotK) {
-		_parse_state = OkParseState::None;
-		return true;
-	}
-
-	return false;
-}
-bool MavlinkReceiver::waiting_for_ok() const
-{
-	return _parse_state == OkParseState::Waiting;
-}

src/modules/mavlink/mavlink_receiver.h

@@ -138,10 +138,6 @@ public:
 
 	void request_stop() { _should_exit.store(true); }
 
-	void wait_for_ok();
-	bool got_ok();
-	bool waiting_for_ok() const;
-
 private:
 	static void *start_trampoline(void *context);
 	void run();
@@ -247,8 +243,6 @@ private:
 	 */
 	void updateParams() override;
 
-	void parse_for_ok(int nread, const char *buf);
-
 	Mavlink				*_mavlink;
 
 	MavlinkFTP			_mavlink_ftp;
@@ -402,14 +396,6 @@ private:
 	hrt_abstime _heartbeat_component_udp_bridge{0};
 	hrt_abstime _heartbeat_component_uart_bridge{0};
 
-	enum class OkParseState {
-		None,
-		Waiting,
-		GotO,
-		GotK,
-	} _parse_state = OkParseState::None;
-
-
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,