diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index 38dd1f0eee..75264b086f 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/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) { diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index edc00537cf..64a7836825 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/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(); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 16c335aac3..893a623251 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/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]); } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 6042cca9b0..f4ee619565 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/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? */