Merged master

2 files changed, 147 insertions, 15 deletions

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index a5fc34184..d19a7aec3 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
 
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 22374a1fe..a6204c9fa 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -138,6 +139,77 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
+/**
  * Sensor app start / stop handling function
  *
  * @ingroup apps
@@ -189,8 +261,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -210,13 +282,15 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -227,6 +301,9 @@ private:
 		float accel_scale[3];
 		float diff_pres_offset_pa;
 
+		int board_rotation;
+		int external_mag_rotation;
+
 		int rc_type;
 
 		int rc_map_roll;
@@ -264,7 +341,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
 		param_t rc_demix;
@@ -304,6 +380,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -313,6 +392,11 @@ private:
 	int		parameters_update();
 
 	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+	/**
 	 * Do accel-related initialisation.
 	 */
 	void		accel_init();
@@ -448,7 +532,10 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3)
 {
 
 	/* basic r/c parameters */
@@ -538,6 +625,10 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -724,10 +815,34 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
 void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
+void
 Sensors::accel_init()
 {
 	int	fd;
@@ -750,7 +865,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -758,6 +873,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");
@@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation*vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation*vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		// XXX TODO add support for external mag orientation
+
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;