Update src/modules/mavlink/mavlink_receiver.cpp

Autotune mavlink - reject invalid param1 or 2 with fail
add cmake variant and px4board file to ark flow mr target (#25793 )
2026-06-06 15:20:04 +08:00 · 2025-10-23 16:11:44 +11:00 · 2025-10-22 10:12:42 +11:00 · 2025-10-21 13:20:55 -06:00 · 2025-10-21 11:48:03 +02:00 · 2025-10-21 11:21:27 +02:00
12 changed files with 258 additions and 175 deletions
@@ -141,6 +141,11 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: ark_can-flow_canbootloader
+    ark_can-flow-mr_default:
+      short: ark_can-flow-mr_default
+      buildType: MinSizeRel
+      settings:
+        CONFIG: ark_can-flow-mr_default
    ark_can-flow-mr_canbootloader:
      short: ark_can-flow-mr_canbootloader
      buildType: MinSizeRel
@@ -0,0 +1,30 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m4"
+CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_DRIVERS_BOOTLOADERS=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
+CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y
+CONFIG_BOARD_UAVCAN_INTERFACES=1
+CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
+CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
+CONFIG_UAVCANNODE_RAW_IMU=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_ACCELERATION is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
+# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -8,9 +8,8 @@ param set-default SENS_FLOW_RATE 150
 param set-default SENS_IMU_CLPNOTI 0

 param set-default SENS_AFBR_S_RATE 25
-param set-default SENS_AFBR_L_RATE 10
-param set-default SENS_AFBR_THRESH 8
-param set-default SENS_AFBR_HYSTER 2
+param set-default SENS_AFBR_L_RATE 5
+param set-default SENS_AFBR_MODE 1

 # Internal SPI
 paa3905 -s start -Y 180
@@ -77,7 +77,7 @@ void AdsbConflict::detect_traffic_conflict(double lat_now, double lon_now, float
 			&& (fabsf(_crosstrack_error.distance) < _conflict_detection_params.crosstrack_separation);

 	const bool _crosstrack_separation_check = (fabsf(alt_now - _transponder_report.altitude) <
-			_conflict_detection_params.crosstrack_separation);
+			_conflict_detection_params.vertical_separation);

 	bool collision_time_check = false;

@@ -1019,6 +1019,7 @@ private:
 	void resetQuatStateYaw(float yaw, float yaw_variance);
 	void propagateQuatReset(const Quatf &quat_before_reset);
 	void resetYawByFusion(float yaw, float yaw_variance);
+	void resetHorizontalVelocityToMatchYaw(float delta_yaw);

 	HeightSensor _height_sensor_ref{HeightSensor::UNKNOWN};
 	PositionSensor _position_sensor_ref{PositionSensor::GNSS};
@@ -140,6 +140,10 @@ void Ekf::resetQuatStateYaw(const float yaw, const float yaw_variance)
 	_time_last_heading_fuse = _time_delayed_us;

 	propagateQuatReset(quat_before_reset);
+
+	// rotate horizontal velocity by the yaw change
+	const float yaw_diff = wrap_pi(yaw - getEulerYaw(quat_before_reset));
+	resetHorizontalVelocityToMatchYaw(yaw_diff);
 }

 void Ekf::propagateQuatReset(const Quatf &quat_before_reset)
@@ -189,4 +193,16 @@ void Ekf::resetYawByFusion(const float yaw, const float yaw_variance)
 	fuseYaw(aid_src_status, H_YAW, reset_yaw);

 	propagateQuatReset(quat_before_reset);
+
+	resetHorizontalVelocityToMatchYaw(-aid_src_status.innovation);
+}
+
+void Ekf::resetHorizontalVelocityToMatchYaw(const float delta_yaw)
+{
+	if (!isNorthEastAidingActive() && fabsf(delta_yaw) > 0.3f) {
+		const matrix::Dcm2f R_yaw(delta_yaw);
+		const Vector2f vel_rotated = R_yaw * Vector2f(_state.vel);
+		const float vel_var = fmaxf(P(State::vel.idx, State::vel.idx), P(State::vel.idx + 1, State::vel.idx + 1));
+		resetHorizontalVelocityTo(vel_rotated, vel_var);
+	}
 }
