fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 23:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers replace math::Vector<3> with matrix::Vector3f

Browse Source
This commit is contained in:
Daniel Agar
2018-03-21 13:48:45 -04:00
parent e504dc86b7
commit 1b174eeca2
20 changed files with 97 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/integrator.cpp
+8 -8
View File
@@ -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;
src/drivers/device/integrator.h
+9 -9
View File
@@ -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 */
src/drivers/imu/adis16448/adis16448.cpp
+4 -4
View File
@@ -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);
src/drivers/imu/bmi055/bmi055_accel.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/bmi055/bmi055_gyro.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/bmi160/bmi160.cpp
+4 -4
View File
@@ -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);
src/drivers/imu/fxas21002c/fxas21002c.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/fxos8701cq/fxos8701cq.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/l3gd20/l3gd20.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/lsm303d/lsm303d.cpp
+2 -2
View File
@@ -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);
src/drivers/imu/mpu6000/mpu6000.cpp
+4 -4
View File
@@ -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);
src/drivers/imu/mpu9250/mpu9250.cpp
+4 -4
View File
@@ -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);