sensors move to matrix lib

src/modules/sensors/temperature_compensation.cpp

@@ -377,8 +377,8 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
 	return -1;
 }
 
-int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
-		float *offsets, float *scales)
+int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data,
+		float temperature, float *offsets, float *scales)
 {
 	if (_parameters.gyro_tc_enable != 1) {
 		return 0;
@@ -406,7 +406,7 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve
 	return 1;
 }
 
-int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data,
+int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data,
 		float temperature, float *offsets, float *scales)
 {
 	if (_parameters.accel_tc_enable != 1) {

src/modules/sensors/temperature_compensation.h

@@ -89,14 +89,13 @@ public:
 	 *         1: corrections applied but no changes to offsets & scales,
 	 *         2: corrections applied and offsets & scales updated
 	 */
-	int apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
-				   float *offsets, float *scales);
+	int apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
+				   float *scales);
 
-	int apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data, float temperature,
-				    float *offsets, float *scales);
+	int apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
+				    float *scales);
 
-	int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
-				   float *offsets, float *scales);
+	int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales);
 
 	/** output current configuration status to console */
 	void print_status();

src/modules/sensors/voted_sensors_update.cpp

@@ -134,14 +134,13 @@ void VotedSensorsUpdate::deinit()
 
 void VotedSensorsUpdate::parameters_update()
 {
-	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	/* fine tune board offset */
-	math::Matrix<3, 3> board_rotation_offset;
-	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]);
+	matrix::Dcmf board_rotation_offset = matrix::Eulerf(
+			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]);
 
-	_board_rotation = board_rotation_offset * _board_rotation;
+	_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
 
 	// initialze all mag rotations with the board rotation in case there is no calibration data available
 	for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
@@ -513,7 +512,7 @@ void VotedSensorsUpdate::parameters_update()
 				/* now get the mag rotation */
 				if (mag_rot >= 0) {
 					// Set external magnetometers to use the parameter value
-					get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[topic_instance]);
+					_mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot);
 
 				} else {
 					// Set internal magnetometers to use the board rotation
@@ -567,7 +566,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 
 			_accel_device_id[uorb_index] = accel_report.device_id;
 
-			math::Vector<3> accel_data;
+			matrix::Vector3f accel_data;
 
 			if (accel_report.integral_dt != 0) {
 				/*
@@ -579,8 +578,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 
 				// convert the delta velocities to an equivalent acceleration before application of corrections
 				float dt_inv = 1.e6f / accel_report.integral_dt;
-				accel_data = math::Vector<3>(accel_report.x_integral * dt_inv, accel_report.y_integral * dt_inv,
-							     accel_report.z_integral * dt_inv);
+				accel_data = matrix::Vector3f(accel_report.x_integral * dt_inv,
+							      accel_report.y_integral * dt_inv,
+							      accel_report.z_integral * dt_inv);
 
 				_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
 
@@ -589,7 +589,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 
 				// Correct each sensor for temperature effects
 				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				accel_data = math::Vector<3>(accel_report.x, accel_report.y, accel_report.z);
+				accel_data = matrix::Vector3f(accel_report.x, accel_report.y, accel_report.z);
 
 				// handle the cse where this is our first output
 				if (_last_accel_timestamp[uorb_index] == 0) {
@@ -674,7 +674,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 
 			_gyro_device_id[uorb_index] = gyro_report.device_id;
 
-			math::Vector<3> gyro_rate;
+			matrix::Vector3f gyro_rate;
 
 			if (gyro_report.integral_dt != 0) {
 				/*
@@ -686,8 +686,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 
 				// convert the delta angles to an equivalent angular rate before application of corrections
 				float dt_inv = 1.e6f / gyro_report.integral_dt;
-				gyro_rate = math::Vector<3>(gyro_report.x_integral * dt_inv, gyro_report.y_integral * dt_inv,
-							    gyro_report.z_integral * dt_inv);
+				gyro_rate = matrix::Vector3f(gyro_report.x_integral * dt_inv,
+							     gyro_report.y_integral * dt_inv,
+							     gyro_report.z_integral * dt_inv);
 
 				_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
 
@@ -696,7 +697,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 
 				// Correct each sensor for temperature effects
 				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				gyro_rate = math::Vector<3>(gyro_report.x, gyro_report.y, gyro_report.z);
+				gyro_rate = matrix::Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
 
 				// handle the case where this is our first output
 				if (_last_sensor_data[uorb_index].timestamp == 0) {
@@ -782,7 +783,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 				_mag.priority[uorb_index] = (uint8_t)priority;
 			}
 
-			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
+			matrix::Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
 			vect = _mag_rotation[uorb_index] * vect;
 
 			_last_sensor_data[uorb_index].magnetometer_ga[0] = vect(0);
@@ -790,8 +791,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 			_last_sensor_data[uorb_index].magnetometer_ga[2] = vect(2);
 
 			_last_mag_timestamp[uorb_index] = mag_report.timestamp;
-			_mag.voter.put(uorb_index, mag_report.timestamp, vect.data,
-				       mag_report.error_count, _mag.priority[uorb_index]);
+			_mag.voter.put(uorb_index, mag_report.timestamp, vect.data(), mag_report.error_count, _mag.priority[uorb_index]);
 		}
 	}
 
@@ -851,15 +851,14 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 			_baro_device_id[uorb_index] = baro_report.device_id;
 
 			got_update = true;
-			math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
+			matrix::Vector3f vect(baro_report.altitude, 0.f, 0.f);
 
 			_last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude;
 			_last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature;
 			_last_baro_pressure[uorb_index] = corrected_pressure;
 
 			_last_baro_timestamp[uorb_index] = baro_report.timestamp;
-			_baro.voter.put(uorb_index, baro_report.timestamp, vect.data,
-					baro_report.error_count, _baro.priority[uorb_index]);
+			_baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]);
 		}
 	}
 

src/modules/sensors/voted_sensors_update.h

@@ -253,8 +253,8 @@ private:
 	uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */
 	uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */
 
-	math::Matrix<3, 3>	_board_rotation = {};	/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix<3, 3>	_mag_rotation[MAG_COUNT_MAX] = {};	/**< rotation matrix for the orientation that the external mag0 is mounted */
+	matrix::Dcmf	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
+	matrix::Dcmf	_mag_rotation[MAG_COUNT_MAX];	/**< rotation matrix for the orientation that the external mag0 is mounted */
 
 	const Parameters &_parameters;
 	const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */