From 1b174eeca220a1176f2c3146515a4ccacf592e76 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Mar 2018 20:28:38 -0400 Subject: [PATCH] drivers replace math::Vector<3> with matrix::Vector3f --- src/drivers/device/integrator.cpp | 16 +++++----- src/drivers/device/integrator.h | 18 +++++------ src/drivers/imu/adis16448/adis16448.cpp | 8 ++--- src/drivers/imu/bmi055/bmi055_accel.cpp | 4 +-- src/drivers/imu/bmi055/bmi055_gyro.cpp | 4 +-- src/drivers/imu/bmi160/bmi160.cpp | 8 ++--- src/drivers/imu/fxas21002c/fxas21002c.cpp | 4 +-- src/drivers/imu/fxos8701cq/fxos8701cq.cpp | 4 +-- src/drivers/imu/l3gd20/l3gd20.cpp | 4 +-- src/drivers/imu/lsm303d/lsm303d.cpp | 4 +-- src/drivers/imu/mpu6000/mpu6000.cpp | 8 ++--- src/drivers/imu/mpu9250/mpu9250.cpp | 8 ++--- src/lib/conversion/rotation.cpp | 9 ++++++ src/lib/conversion/rotation.h | 4 +-- .../df_ak8963_wrapper/df_ak8963_wrapper.cpp | 6 ++-- .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 10 +++--- .../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 32 +++++++++---------- .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp | 18 +++++------ .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 8 ++--- .../posix/drivers/gyrosim/gyrosim.cpp | 8 ++--- 20 files changed, 97 insertions(+), 88 deletions(-) diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index cc01bc92c8..2835906b91 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -63,7 +63,7 @@ Integrator::~Integrator() } bool -Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt) +Integrator::put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt) { if (_last_integration_time == 0) { /* this is the first item in the integrator */ @@ -84,7 +84,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ } // Use trapezoidal integration to calculate the delta integral - math::Vector<3> delta_alpha = (val + _last_val) * dt * 0.5f; + matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f; _last_val = val; // Calculate coning corrections if required @@ -126,7 +126,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ } bool -Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral, +Integrator::put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt) { if (_last_integration_time == 0) { @@ -145,10 +145,10 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math:: return put(timestamp, val, integral, integral_dt); } -math::Vector<3> +matrix::Vector3f Integrator::get(bool reset, uint64_t &integral_dt) { - math::Vector<3> val = _alpha; + matrix::Vector3f val = _alpha; if (reset) { _reset(integral_dt); @@ -157,11 +157,11 @@ Integrator::get(bool reset, uint64_t &integral_dt) return val; } -math::Vector<3> -Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val) +matrix::Vector3f +Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, matrix::Vector3f &filtered_val) { // Do the usual get with reset first but don't return yet. - math::Vector<3> ret_integral = get(reset, integral_dt); + matrix::Vector3f ret_integral = get(reset, integral_dt); // Because we need both the integral and the integral_dt. filtered_val(0) = ret_integral(0) * 1000000 / integral_dt; diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 2917fbcab6..ecf1e649fa 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -60,7 +60,7 @@ public: * @return true if putting the item triggered an integral reset and the integral should be * published. */ - bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + bool put(uint64_t timestamp, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt); /** * Put an item into the integral but provide an interval instead of a timestamp. @@ -74,7 +74,7 @@ public: * @return true if putting the item triggered an integral reset and the integral should be * published. */ - bool put_with_interval(unsigned interval_us, math::Vector<3> &val, math::Vector<3> &integral, + bool put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, uint64_t &integral_dt); /** @@ -84,7 +84,7 @@ public: * @param integral_dt Get the dt in us of the current integration (only if reset). * @return the integral since the last read-reset */ - math::Vector<3> get(bool reset, uint64_t &integral_dt); + matrix::Vector3f get(bool reset, uint64_t &integral_dt); /** * Get the current integral and reset the integrator if needed. Additionally give the @@ -95,18 +95,18 @@ public: * @param filtered_val The integral differentiated by the integration time. * @return the integral since the last read-reset */ - math::Vector<3> get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val); + matrix::Vector3f get_and_filtered(bool reset, uint64_t &integral_dt, matrix::Vector3f &filtered_val); private: uint64_t _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset, 0 if no auto-reset */ uint64_t _last_integration_time; /**< timestamp of the last integration step */ uint64_t _last_reset_time; /**< last auto-announcement of integral value */ - math::Vector<3> _alpha; /**< integrated value before coning corrections are applied */ - math::Vector<3> _last_alpha; /**< previous value of _alpha */ - math::Vector<3> _beta; /**< accumulated coning corrections */ - math::Vector<3> _last_val; /**< previous input */ - math::Vector<3> _last_delta_alpha; /**< integral from previous previous sampling interval */ + matrix::Vector3f _alpha; /**< integrated value before coning corrections are applied */ + matrix::Vector3f _last_alpha; /**< previous value of _alpha */ + matrix::Vector3f _beta; /**< accumulated coning corrections */ + matrix::Vector3f _last_val; /**< previous input */ + matrix::Vector3f _last_delta_alpha; /**< integral from previous previous sampling interval */ bool _coning_comp_on; /**< true to turn on coning corrections */ /* we don't want this class to be copied */ diff --git a/src/drivers/imu/adis16448/adis16448.cpp b/src/drivers/imu/adis16448/adis16448.cpp index 94a805030b..4a0a9f35ec 100644 --- a/src/drivers/imu/adis16448/adis16448.cpp +++ b/src/drivers/imu/adis16448/adis16448.cpp @@ -1533,16 +1533,16 @@ ADIS16448::measure() arb.temperature_raw = report.temp; arb.temperature = (report.temp * 0.07386f) + 31.0f; - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); arb.y_integral = aval_integrated(1); arb.z_integral = aval_integrated(2); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/bmi055/bmi055_accel.cpp b/src/drivers/imu/bmi055/bmi055_accel.cpp index 66d05f09ff..40cb61fb3d 100644 --- a/src/drivers/imu/bmi055/bmi055_accel.cpp +++ b/src/drivers/imu/bmi055/bmi055_accel.cpp @@ -758,8 +758,8 @@ BMI055_accel::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); diff --git a/src/drivers/imu/bmi055/bmi055_gyro.cpp b/src/drivers/imu/bmi055/bmi055_gyro.cpp index 2829c17f53..1b98ceb4bb 100644 --- a/src/drivers/imu/bmi055/bmi055_gyro.cpp +++ b/src/drivers/imu/bmi055/bmi055_gyro.cpp @@ -730,8 +730,8 @@ BMI055_gyro::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index fd51c6aeee..3b97eeb790 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -1180,8 +1180,8 @@ BMI160::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); @@ -1218,8 +1218,8 @@ BMI160::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/fxas21002c/fxas21002c.cpp b/src/drivers/imu/fxas21002c/fxas21002c.cpp index ff95ff9db8..15dc70661c 100644 --- a/src/drivers/imu/fxas21002c/fxas21002c.cpp +++ b/src/drivers/imu/fxas21002c/fxas21002c.cpp @@ -1062,8 +1062,8 @@ FXAS21002C::measure() gyro_report.y = _gyro_filter_y.apply(y_in_new); gyro_report.z = _gyro_filter_z.apply(z_in_new); - math::Vector<3> gval(x_in_new, y_in_new, z_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); gyro_report.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp index 7490602edd..2c01bf5b17 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp @@ -1460,8 +1460,8 @@ FXOS8701CQ::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); accel_report.x_integral = aval_integrated(0); diff --git a/src/drivers/imu/l3gd20/l3gd20.cpp b/src/drivers/imu/l3gd20/l3gd20.cpp index 356c93bdfd..bf988fa7ea 100644 --- a/src/drivers/imu/l3gd20/l3gd20.cpp +++ b/src/drivers/imu/l3gd20/l3gd20.cpp @@ -1032,8 +1032,8 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(yin); report.z = _gyro_filter_z.apply(zin); - math::Vector<3> gval(xin, yin, zin); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(xin, yin, zin); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt); report.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/lsm303d/lsm303d.cpp b/src/drivers/imu/lsm303d/lsm303d.cpp index 9268090e5e..b425a3331e 100644 --- a/src/drivers/imu/lsm303d/lsm303d.cpp +++ b/src/drivers/imu/lsm303d/lsm303d.cpp @@ -1569,8 +1569,8 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); accel_report.x_integral = aval_integrated(0); diff --git a/src/drivers/imu/mpu6000/mpu6000.cpp b/src/drivers/imu/mpu6000/mpu6000.cpp index df97c4b0fa..16939944c0 100644 --- a/src/drivers/imu/mpu6000/mpu6000.cpp +++ b/src/drivers/imu/mpu6000/mpu6000.cpp @@ -2012,8 +2012,8 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); @@ -2055,8 +2055,8 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 03fcff7e6f..97bb7ddf11 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -1423,8 +1423,8 @@ MPU9250::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); @@ -1461,8 +1461,8 @@ MPU9250::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0); diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 61da4d882a..6b0e4f80f4 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -50,6 +50,15 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) rot_matrix->from_euler(roll, pitch, yaw); } +__EXPORT matrix::Dcmf +get_rot_matrix(enum Rotation rot) +{ + return matrix::Dcmf{matrix::Eulerf{ + math::radians((float)rot_lookup[rot].roll), + math::radians((float)rot_lookup[rot].pitch), + math::radians((float)rot_lookup[rot].yaw)}}; +} + #define HALF_SQRT_2 0.70710678118654757f __EXPORT void diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index a2624844a2..fedb06ee37 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -133,9 +133,9 @@ const rot_lookup_t rot_lookup[] = { /** * Get the rotation matrix */ -__EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); +__EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); +__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); /** * rotate a 3 element float vector in-place diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp index 5256ea901c..e652b28f84 100644 --- a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp +++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp @@ -111,7 +111,7 @@ private: float z_scale; } _mag_calibration; - math::Matrix<3, 3> _rotation_matrix; + matrix::Dcmf _rotation_matrix; int _mag_orb_class_instance; @@ -136,7 +136,7 @@ DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) : _mag_calibration.z_offset = 0.0f; // Get sensor rotation matrix - get_rot_matrix(rotation, &_rotation_matrix); + _rotation_matrix = get_rot_matrix(rotation); } DfAK8963Wrapper::~DfAK8963Wrapper() @@ -285,7 +285,7 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data) mag_report.y_raw = 0; mag_report.z_raw = 0; - math::Vector<3> mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga); + matrix::Vector3f mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga); // apply sensor rotation on the accel measurement mag_val = _rotation_matrix * mag_val; diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp index f8c84d7913..a5e36e6ca4 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp @@ -111,7 +111,7 @@ private: float z_scale; } _mag_calibration; - math::Matrix<3, 3> _rotation_matrix; + matrix::Dcmf _rotation_matrix; int _mag_orb_class_instance; @@ -136,7 +136,7 @@ DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) : _mag_calibration.z_offset = 0.0f; // Get sensor rotation matrix - get_rot_matrix(rotation, &_rotation_matrix); + _rotation_matrix = get_rot_matrix(rotation); } DfHmc5883Wrapper::~DfHmc5883Wrapper() @@ -283,9 +283,9 @@ int DfHmc5883Wrapper::_publish(struct mag_sensor_data &data) mag_report.y_raw = 0; mag_report.z_raw = 0; - math::Vector<3> mag_val(data.field_x_ga, - data.field_y_ga, - data.field_z_ga); + matrix::Vector3f mag_val(data.field_x_ga, + data.field_y_ga, + data.field_z_ga); // apply sensor rotation on the accel measurement mag_val = _rotation_matrix * mag_val; diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp index d8f75e5b5d..79d6d98ce2 100644 --- a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp +++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp @@ -145,7 +145,7 @@ private: float z_scale; } _mag_calibration; - math::Matrix<3, 3> _rotation_matrix; + matrix::Dcmf _rotation_matrix; int _accel_orb_class_instance; int _gyro_orb_class_instance; int _mag_orb_class_instance; @@ -221,7 +221,7 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) : } // Get sensor rotation matrix - get_rot_matrix(rotation, &_rotation_matrix); + _rotation_matrix = get_rot_matrix(rotation); } DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper() @@ -569,11 +569,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) _update_gyro_calibration(); } - math::Vector<3> vec_integrated_unused; + matrix::Vector3f vec_integrated_unused; uint64_t integral_dt_unused; - math::Vector<3> accel_val(data.accel_m_s2_x, - data.accel_m_s2_y, - data.accel_m_s2_z); + matrix::Vector3f accel_val(data.accel_m_s2_x, + data.accel_m_s2_y, + data.accel_m_s2_z); // apply sensor rotation on the accel measurement accel_val = _rotation_matrix * accel_val; // Apply calibration after rotation @@ -584,9 +584,9 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) accel_val, vec_integrated_unused, integral_dt_unused); - math::Vector<3> gyro_val(data.gyro_rad_s_x, - data.gyro_rad_s_y, - data.gyro_rad_s_z); + matrix::Vector3f gyro_val(data.gyro_rad_s_x, + data.gyro_rad_s_y, + data.gyro_rad_s_z); // apply sensor rotation on the gyro measurement gyro_val = _rotation_matrix * gyro_val; // Apply calibration after rotation @@ -656,12 +656,12 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) mag_report.z_raw = 0; } - math::Vector<3> gyro_val_filt; - math::Vector<3> accel_val_filt; + matrix::Vector3f gyro_val_filt; + matrix::Vector3f accel_val_filt; // Read and reset. - math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); + matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); // Use the filtered (by integration) values to get smoother / less noisy data. gyro_report.x = gyro_val_filt(0); @@ -673,9 +673,9 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) accel_report.z = accel_val_filt(2); if (_mag_enabled) { - math::Vector<3> mag_val(data.mag_ga_x, - data.mag_ga_y, - data.mag_ga_z); + matrix::Vector3f mag_val(data.mag_ga_x, + data.mag_ga_y, + data.mag_ga_z); mag_val = _rotation_matrix * mag_val; mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale; mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale; diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp index 7b64490147..256693fa36 100644 --- a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp @@ -131,7 +131,7 @@ private: float z_scale; } _gyro_calibration; - math::Matrix<3, 3> _rotation_matrix; + matrix::Dcmf _rotation_matrix; int _accel_orb_class_instance; int _gyro_orb_class_instance; @@ -193,7 +193,7 @@ DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) : _gyro_calibration.z_offset = 0.0f; // Get sensor rotation matrix - get_rot_matrix(rotation, &_rotation_matrix); + _rotation_matrix = get_rot_matrix(rotation); } DfMPU6050Wrapper::~DfMPU6050Wrapper() @@ -431,10 +431,10 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) uint64_t now = hrt_absolute_time(); - math::Vector<3> vec_integrated_unused; + matrix::Vector3f vec_integrated_unused; uint64_t integral_dt_unused; - math::Vector<3> accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z); + matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z); // apply sensor rotation on the accel measurement accel_val = _rotation_matrix * accel_val; @@ -448,7 +448,7 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) vec_integrated_unused, integral_dt_unused); - math::Vector<3> gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z); + matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z); // apply sensor rotation on the gyro measurement gyro_val = _rotation_matrix * gyro_val; @@ -511,12 +511,12 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) accel_report.y_raw = 0; accel_report.z_raw = 0; - math::Vector<3> gyro_val_filt; - math::Vector<3> accel_val_filt; + matrix::Vector3f gyro_val_filt; + matrix::Vector3f accel_val_filt; // Read and reset. - math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); + matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); // Use the filtered (by integration) values to get smoother / less noisy data. gyro_report.x = gyro_val_filt(0); diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index b06156ecef..1438fb8cac 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -620,8 +620,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); accel_report.x_integral = aval_integrated(0); @@ -652,8 +652,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new); gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new); - math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); gyro_report.x_integral = gval_integrated(0); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 9781f539f9..d9449116db 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -1050,8 +1050,8 @@ GYROSIM::_measure() arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_z; - math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z); - math::Vector<3> aval_integrated; + matrix::Vector3f aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z); + matrix::Vector3f aval_integrated; bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); arb.x_integral = aval_integrated(0); @@ -1075,8 +1075,8 @@ GYROSIM::_measure() grb.y = mpu_report.gyro_y; grb.z = mpu_report.gyro_z; - math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z); - math::Vector<3> gval_integrated; + matrix::Vector3f gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z); + matrix::Vector3f gval_integrated; bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); grb.x_integral = gval_integrated(0);