From 69bd3ecf952c43ddcbc8fb3bd8828380fe7e8292 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 22 Feb 2021 12:33:40 -0500
Subject: [PATCH] sensor_calibration: respect board rotation even if
 uncalibrated

 - fixes https://github.com/PX4/PX4-Autopilot/issues/16873
---
 src/lib/sensor_calibration/Accelerometer.cpp | 10 ++++++++--
 src/lib/sensor_calibration/Gyroscope.cpp     | 10 ++++++++--
 src/lib/sensor_calibration/Magnetometer.cpp  | 10 ++++++++--
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp
index 94ded6cc6f..daa321223c 100644
--- a/src/lib/sensor_calibration/Accelerometer.cpp
+++ b/src/lib/sensor_calibration/Accelerometer.cpp
@@ -220,8 +220,14 @@ void Accelerometer::ParametersUpdate()
 
 void Accelerometer::Reset()
 {
-	_rotation.setIdentity();
-	_rotation_enum = ROTATION_NONE;
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
 	_offset.zero();
 	_scale = Vector3f{1.f, 1.f, 1.f};
 	_thermal_offset.zero();
diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp
index 9baba5b694..445a049408 100644
--- a/src/lib/sensor_calibration/Gyroscope.cpp
+++ b/src/lib/sensor_calibration/Gyroscope.cpp
@@ -205,8 +205,14 @@ void Gyroscope::ParametersUpdate()
 
 void Gyroscope::Reset()
 {
-	_rotation.setIdentity();
-	_rotation_enum = ROTATION_NONE;
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
 	_offset.zero();
 	_thermal_offset.zero();
 
diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp
index b5b7a6b38a..08f4725a1b 100644
--- a/src/lib/sensor_calibration/Magnetometer.cpp
+++ b/src/lib/sensor_calibration/Magnetometer.cpp
@@ -207,8 +207,14 @@ void Magnetometer::ParametersUpdate()
 
 void Magnetometer::Reset()
 {
-	_rotation.setIdentity();
-	_rotation_enum = ROTATION_NONE;
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
 	_offset.zero();
 	_scale.setIdentity();