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

sensors: create vehicle_acceleration module (#12597)

Browse Source
This commit is contained in:
Daniel Agar
2019-08-07 05:05:48 -04:00
committed by GitHub
parent 50fbb56737
commit a917f22b65
25 changed files with 492 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -20
View File
@@ -1482,31 +1482,17 @@ void Ekf2::run()
{
// publish all corrected sensor readings and bias estimates after mag calibration is updated above
sensor_bias_s bias;
sensor_bias_s bias{};
bias.timestamp = now;
// In-run bias estimates
float gyro_bias[3];
_ekf.get_gyro_bias(gyro_bias);
bias.gyro_x_bias = gyro_bias[0];
bias.gyro_y_bias = gyro_bias[1];
bias.gyro_z_bias = gyro_bias[2];
_ekf.get_gyro_bias(bias.gyro_bias);
_ekf.get_accel_bias(bias.accel_bias);
float accel_bias[3];
_ekf.get_accel_bias(accel_bias);
bias.accel_x_bias = accel_bias[0];
bias.accel_y_bias = accel_bias[1];
bias.accel_z_bias = accel_bias[2];
bias.mag_x_bias = _last_valid_mag_cal[0];
bias.mag_y_bias = _last_valid_mag_cal[1];
bias.mag_z_bias = _last_valid_mag_cal[2];
// TODO: remove from sensor_bias?
bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0];
bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1];
bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2];
bias.mag_bias[0] = _last_valid_mag_cal[0];
bias.mag_bias[1] = _last_valid_mag_cal[1];
bias.mag_bias[2] = _last_valid_mag_cal[2];
_sensor_bias_pub.publish(bias);
}