fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 09:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_combined.msg: make timestamps relative

Browse Source 
This is needed for the new logger & saves some space as well.
This commit is contained in:
Beat Küng
2016-07-07 11:35:50 +02:00
committed by Lorenz Meier
parent c66f26245c
commit c5ea4b43be
12 changed files with 94 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+7 -6
View File
@@ -644,8 +644,9 @@ void AttitudePositionEstimatorEKF::task_main()
* We run the filter only once all data has been fetched
**/
if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
&& _sensor_combined.magnetometer_timestamp) {
if (_baro_init && _sensor_combined.timestamp &&
_sensor_combined.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID &&
_sensor_combined.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1365,7 +1366,7 @@ void AttitudePositionEstimatorEKF::pollData()
perf_count(_perf_gyro);
if (_last_accel != _sensor_combined.accelerometer_timestamp) {
if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
@@ -1374,18 +1375,18 @@ void AttitudePositionEstimatorEKF::pollData()
float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
_ekf->accel = _ekf->dVelIMU / accel_dt;
_last_accel = _sensor_combined.accelerometer_timestamp;
_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
}
Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
if (mag.length() > 0.1f && _last_mag != _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative) {
_ekf->magData.x = mag.x;
_ekf->magData.y = mag.y;
_ekf->magData.z = mag.z;
_newDataMag = true;
_last_mag = _sensor_combined.magnetometer_timestamp;
_last_mag = _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative;
perf_count(_perf_mag);
}