From c5ea4b43be86d11901bcf50cdd369bdc0307b212 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 25 Jun 2016 15:57:03 +0200 Subject: [PATCH] sensor_combined.msg: make timestamps relative This is needed for the new logger & saves some space as well. --- msg/sensor_combined.msg | 8 +-- .../attitude_estimator_ekf_main.cpp | 8 +-- .../attitude_estimator_q_main.cpp | 4 +- src/modules/commander/commander.cpp | 3 +- src/modules/ekf2/ekf2_main.cpp | 16 ++++-- src/modules/ekf2_replay/ekf2_replay_main.cpp | 4 +- .../ekf_att_pos_estimator_main.cpp | 13 ++--- src/modules/mavlink/mavlink_messages.cpp | 12 ++--- src/modules/mavlink/mavlink_receiver.cpp | 6 +-- .../position_estimator_inav_main.cpp | 12 ++--- src/modules/sdlog2/sdlog2.c | 12 ++--- src/modules/sensors/sensors.cpp | 49 +++++++++++++++---- 12 files changed, 94 insertions(+), 53 deletions(-) diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index f474900a34..12300a233e 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -5,18 +5,20 @@ # change with board revisions and sensor updates. # +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid + # gyro timstamp is equal to the timestamp of the message float32[3] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame uint32 gyro_integral_dt # delta time for gyro integral in us -uint64 accelerometer_timestamp # Accelerometer timestamp +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp float32[3] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 uint32 accelerometer_integral_dt # delta time for accel integral in us -uint64 magnetometer_timestamp # Magnetometer timestamp +int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss -uint64 baro_timestamp # Barometer timestamp +int32 baro_timestamp_relative # timestamp + baro_timestamp_relative = Barometer timestamp float32 baro_alt_meter # Altitude, already temp. comp. float32 baro_temp_celcius # Temperature in degrees celsius diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1a66b19f94..689d44e6aa 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp; + sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative; } hrt_abstime vel_t = 0; @@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw_accel(2) - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.magnetometer_timestamp; + sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative; } bool vision_updated = false; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f56a6793aa..60641f1bc0 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -334,7 +334,7 @@ void AttitudeEstimatorQ::task_main() _gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt; } - if (sensors.accelerometer_timestamp > 0) { + if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { float accel_dt = sensors.accelerometer_integral_dt / 1.e6f; _accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt; _accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt; @@ -346,7 +346,7 @@ void AttitudeEstimatorQ::task_main() } } - if (sensors.magnetometer_timestamp > 0) { + if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { _mag(0) = sensors.magnetometer_ga[0]; _mag(1) = sensors.magnetometer_ga[1]; _mag(2) = sensors.magnetometer_ga[2]; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c55c2d57d0..7af9ce2f6e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1788,7 +1788,8 @@ int commander_thread_main(int argc, char *argv[]) * vertical separation from other airtraffic the operator has to know when the * barometer is inoperational. * */ - if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + hrt_abstime baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative; + if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status_flags.barometer_failure) { status_flags.barometer_failure = false; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index cb6e21d3aa..ea0a1ffada 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -509,10 +509,18 @@ void Ekf2::task_main() sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s); // read mag data - _ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga); + if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + _ekf.setMagData(0, sensors.magnetometer_ga); + } else { + _ekf.setMagData(sensors.timestamp + sensors.magnetometer_timestamp_relative, sensors.magnetometer_ga); + } // read baro data - _ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter); + if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { + _ekf.setBaroData(0, &sensors.baro_alt_meter); + } else { + _ekf.setBaroData(sensors.timestamp + sensors.baro_timestamp_relative, &sensors.baro_alt_meter); + } // read gps data if available if (gps_updated) { @@ -907,8 +915,8 @@ void Ekf2::task_main() replay.time_ref = now; replay.gyro_integral_dt = sensors.gyro_integral_dt; replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; - replay.magnetometer_timestamp = sensors.magnetometer_timestamp; - replay.baro_timestamp = sensors.baro_timestamp; + replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative; + replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative; memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad)); memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s, sizeof(replay.accelerometer_integral_m_s)); diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index a29a21774d..bdf823d335 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -379,8 +379,8 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _sensors.timestamp = replay_part1.time_ref; _sensors.gyro_integral_dt = replay_part1.gyro_integral_dt; _sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt; - _sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp; - _sensors.baro_timestamp = replay_part1.baro_timestamp; + _sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp); + _sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp); _sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad; _sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad; _sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d426c8d3ea..d1588f5a81 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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); } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index da77fd07ba..3c5f24b925 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -724,10 +724,10 @@ protected: if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.timestamp + sensor.accelerometer_timestamp_relative) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; } if (_gyro_timestamp != sensor.timestamp) { @@ -736,16 +736,16 @@ protected: _gyro_timestamp = sensor.timestamp; } - if (_mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative; } - if (_baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; } _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 62cf3b3f9c..edb2a8f73b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1689,16 +1689,16 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) _hil_prev_accel[1] = imu.yacc; _hil_prev_accel[2] = imu.zacc; hil_sensors.accelerometer_integral_dt = dt * 1e6f; - hil_sensors.accelerometer_timestamp = timestamp; + hil_sensors.accelerometer_timestamp_relative = 0; hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_timestamp = timestamp; + hil_sensors.magnetometer_timestamp_relative = 0; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_timestamp = timestamp; + hil_sensors.baro_timestamp_relative = 0; /* publish combined sensor topic */ if (_sensors_pub == nullptr) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 7ef5cd7106..d084517b3e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -425,8 +425,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_timestamp != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp; + if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) { + baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; baro_wait_for_sample_time = hrt_absolute_time(); /* mean calculation over several measurements */ @@ -502,7 +502,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_timestamp != accel_timestamp) { + if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) { if (att.R_valid) { float sensor_accel[3]; float accel_dt = sensor.accelerometer_integral_dt / 1.e6f; @@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp; + accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; accel_updates++; } - if (sensor.baro_timestamp != baro_timestamp) { + if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_timestamp = sensor.baro_timestamp; + baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3ad1925c6f..4accf90a79 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1642,18 +1642,18 @@ int sdlog2_thread_main(int argc, char *argv[]) write_IMU = true; } - if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { - accelerometer_timestamp = buf.sensor.accelerometer_timestamp; + if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative; write_IMU = true; } - if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { - magnetometer_timestamp = buf.sensor.magnetometer_timestamp; + if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative; write_IMU = true; } - if (buf.sensor.baro_timestamp != barometer_timestamp) { - barometer_timestamp = buf.sensor.baro_timestamp; + if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) { + barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative; write_SENS = true; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a268d9b24b..4b24e78f5a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -210,7 +210,8 @@ private: struct SensorData { SensorData() - : subscription_count(0), + : last_best_vote(0), + subscription_count(0), voter(SENSOR_COUNT_MAX), last_failover_count(0) { @@ -221,6 +222,7 @@ private: int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */ + uint8_t last_best_vote; /**< index of the latest best vote */ int subscription_count; DataValidatorGroup voter; unsigned int last_failover_count; @@ -267,6 +269,9 @@ private: float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */ float _last_best_baro_pressure; /**< pressure from last best baro */ sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ hrt_abstime _vibration_warning_timestamp; bool _vibration_warning; @@ -595,6 +600,9 @@ Sensors::Sensors() : memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); memset(&_last_sensor_data, 0, sizeof(_last_sensor_data)); + memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); + memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp)); + memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1088,19 +1096,19 @@ Sensors::accel_poll(struct sensor_combined_s &raw) sensor_value = vect_val; - if (_last_sensor_data[i].accelerometer_timestamp == 0) { - _last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000; + if (_last_accel_timestamp[i] == 0) { + _last_accel_timestamp[i] = accel_report.timestamp - 1000; } _last_sensor_data[i].accelerometer_integral_dt = - (uint32_t)(accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp); + (uint32_t)(accel_report.timestamp - _last_accel_timestamp[i]); float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f; _last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt; _last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt; _last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt; } - _last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp; + _last_accel_timestamp[i] = accel_report.timestamp; _accel.voter.put(i, accel_report.timestamp, sensor_value.data, accel_report.error_count, _accel.priority[i]); } @@ -1115,7 +1123,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1]; raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2]; raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt; - raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp; + _accel.last_best_vote = (uint8_t)best_index; } } } @@ -1189,6 +1197,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2]; raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt; raw.timestamp = _last_sensor_data[best_index].timestamp; + _gyro.last_best_vote = (uint8_t)best_index; } } } @@ -1219,7 +1228,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) _last_sensor_data[i].magnetometer_ga[1] = vect(1); _last_sensor_data[i].magnetometer_ga[2] = vect(2); - _last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp; + _last_mag_timestamp[i] = mag_report.timestamp; _mag.voter.put(i, mag_report.timestamp, vect.data, mag_report.error_count, _mag.priority[i]); } @@ -1233,7 +1242,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0]; raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1]; raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2]; - raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp; + _mag.last_best_vote = (uint8_t)best_index; } } } @@ -1263,7 +1272,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) _last_sensor_data[i].baro_temp_celcius = baro_report.temperature; _last_baro_pressure[i] = baro_report.pressure; - _last_sensor_data[i].baro_timestamp = baro_report.timestamp; + _last_baro_timestamp[i] = baro_report.timestamp; _baro.voter.put(i, baro_report.timestamp, vect.data, baro_report.error_count, _baro.priority[i]); } @@ -1276,8 +1285,8 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (best_index >= 0) { raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter; raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius; - raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp; _last_best_baro_pressure = _last_baro_pressure[best_index]; + _baro.last_best_vote = (uint8_t)best_index; } } } @@ -2267,6 +2276,12 @@ Sensors::task_main() struct sensor_combined_s raw = {}; + raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + + raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + + raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + /* * do subscriptions */ @@ -2376,6 +2391,20 @@ Sensors::task_main() diff_pres_poll(raw); if (_publishing && raw.timestamp > 0) { + + /* construct relative timestamps */ + if (_last_accel_timestamp[_accel.last_best_vote]) { + raw.accelerometer_timestamp_relative = (int32_t)(_last_accel_timestamp[_accel.last_best_vote] - raw.timestamp); + } + + if (_last_mag_timestamp[_mag.last_best_vote]) { + raw.magnetometer_timestamp_relative = (int32_t)(_last_mag_timestamp[_mag.last_best_vote] - raw.timestamp); + } + + if (_last_baro_timestamp[_baro.last_best_vote]) { + raw.baro_timestamp_relative = (int32_t)(_last_baro_timestamp[_baro.last_best_vote] - raw.timestamp); + } + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); check_failover(_accel, "Accel");