From e421260f7c3e2a3f7145c2f643f8440060a84777 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:57:53 +0100 Subject: Removed bogus sensor counters, replaced them with proper timestamps --- .../attitude_estimator_ekf_main.cpp | 14 ++++------ .../attitude_estimator_so3_main.cpp | 13 ++++------ src/modules/mavlink/mavlink_receiver.cpp | 11 ++++---- src/modules/mavlink/orb_listener.c | 24 ++++++++--------- .../position_estimator_inav_main.c | 16 ++++++------ src/modules/sdlog2/sdlog2.c | 30 +++++++++++----------- src/modules/sensors/sensors.cpp | 13 ++++------ src/modules/uORB/topics/sensor_combined.h | 17 ++++++------ 8 files changed, 63 insertions(+), 75 deletions(-) (limited to 'src/modules') 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 a70a14fe4..1741fab8b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -249,7 +249,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; @@ -333,9 +332,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -345,11 +343,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } z_k[3] = raw.accelerometer_m_s2[0]; @@ -357,11 +354,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613..e2d69cd61 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -526,9 +526,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,11 +537,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,11 +548,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..37e5b8c61 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -388,7 +388,6 @@ handle_message(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = hil_counter; /* accelerometer */ static const float mg2ms2 = 9.8f / 1000.0f; @@ -400,7 +399,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.accelerometer_timestamp = hil_sensors.timestamp; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -418,17 +417,17 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_timestamp = hil_sensors.timestamp; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; + hil_sensors.baro_timestamp = hil_sensors.timestamp; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + hil_sensors.differential_pressure_timestamp = hil_sensors.timestamp; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; @@ -829,7 +828,7 @@ receive_thread(void *arg) while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + if (nread < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be..4a4ebf709 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -181,33 +181,33 @@ l_sensor_combined(const struct listener *l) /* mark individual fields as changed */ uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; + static hrt_abstime accel_counter = 0; + static hrt_abstime gyro_counter = 0; + static hrt_abstime mag_counter = 0; + static hrt_abstime baro_counter = 0; - if (accel_counter != raw.accelerometer_counter) { + if (accel_counter != raw.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; + accel_counter = raw.accelerometer_timestamp; } - if (gyro_counter != raw.gyro_counter) { + if (gyro_counter != raw.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; + gyro_counter = raw.timestamp; } - if (mag_counter != raw.magnetometer_counter) { + if (mag_counter != raw.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; + mag_counter = raw.magnetometer_timestamp; } - if (baro_counter != raw.baro_counter) { + if (baro_counter != raw.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; + baro_counter = raw.baro_timestamp; } if (gcs_link) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d92..1270ff5cf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -177,8 +177,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime landed_time = 0; bool flag_armed = false; - uint32_t accel_counter = 0; - uint32_t baro_counter = 0; + hrt_abstime accel_timestamp = 0; + hrt_abstime baro_timestamp = 0; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -242,8 +242,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_counter != baro_counter) { - baro_counter = sensor.baro_counter; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { @@ -354,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter != accel_counter) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -376,13 +376,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(accel_corr, 0, sizeof(accel_corr)); } - accel_counter = sensor.accelerometer_counter; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - if (sensor.baro_counter != baro_counter) { + if (sensor.baro_timestamp != baro_timestamp) { baro_corr = - sensor.baro_alt_meter - z_est[0]; - baro_counter = sensor.baro_counter; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13c..1b9137462 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -903,11 +903,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - uint16_t gyro_counter = 0; - uint16_t accelerometer_counter = 0; - uint16_t magnetometer_counter = 0; - uint16_t baro_counter = 0; - uint16_t differential_pressure_counter = 0; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; + hrt_abstime differential_pressure_timestamp = 0; /* enable logging on start if needed */ if (log_on_start) @@ -1003,28 +1003,28 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; + if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; write_IMU = true; } - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; + if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; write_IMU = true; } - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; + if (buf.sensor.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; write_SENS = true; } - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; + if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { + differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; write_SENS = true; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c27..a494cd08d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_counter++; + raw.accelerometer_timestamp = accel_report.timestamp; } } @@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_counter++; + raw.timestamp = gyro_report.timestamp; } } @@ -987,7 +987,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_counter++; + raw.magnetometer_timestamp = mag_report.timestamp; } } @@ -1005,7 +1005,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_timestamp = _barometer.timestamp; } } @@ -1019,7 +1019,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + raw.differential_pressure_timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, @@ -1560,9 +1560,6 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); - /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e..92812efd4 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** -- cgit v1.2.3