forked from Archive/PX4-Autopilot
sensor_combined.msg: make timestamps relative
This is needed for the new logger & saves some space as well.
This commit is contained in:
parent
c66f26245c
commit
c5ea4b43be
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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++;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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");
|
||||
|
|
Loading…
Reference in New Issue