fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 13:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

change gyro & accel dt from float to uint64. This has the benefit of

Browse Source 
calculating the estimator timeslip correctly.

Signed-off-by: Nicolae Rosia <nicolae.rosia@gmail.com>
This commit is contained in:
Nicolae Rosia
2017-07-12 17:38:35 +03:00
committed by Lorenz Meier
parent 5618f34f3b
commit 0a22a9c47c
6 changed files with 25 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+11 -9
View File
@@ -582,14 +582,16 @@ void Ekf2::run()
// push imu data into estimator
float gyro_integral[3];
gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
gyro_integral[0] = sensors.gyro_rad[0] * gyro_dt;
gyro_integral[1] = sensors.gyro_rad[1] * gyro_dt;
gyro_integral[2] = sensors.gyro_rad[2] * gyro_dt;
float accel_integral[3];
accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
accel_integral[0] = sensors.accelerometer_m_s2[0] * accel_dt;
accel_integral[1] = sensors.accelerometer_m_s2[1] * accel_dt;
accel_integral[2] = sensors.accelerometer_m_s2[2] * accel_dt;
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
gyro_integral, accel_integral);
// read mag data
@@ -780,7 +782,7 @@ void Ekf2::run()
start_time_us = now;
} else if (start_time_us > 0) {
integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6);
integrated_time_us += sensors.gyro_integral_dt;
}
matrix::Quaternion<float> q;
@@ -1160,7 +1162,7 @@ void Ekf2::run()
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
float alpha = math::constrain(sensors.accelerometer_integral_dt * _innov_lpf_tau_inv, 0.0f, 1.0f);
float alpha = math::constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f);
float beta = 1.0f - alpha;
_vel_innov_lpf_ned(0) = beta * _vel_innov_lpf_ned(0) + alpha * math::constrain(innovations.vel_pos_innov[0],
-_vel_innov_spike_lim, _vel_innov_spike_lim);
src/modules/sdlog2/sdlog2_messages.h
+2 -2
View File
@@ -518,8 +518,8 @@ struct log_EST5_s {
#define LOG_RPL1_MSG 51
struct log_RPL1_s {
uint64_t time_ref;
float gyro_integral_dt;
float accelerometer_integral_dt;
uint64_t gyro_integral_dt;
uint64_t accelerometer_integral_dt;
uint64_t magnetometer_timestamp;
uint64_t baro_timestamp;
float gyro_x_rad;
src/modules/sensors/voted_sensors_update.cpp
+4 -4
View File
@@ -563,7 +563,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
accel_data = math::Vector<3>(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 / 1.e6f;
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
} else {
// using the value instead of the integral (the integral is the prefered choice)
@@ -579,7 +579,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// approximate the delta time using the difference in accel data time stamps
_last_sensor_data[uorb_index].accelerometer_integral_dt =
(accel_report.timestamp - _last_accel_timestamp[uorb_index]) / 1.e6f;
(accel_report.timestamp - _last_accel_timestamp[uorb_index]);
}
// handle temperature compensation
@@ -670,7 +670,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
gyro_rate = math::Vector<3>(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 / 1.e6f;
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
} else {
//using the value instead of the integral (the integral is the prefered choice)
@@ -686,7 +686,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
// approximate the delta time using the difference in gyro data time stamps
_last_sensor_data[uorb_index].gyro_integral_dt =
(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp) / 1.e6f;
(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp);
}
// handle temperature compensation