@@ -67,22 +67,22 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0057,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
 6590000,1,-0.0093,-0.011,0.00016,0.0037,0.0057,-0.099,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
 6690000,1,-0.0093,-0.011,9.4e-05,0.0046,0.0053,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
-6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0052,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7
-6890000,0.71,0.0013,-0.014,0.7,0.00052,0.0065,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8
-6990000,0.71,0.0013,-0.014,0.71,-0.00025,0.0048,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8
-7090000,0.71,0.0012,-0.014,0.71,-0.00081,0.0013,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8
-7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0021,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8
-7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.01,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9
-7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9
-7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.012,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9
-7590000,0.71,0.0017,-0.014,0.71,-0.0034,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9
-7690000,0.71,0.0017,-0.014,0.71,-0.0046,0.018,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2
-7790000,0.71,0.0017,-0.014,0.71,-0.011,0.017,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2
-7890000,0.71,0.0017,-0.014,0.71,-0.011,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2
-7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.023,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2
-8090000,0.71,0.0017,-0.014,0.71,-0.0051,0.025,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1
-8190000,0.71,0.0018,-0.014,0.71,-0.014,0.029,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
-8290000,0.71,0.0017,-0.014,0.71,-0.018,0.023,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
+6790000,0.71,0.0012,-0.014,0.71,-0.0056,0.0034,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.1,0.1,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7
+6890000,0.71,0.0013,-0.014,0.7,-0.0076,0.0039,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.18,0.18,0.46,0.1,0.1,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8
+6990000,0.71,0.0013,-0.014,0.71,-0.0078,0.0042,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.19,0.19,0.36,0.11,0.11,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8
+7090000,0.71,0.0012,-0.014,0.71,-0.0084,0.0028,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.2,0.2,0.29,0.12,0.12,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8
+7190000,0.71,0.0013,-0.014,0.71,-0.01,0.0027,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.21,0.21,0.24,0.14,0.14,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8
+7290000,0.71,0.0015,-0.014,0.71,-0.012,0.0052,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.22,0.23,0.2,0.16,0.16,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9
+7390000,0.71,0.0016,-0.014,0.71,-0.012,0.0081,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.24,0.25,0.18,0.18,0.18,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9
+7490000,0.71,0.0015,-0.014,0.71,-0.013,0.0071,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.27,0.27,0.15,0.21,0.21,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9
+7590000,0.71,0.0017,-0.014,0.71,-0.014,0.012,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.29,0.29,0.14,0.25,0.25,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9
+7690000,0.71,0.0017,-0.014,0.71,-0.015,0.011,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.32,0.32,0.13,0.29,0.29,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2
+7790000,0.71,0.0017,-0.014,0.71,-0.019,0.011,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.36,0.36,0.12,0.34,0.34,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2
+7890000,0.71,0.0017,-0.014,0.71,-0.02,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.39,0.39,0.11,0.4,0.4,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2
+7990000,0.71,0.0017,-0.014,0.71,-0.02,0.017,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.43,0.43,0.1,0.47,0.47,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2
+8090000,0.71,0.0017,-0.014,0.71,-0.018,0.019,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.47,0.47,0.1,0.55,0.55,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1
+8190000,0.71,0.0018,-0.014,0.71,-0.024,0.021,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.52,0.51,0.099,0.64,0.64,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
+8290000,0.71,0.0017,-0.014,0.71,-0.027,0.018,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.56,0.55,0.097,0.74,0.74,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
 8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00048,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00052,0.00039,-0.021,0.21,-9.6e-06,0.43,-0.00031,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
 8490000,0.71,0.0017,-0.014,0.71,-0.0029,0.0026,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00052,0.00039,-0.025,0.21,-9.3e-06,0.43,-0.00032,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2
 8590000,0.71,0.0022,-0.014,0.71,-0.00042,0.00097,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-7e-05,-0.00052,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00043,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2
