WIP: Support in-flight fine tuning of board alignment. Implemented by applying a user supplied rotation matrix via new SENS_BOARD_(PITCH, ROLL, YAW)_OFF params in the sensors app. Currently only tested in QGC.

2 files changed, 47 insertions, 3 deletions

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 999cf8bb3..c90633822 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -243,6 +243,30 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
 /**
+ * Board rotation pitch offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+ PARAM_DEFINE_FLOAT(SENS_BOARD_PITCH_OFF, 0);
+
+/**
+ * Board rotation roll offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_ROLL_OFF, 0);
+
+/**
+ * Board rotation YAW offset
+ *
+ * This parameter defines a pitch offset from the board rotation. It allows the user
+ * to fine tune the board offset in the event of misalignment.
+ */
+PARAM_DEFINE_FLOAT(SENS_BOARD_YAW_OFF, 0);
+
+/**
  * External magnetometer rotation
  *
  * This parameter defines the rotation of the external magnetometer relative
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b268b1b36..1a6202d7d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
+	math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/
+	
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -252,6 +254,8 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
+		
+		float board_offset[3];
 
 		int rc_map_roll;
 		int rc_map_pitch;
@@ -341,6 +345,8 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
+		
+		param_t board_offset[3];
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -587,6 +593,11 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+	
+	/* rotation offsets */
+	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_ROLL_OFF");
+	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_PITCH_OFF");
+	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_YAW_OFF");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -791,6 +802,15 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+	
+	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
+	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
+	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
+	
+	_board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
+							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
+
 
 	return OK;
 }
@@ -970,7 +990,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
+		vect = _board_rotation * _board_rotation_offset * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
 		raw.accelerometer_m_s2[1] = vect(1);
@@ -996,7 +1016,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
+		vect = _board_rotation * _board_rotation_offset * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
 		raw.gyro_rad_s[1] = vect(1);
@@ -1027,7 +1047,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			vect = _external_mag_rotation * vect;
 
 		} else {
-			vect = _board_rotation * vect;
+			vect = _board_rotation * _board_rotation_offset * vect;
 		}
 
 		raw.magnetometer_ga[0] = vect(0);