fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/ekf2_replay.msg
View File

@ -1,6 +1,6 @@
float32 gyro_integral_dt # gyro integration period in s
float32 accelerometer_integral_dt # accelerometer integration period in s
uint64 gyro_integral_dt # gyro integration period in s
uint64 accelerometer_integral_dt # accelerometer integration period in s
uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us
uint64 baro_timestamp # timestamp of barometer measurement in us
uint64 rng_timestamp # timestamp of range finder measurement in us

4
msg/sensor_combined.msg
View File

@ -10,11 +10,11 @@ int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relat
# gyro timstamp is equal to the timestamp of the message
float32[3] gyro_rad # average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period
float32 gyro_integral_dt # gyro measurement sampling period in s
uint64 gyro_integral_dt # gyro measurement sampling period in s
int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
float32 accelerometer_integral_dt # accelerometer measurement sampling period in s
uint64 accelerometer_integral_dt # accelerometer measurement sampling period in s
int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss

8
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -1359,7 +1359,7 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->angRate.y = _sensor_combined.gyro_rad[1];
_ekf->angRate.z = _sensor_combined.gyro_rad[2];
float gyro_dt = _sensor_combined.gyro_integral_dt;
float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
_ekf->dAngIMU = _ekf->angRate * gyro_dt;
perf_count(_perf_gyro);
@ -1370,7 +1370,7 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
float accel_dt = _sensor_combined.accelerometer_integral_dt;
float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
_ekf->dVelIMU = _ekf->accel * accel_dt;
_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
@ -1394,8 +1394,8 @@ void AttitudePositionEstimatorEKF::pollData()
// leave this in as long as larger improvements are still being made.
#if 0
float deltaTIntegral = _sensor_combined.gyro_integral_dt;
float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt;
float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1.e6f;
float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1.e6f;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;

20
src/modules/ekf2/ekf2_main.cpp
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);

4
src/modules/sdlog2/sdlog2_messages.h
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;

8
src/modules/sensors/voted_sensors_update.cpp
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