@@ -69,10 +69,10 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 6690000,1,-0.0088,-0.011,0.00052,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
 6790000,1,-0.0089,-0.011,0.00049,0.003,0.016,-0.043,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
 6890000,1,-0.0087,-0.011,0.0004,0.0023,0.016,-0.039,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8
-6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8
-7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8
-7190000,0.98,-0.0065,-0.012,0.18,-6.6e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8
-7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9
+6990000,0.98,-0.0067,-0.012,0.18,-0.0032,0.013,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.16,0.031,0.097,0.097,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8
+7090000,0.98,-0.0065,-0.012,0.18,-0.0041,0.017,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.16,0.16,0.03,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8
+7190000,0.98,-0.0065,-0.012,0.18,-0.0046,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.16,0.16,0.029,0.11,0.11,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8
+7290000,0.98,-0.0064,-0.012,0.18,-0.0041,0.023,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.17,0.17,0.028,0.12,0.12,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9
 7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00095,-0.032,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.9e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9
 7490000,0.98,-0.0063,-0.012,0.18,0.00098,0.0035,-0.026,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.8e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9
 7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.0061,-0.023,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00035,-0.00039,-8.8e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9
@@ -214,3 +214,60 @@ TEST_F(EkfMagTest, suddenInclinationChange)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
 }
+
+TEST_F(EkfMagTest, velocityRotationOnYawReset)
+{
+	// GIVEN: Mag fusion is active and vehicle is flying with airspeed
+	const float initial_mag_heading = M_PI_F / 4.f; // 45 degrees
+	Vector3f mag_data(0.2f * cosf(initial_mag_heading), -0.2f * sinf(initial_mag_heading), 0.4f);
+	_sensor_simulator._mag.setData(mag_data);
+	_sensor_simulator.runSeconds(_init_duration_s);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	const float airspeed_body = 15.0f; // 15 m/s airspeed in body X direction
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body, airspeed_body);
+
+	_ekf_wrapper.enableBetaFusion();
+	_sensor_simulator.runSeconds(3);
+
+	// initial state
+	const Vector3f vel_before = _ekf->getVelocity();
+	const float yaw_before = _ekf_wrapper.getYawAngle();
+	const matrix::Dcm2f R_ned_to_body_before(-yaw_before);
+	const Vector2f vel_body_before = R_ned_to_body_before * Vector2f(vel_before);
+
+	// WHEN: Mag heading suddenly changes by more than 0.3 rad (90 degrees)
+	const float new_mag_heading = yaw_before + M_PI_F / 2.f;
+	mag_data = Vector3f(0.2f * cosf(new_mag_heading), -0.2f * sinf(new_mag_heading), 0.4f);
+	_sensor_simulator._mag.setData(mag_data);
+	_sensor_simulator.runSeconds(8.f);
+
+	// THEN: the yaw should be reset to the new mag heading
+	const float yaw_after = _ekf_wrapper.getYawAngle();
+	EXPECT_NEAR(yaw_after, new_mag_heading, radians(5.0f))
+			<< "Yaw after: " << degrees(yaw_after)
+			<< " Expected: " << degrees(new_mag_heading);
+
+	// AND: the NED velocity should be rotated to maintain consistent body-frame velocity
+	const Vector3f vel_after = _ekf->getVelocity();
+
+	// Calculate body-frame velocity after reset
+	const matrix::Dcm2f R_ned_to_body_after(-yaw_after);
+	const Vector2f vel_body_after = R_ned_to_body_after * Vector2f(vel_after);
+
+	// Body-frame velocity should remain approximately the same
+	EXPECT_NEAR(vel_body_before(0), vel_body_after(0), 1.0f)
+			<< "Body-frame velocity X before: " << vel_body_before(0)
+			<< " after: " << vel_body_after(0);
+	EXPECT_NEAR(vel_body_before(1), vel_body_after(1), 1.0f)
+			<< "Body-frame velocity Y before: " << vel_body_before(1)
+			<< " after: " << vel_body_after(1);
+
+	// Verify that the yaw change was sufficient to trigger velocity rotation (> 0.3 rad)
+	const float yaw_change = fabsf(wrap_pi(yaw_after - yaw_before));
+	EXPECT_GT(yaw_change, 0.3f) << "Yaw change: " << degrees(yaw_change) << " deg";
+}
@@ -187,7 +187,7 @@ Mavlink::mavlink_update_parameters()
 {
 	updateParams();

-	set_protocol_version(_param_mav_proto_ver.get());
+	setProtocolVersion(_param_mav_proto_ver.get());

 	if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) {
 		_param_mav_type.set(0);
@@ -272,7 +272,7 @@ Mavlink::set_instance_id()
 	return false;
 }

