sensor_combined.msg: make timestamps relative

This is needed for the new logger & saves some space as well.
This commit is contained in:
Beat Küng 2016-06-25 15:57:03 +02:00 committed by Lorenz Meier
parent c66f26245c
commit c5ea4b43be
12 changed files with 94 additions and 53 deletions

View File

@ -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

View File

@ -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;

View File

@ -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];

View File

@ -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;

View File

@ -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));

View File

@ -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;

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);
}

View File

@ -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);

View File

@ -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) {

View File

@ -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++;
}
}

View File

@ -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;
}

View File

@ -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");