fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 18:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed bogus sensor counters, replaced them with proper timestamps

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-12 12:57:53 +01:00
parent 0d3a743f75
commit e421260f7c
8 changed files with 63 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+5 -9
View File
@@ -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];
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+5 -8
View File
@@ -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];
src/modules/mavlink/mavlink_receiver.cpp
+5 -6
View File
@@ -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);
}
src/modules/mavlink/orb_listener.c
+12 -12
View File
@@ -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)
src/modules/position_estimator_inav/position_estimator_inav_main.c
+8 -8
View File
@@ -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++;
}
}
src/modules/sdlog2/sdlog2.c
+15 -15
View File
@@ -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;
}
src/modules/sensors/sensors.cpp
+5 -8
View File
@@ -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);
src/modules/uORB/topics/sensor_combined.h
+8 -9
View File
@@ -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 */
};
/**