-void Mavlink::set_protocol_version(unsigned version)
+void Mavlink::setProtocolVersion(uint8_t version)
 {
 	if (version == 1) {
 		get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
@@ -3041,7 +3041,7 @@ Mavlink::display_status()
 	}

 	printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
-	printf("\tMAVLink version: %" PRId32 "\n", _protocol_version);
+	printf("\tMAVLink version: %" PRId8 "\n", _protocol_version);

 	printf("\ttransport protocol: ");

@@ -109,22 +109,10 @@ class Mavlink final : public ModuleParams
 {

 public:
-	/**
-	 * Constructor
-	 */
 	Mavlink();
-
-	/**
-	 * Destructor, also kills the mavlinks task.
-	 */
 	~Mavlink();

-	/**
-	* Start the mavlink task.
-	 *
-	 * @return OK on success.
-	 */
-	static int		start(int argc, char *argv[]);
+	static int start(int argc, char *argv[]);

 	bool running() const { return _task_running.load(); }
 	bool should_exit() const { return _task_should_exit.load(); }
@@ -134,63 +122,40 @@ public:
 		_receiver.request_stop();
 	}

-	/**
-	 * Display the mavlink status.
-	 */
-	void			display_status();
+	void display_status();
+	void display_status_streams();

-	/**
-	 * Display the status of all enabled streams.
-	 */
-	void			display_status_streams();
+	static int stop_command(int argc, char *argv[]);
+	static int stream_command(int argc, char *argv[]);

-	static int		stop_command(int argc, char *argv[]);
-	static int		stream_command(int argc, char *argv[]);
+	static int instance_count();
+	static Mavlink *new_instance();
+	static Mavlink *get_instance_for_device(const char *device_name);

-	static int		instance_count();
+	mavlink_message_t *get_buffer() { return &_mavlink_buffer; }
+	mavlink_status_t *get_status() { return &_mavlink_status; }

-	static Mavlink		*new_instance();
+	void setProtocolVersion(uint8_t version);
+	uint8_t getProtocolVersion() const { return _protocol_version; };

-	static Mavlink 		*get_instance_for_device(const char *device_name);
+	static int destroy_all_instances();
+	static int get_status_all_instances(bool show_streams_status);
+	static bool serial_instance_exists(const char *device_name, Mavlink *self);

-	mavlink_message_t 	*get_buffer() { return &_mavlink_buffer; }
+	static bool component_was_seen(int system_id, int component_id, Mavlink &self);
+	static void forward_message(const mavlink_message_t *msg, Mavlink *self);

-	mavlink_status_t 	*get_status() { return &_mavlink_status; }
+	bool check_events() const { return _should_check_events.load(); }
+	void check_events_enable() { _should_check_events.store(true); }
+	void check_events_disable() { _should_check_events.store(false); }

-	void			set_protocol_version(unsigned version);
+	bool sending_parameters() const { return _sending_parameters.load(); }
+	void set_sending_parameters(bool sending) { _sending_parameters.store(sending); }

-	static int		destroy_all_instances();
+	int get_uart_fd() const { return _uart_fd; }

-	static int		get_status_all_instances(bool show_streams_status);
-
-	static bool		serial_instance_exists(const char *device_name, Mavlink *self);
-
-	static bool		component_was_seen(int system_id, int component_id, Mavlink &self);
-
-	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
-
-	bool			check_events() const { return _should_check_events.load(); }
-	void			check_events_enable() { _should_check_events.store(true); }
-	void			check_events_disable() { _should_check_events.store(false); }
-
-	bool			sending_parameters() const { return _sending_parameters.load(); }
-	void			set_sending_parameters(bool sending) { _sending_parameters.store(sending); }
-
-	int			get_uart_fd() const { return _uart_fd; }
-
-	/**
-	 * Get the MAVLink system id.
-	 *
-	 * @return The system ID of this vehicle
-	 */
-	int			get_system_id() const { return mavlink_system.sysid; }
-
-	/**
-	 * Get the MAVLink component id.
-	 *
-	 * @return The component ID of this vehicle
-	 */
-	int			get_component_id() const { return mavlink_system.compid; }
+	int get_system_id() const { return mavlink_system.sysid; }
+	int get_component_id() const { return mavlink_system.compid; }

 	const char *_device_name{DEFAULT_DEVICE_NAME};

@@ -613,7 +578,7 @@ private:
 	uint64_t		_last_write_success_time{0};
 	uint64_t		_last_write_try_time{0};
 	uint64_t		_mavlink_start_time{0};
-	int32_t _protocol_version = 0; ///< after initialization the only values are 1 and 2
+	uint8_t _protocol_version = 0; ///< after initialization the only values are 1 and 2

 	unsigned		_bytes_tx{0};
 	unsigned		_bytes_txerr{0};
@@ -618,108 +618,118 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const

 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {

-		bool has_module = true;
-		autotune_attitude_control_status_s status{};
-		_autotune_attitude_control_status_sub.copy(&status);
+		// Check command validity
+		if ((int)roundf(vehicle_command.param1) != 1 || (int)roundf(vehicle_command.param2) != 0) {
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+			send_ack = true;

-		// if not busy enable via the parameter
-		// do not check the return value of the uORB copy above because the module
-		// starts publishing only when MC_AT_START is set
-		if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
+		} else {

-			if (!vehicle_status.in_transition_mode) {
-				param_t atune_start;
+			bool has_module = true;
+			autotune_attitude_control_status_s status{};
+			_autotune_attitude_control_status_sub.copy(&status);

-				switch (vehicle_status.vehicle_type) {
-				case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
-					atune_start = param_find("FW_AT_START");
+			// if not busy enable via the parameter
+			// do not check the return value of the uORB copy above because the module
+			// starts publishing only when MC_AT_START is set
+			if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
+				vehicle_status_s vehicle_status{};
+				_vehicle_status_sub.copy(&vehicle_status);

-					break;
+				if (!vehicle_status.in_transition_mode) {
+					param_t atune_start;

-				case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
-					atune_start = param_find("MC_AT_START");
+					switch (vehicle_status.vehicle_type) {
+					case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
+						atune_start = param_find("FW_AT_START");

-					break;
+						break;

-				default:
-					atune_start = PARAM_INVALID;
-					break;
-				}
+					case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
+						atune_start = param_find("MC_AT_START");

-				if (atune_start == PARAM_INVALID) {
-					has_module = false;
+						break;
+
+					default:
+						atune_start = PARAM_INVALID;
+						break;
+					}
+
+					if (atune_start == PARAM_INVALID) {
+						has_module = false;
+
+					} else {
+						int32_t start = 1;
+						param_set(atune_start, &start);
+					}

 				} else {
-					int32_t start = 1;
-					param_set(atune_start, &start);
+					has_module = false;
+				}
+			}
+
+			if (has_module) {
+
+				// most are in progress
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+
+				switch (status.state) {
+				case autotune_attitude_control_status_s::STATE_IDLE:
+				case autotune_attitude_control_status_s::STATE_INIT:
+					progress = 0;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_ROLL:
+				case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
+					progress = 20;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_PITCH:
+				case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
+					progress = 40;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_YAW:
+				case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
+					progress = 60;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_VERIFICATION:
+					progress = 80;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_APPLY:
+					progress = 85;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_TEST:
+					progress = 90;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
+					progress = 95;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_COMPLETE:
+					progress = 100;
+					// ack it properly with an ACCEPTED once we're done
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					break;
+
+				case autotune_attitude_control_status_s::STATE_FAIL:
+					progress = 0;
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+					break;
 				}

 			} else {
-				has_module = false;
-			}
-		}
-
-		if (has_module) {
-
-			// most are in progress
-			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
-
-			switch (status.state) {
-			case autotune_attitude_control_status_s::STATE_IDLE:
-			case autotune_attitude_control_status_s::STATE_INIT:
-				progress = 0;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_ROLL:
-			case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
-				progress = 20;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_PITCH:
-			case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
-				progress = 40;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_YAW:
-			case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
-				progress = 60;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_VERIFICATION:
-				progress = 80;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_APPLY:
-				progress = 85;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_TEST:
-				progress = 90;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
-				progress = 95;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_COMPLETE:
-				progress = 100;
-				// ack it properly with an ACCEPTED once we're done
-				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-				break;
-
-			case autotune_attitude_control_status_s::STATE_FAIL:
-				progress = 0;
-				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
-				break;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 			}

-		} else {
-			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			send_ack = true;
+
 		}

-		send_ack = true;

 	} else {
 		send_ack = false;
@@ -3230,9 +3240,9 @@ MavlinkReceiver::run()

 						// If we receive a complete MAVLink 2 packet, also switch the outgoing protocol version
 						if (!(_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
-						    && (_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
+						    && _mavlink.getProtocolVersion() != 2) {
 							PX4_INFO("Upgrade to MAVLink v2 because of incoming packet");
-							_mavlink.set_protocol_version(2);
+							_mavlink.setProtocolVersion(2);
 						}

 						switch (_mavlink.get_mode()) {
Author	SHA1	Message	Date
Hamish Willee	728b10e0e0	Update src/modules/mavlink/mavlink_receiver.cpp	2025-10-23 16:11:44 +11:00
Hamish Willee	36bbb4a190	Autotune mavlink - reject invalid param1 or 2 with fail	2025-10-22 10:12:42 +11:00
Andrew Brahim	1297780d28	add cmake variant and px4board file to ark flow mr target (#25793 ) * add cmake variant and px4board file Signed-off-by: dirksavage88 <dirksavage88@gmail.com> * boards: ark flow mr update default.px4board and rc.board_sensors --------- Signed-off-by: dirksavage88 <dirksavage88@gmail.com> Co-authored-by: alexklimaj <alex@arkelectron.com>	2025-10-21 13:20:55 -06:00
Marco Hauswirth	59f6c69067	EKF2: rotate vel on heading reset (#25753 ) * correct NED velocity according to yaw reset to keep "body-velocity" consistent * add unit test velocityRotationOnYawReset * '[AUTO COMMIT] update change indication' * dont adjust velocity on yaw-reset when NE aiding active --------- Co-authored-by: haumarco <haumarco@users.noreply.github.com>	2025-10-21 11:48:03 +02:00
Tobias Fenner	71f56df23b	adsb: Fix vertical separation check using wrong parameter The collision detection incorrectly used crosstrack_separation (horizontal radius) for both horizontal and vertical checks. This resulted in the vertical separation threshold being ignored, creating a spherical detection zone instead of the intended cylindrical "hockey puck" shape. Fixed by using vertical_separation parameter for altitude difference check on line 80. The bug was likely introduced as a copy-paste error when creating the vertical check from the horizontal check template. The existing unit test uses identical values for both parameters (500.0f), which masked this bug. Testing: Code review. The existing unit tests pass, but they don't catch this bug due to using equal values. Future test improvement would be to use different crosstrack_separation and vertical_separation values. Signed-off-by: Tobias Fenner <tobyrfenner@gmail.com>	2025-10-21 11:21:27 +02:00
Jacob Dahl	d30fa62f40	mavlink: take in @dakejahl 's refactoring and extend it	2025-10-21 10:24:30 +02:00