mavlink: early return in parser

Signed-off-by: Julian Oes <julian@oes.ch>
mavlink: add MAV_SIK_RADIO_ID param back in
2026-06-13 19:10:04 +08:00 · 2023-02-09 21:06:16 +13:00 · 2023-02-09 21:05:44 +13:00 · 2023-02-08 13:15:33 +13:00 · 2023-02-07 13:57:10 -05:00 · 2023-02-07 13:28:58 -05:00
19 changed files with 257 additions and 391 deletions
@@ -2,7 +2,6 @@
        "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,
@@ -20,7 +19,6 @@
        "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,
@@ -31,7 +29,6 @@
        ],
        "debug.toolBarLocation": "docked",
        "editor.defaultFormatter": "chiehyu.vscode-astyle",
-        "editor.dragAndDrop": false,
        "editor.insertSpaces": false,
        "editor.minimap.maxColumn": 120,
        "editor.minimap.renderCharacters": false,
@@ -127,12 +124,7 @@
                "${workspaceFolder}/build": true
        },
        "search.showLineNumbers": true,
-        "telemetry.enableTelemetry": false,
-        "terminal.integrated.scrollback": 5000,
-        "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}",
-        "workbench.editor.highlightModifiedTabs": true,
-        "workbench.enableExperiments": false,
-        "workbench.settings.enableNaturalLanguageSearch": false,
+        "terminal.integrated.scrollback": 15000,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
        }
@@ -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 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX 0
 param set-default CA_ROTOR1_PY -1
@@ -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 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@@ -54,6 +54,9 @@ 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

@@ -685,6 +685,7 @@
      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>
@@ -708,6 +709,7 @@
      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>
@@ -767,6 +769,7 @@
      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>
@@ -789,6 +792,8 @@
    <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>
@@ -84,6 +84,7 @@ 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.
@@ -32,6 +32,7 @@
 ############################################################################

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

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

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

 	bool ParametersLoad();
@@ -1713,9 +1713,14 @@ void Commander::run()

 			perf_begin(_preflight_check_perf);
 			_health_and_arming_checks.update();
-			_vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
-			perf_end(_preflight_check_perf);
+			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;
+			}
+
+			perf_end(_preflight_check_perf);
 			checkAndInformReadyForTakeoff();
 		}

@@ -1,276 +0,0 @@
-// -----------------------------------------------------------------------------
-// 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
@@ -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, 7);
+PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3);

 /**
 * Magnetometer measurement delay relative to IMU measurements
@@ -229,12 +229,14 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	}

 	if (_type_previous != WaypointType::land) {
-		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
-		_land_position = Vector3f(_target(0), _target(1), NAN);
+		// initialize yaw
 		_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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2023 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,7 +2299,14 @@ Mavlink::task_main(int argc, char *argv[])
 			mavlink_update_parameters();
 		}

-		configure_sik_radio();
+		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();
+			}
+		}

 		if (_vehicle_status_sub.updated()) {
 			vehicle_status_s vehicle_status;
@@ -2329,47 +2336,77 @@ Mavlink::task_main(int argc, char *argv[])
 		}


-		// MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY
-		if (_mode == MAVLINK_MODE_IRIDIUM) {
-			int vehicle_command_updates = 0;
+		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_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.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_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);
 						}

-						// send positive command ack
+						_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;
+					}
+
+					// 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);
+
+				} else if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_SET_AT_PARAM) {
+
+
+					// 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 (!_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.
+
 						vehicle_command_ack_s command_ack{};
 						command_ack.command = vehicle_cmd.command;
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -2378,6 +2415,10 @@ 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);
 					}
 				}
 			}
@@ -2744,56 +2785,56 @@ void Mavlink::publish_telemetry_status()
 	_tstatus_updated = false;
 }

-void Mavlink::configure_sik_radio()
+uint8_t Mavlink::configure_sik_radio(uint16_t netid)
 {
-	/* 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();
+	if (_uart_fd < 0) {
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 	}
+
+	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[])
@@ -3282,6 +3323,22 @@ 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()
 {

@@ -729,13 +729,8 @@ private:

 	void check_requested_subscriptions();

-	/**
-	 * 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();
+	uint8_t configure_sik_radio(uint16_t netid);
+	bool wait_for_ok(unsigned timeout_us);

 	/**
 	 * Update rate mult so total bitrate will be equal to _datarate.
@@ -62,12 +62,10 @@ 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. 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
+ * SiK radio to this ID and re-set the parameter to 0. Only applies if this mavlink instance is going through a SiK radio
 *
 * @group MAVLink
- * @min -1
+ * @min 0
 * @max 240
 */
 PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0);
@@ -3213,6 +3213,13 @@ 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);
 				}
@@ -3579,3 +3586,62 @@ 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;
+}
@@ -138,6 +138,10 @@ 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();
@@ -243,6 +247,8 @@ private:
 	 */
 	void updateParams() override;

+	void parse_for_ok(int nread, const char *buf);
+
 	Mavlink				*_mavlink;

 	MavlinkFTP			_mavlink_ftp;
@@ -396,6 +402,14 @@ 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,
Author	SHA1	Message	Date
Julian Oes	1280a7f92d	mavlink: early return in parser Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-09 21:06:16 +13:00
Julian Oes	5f5cb3fac6	mavlink: add MAV_SIK_RADIO_ID param back in However, this is now without the functionality to reset to factory default. Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-09 21:05:44 +13:00
Julian Oes	7044301148	mavlink: use command to set SiK ID This removes the param MAV_SIK_RADIO_ID and replaces it with a MAVLink command to set the radio's net ID. Using a command has the benefit that we get feedback whether the change has been accepted. The code now also reads back the bytes after doing the config in order to check for the radio's OK feedback. Previously, the commands were just sent blindly and there is the chance they did not actually get through. Signed-off-by: Julian Oes <julian@oes.ch>	2023-02-08 13:15:33 +13:00
Daniel Agar	7b3befded5	ekf2: disable new gravity fusion by default	2023-02-07 13:57:10 -05:00
Daniel Sahu	fa6fda6cce	ekf2: new gravity observation (#21038 ) Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>	2023-02-07 13:28:58 -05:00
alessandro	3e149ee6c5	FlightTaskAuto: landing position updates for precision landing (#20951 ) - precision landing works incorrectly, target position is not updated during the descent above target - _prepareLandSetpoints needs to update _land_position continuously Co-authored-by: kapacheuski <kapacheuski@gmail.com>	2023-02-07 12:07:26 -05:00
Jukka Laitinen	deb6053d56	Update status leds every time when prearm check status changes Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-02-07 08:02:32 +01:00
Christian Rauch	ef5761c223	add SPI to stm32f1	2023-02-07 07:54:38 +01:00
Christian Rauch	2f21c590b0	remove some vscode settings	2023-02-06 17:25:36 -05:00
JaeyoungLim	01c5b3934e	Tune GZ plane (rc_cessna) to fly nicely (#21077 ) * Increase control surface joint controller gains * Enable prearm mode and disable airpspeed checks for gz plane	2023-02-06 16:24:01 -05:00
Daniel Agar	661eb2adb4	lib/sensor_calibration: BiasCorrectedSensorOffset() don't incorporate thermal offsets - the thermal offsets are an optional correction applied to the raw data, so when updating an existing calibration offset with new learned bias we don't want this incorporated	2023-02-06 15:09:07 -05:00
Silvan Fuhrer	e153d1defc	ROMFS: fix PY asymmetry (motor 1 was wrongly placed twice as far from the CG as 0) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-04 16:08:40 +01:00