zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Removed stupid sensor counter, replaced it with much more useful timestamps

sbg
Lorenz Meier 11 years ago
parent
eccbccb826
commit
717e1bd374
6 changed files with 43 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
  2. 10
      src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
  3. 16
      src/modules/position_estimator_inav/position_estimator_inav_main.c
  4. 31
      src/modules/sdlog2/sdlog2.c
  5. 10
      src/modules/sensors/sensors.cpp
  6. 17
      src/modules/uORB/topics/sensor_combined.h

10
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
Unescape View File

@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// XXX write this out to perf regs // XXX write this out to perf regs
/* keep track of sensor updates */ /* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params; struct attitude_estimator_ekf_params ekf_params;
@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint8_t update_vect[3] = {0, 0, 0}; uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */ /* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) { if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1; update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp; sensor_last_timestamp[0] = raw.timestamp;
} }
@ -392,9 +390,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */ /* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) { if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1; update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp; sensor_last_timestamp[1] = raw.timestamp;
} }
@ -445,9 +442,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
z_k[5] = raw.accelerometer_m_s2[2] - acc(2); z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */ /* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) { if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1; update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp; sensor_last_timestamp[2] = raw.timestamp;
} }

10
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
Unescape View File

@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
// XXX write this out to perf regs // XXX write this out to perf regs
/* keep track of sensor updates */ /* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_params so3_comp_params; struct attitude_estimator_so3_params so3_comp_params;
@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
uint8_t update_vect[3] = {0, 0, 0}; uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */ /* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) { if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1; update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp; sensor_last_timestamp[0] = raw.timestamp;
} }
@ -538,9 +536,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */ /* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) { if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1; update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp; sensor_last_timestamp[1] = raw.timestamp;
} }
@ -550,9 +547,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
acc[2] = raw.accelerometer_m_s2[2]; acc[2] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */ /* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) { if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1; update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp; sensor_last_timestamp[2] = raw.timestamp;
} }

16
src/modules/position_estimator_inav/position_estimator_inav_main.c
Unescape View File

@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool landed = true; bool landed = true;
hrt_abstime landed_time = 0; hrt_abstime landed_time = 0;
uint32_t accel_counter = 0; hrt_abstime accel_timestamp = 0;
uint32_t baro_counter = 0; hrt_abstime baro_timestamp = 0;
bool ref_inited = false; bool ref_inited = false;
hrt_abstime ref_init_start = 0; hrt_abstime ref_init_start = 0;
@ -310,8 +310,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds_init[0].revents & POLLIN) { if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_counter != baro_counter) { if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
baro_counter = sensor.baro_counter; baro_timestamp = sensor.baro_timestamp;
/* mean calculation over several measurements */ /* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) { if (baro_init_cnt < baro_init_num) {
@ -384,7 +384,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); 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) { if (att.R_valid) {
/* correct accel bias */ /* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[0] -= acc_bias[0];
@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_acc, 0, sizeof(corr_acc));
} }
accel_counter = sensor.accelerometer_counter; accel_timestamp = sensor.accelerometer_timestamp;
accel_updates++; accel_updates++;
} }
if (sensor.baro_counter != baro_counter) { if (sensor.baro_timestamp != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter; baro_timestamp = sensor.baro_timestamp;
baro_updates++; baro_updates++;
} }
} }

31
src/modules/sdlog2/sdlog2.c
Unescape View File

@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_cond_init(&logbuffer_cond, NULL); pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */ /* track changes in sensor_combined topic */
uint16_t gyro_counter = 0; hrt_abstime gyro_timestamp = 0;
uint16_t accelerometer_counter = 0; hrt_abstime accelerometer_timestamp = 0;
uint16_t magnetometer_counter = 0; hrt_abstime magnetometer_timestamp = 0;
uint16_t baro_counter = 0; hrt_abstime barometer_timestamp = 0;
uint16_t differential_pressure_counter = 0; hrt_abstime differential_pressure_timestamp = 0;
/* track changes in distance status */ /* track changes in distance status */
bool dist_bottom_present = false; bool dist_bottom_present = false;
@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[])
bool write_IMU = false; bool write_IMU = false;
bool write_SENS = false; bool write_SENS = false;
if (buf.sensor.gyro_counter != gyro_counter) { if (buf.sensor.timestamp != gyro_timestamp) {
gyro_counter = buf.sensor.gyro_counter; gyro_timestamp = buf.sensor.timestamp;
write_IMU = true; write_IMU = true;
} }
if (buf.sensor.accelerometer_counter != accelerometer_counter) { if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_counter = buf.sensor.accelerometer_counter; accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true; write_IMU = true;
} }
if (buf.sensor.magnetometer_counter != magnetometer_counter) { if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_counter = buf.sensor.magnetometer_counter; magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true; write_IMU = true;
} }
if (buf.sensor.baro_counter != baro_counter) { if (buf.sensor.baro_timestamp != barometer_timestamp) {
baro_counter = buf.sensor.baro_counter; barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true; write_SENS = true;
} }
if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
differential_pressure_counter = buf.sensor.differential_pressure_counter; differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
write_SENS = true; write_SENS = true;
} }
@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS); LOGBUFFER_WRITE_AND_COUNT(SENS);
} }
} }
/* --- ATTITUDE --- */ /* --- ATTITUDE --- */

10
src/modules/sensors/sensors.cpp
Unescape View File

@ -940,7 +940,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw; raw.accelerometer_raw[2] = accel_report.z_raw;
raw.accelerometer_counter++; raw.accelerometer_timestamp = accel_report.timestamp;
} }
} }
@ -966,7 +966,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[1] = gyro_report.y_raw;
raw.gyro_raw[2] = gyro_report.z_raw; raw.gyro_raw[2] = gyro_report.z_raw;
raw.gyro_counter++; raw.timestamp = gyro_report.timestamp;
} }
} }
@ -996,7 +996,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[1] = mag_report.y_raw;
raw.magnetometer_raw[2] = mag_report.z_raw; raw.magnetometer_raw[2] = mag_report.z_raw;
raw.magnetometer_counter++; raw.magnetometer_timestamp = mag_report.timestamp;
} }
} }
@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
raw.baro_counter++; raw.baro_timestamp = _barometer.timestamp;
} }
} }
@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; 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.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, _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,

17
src/modules/uORB/topics/sensor_combined.h
Unescape 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 */ /* 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 */ 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 */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */
int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ 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 */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
int accelerometer_mode; /**< Accelerometer measurement mode */ int accelerometer_mode; /**< Accelerometer measurement mode */
float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ 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 */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
int magnetometer_mode; /**< Magnetometer measurement mode */ int magnetometer_mode; /**< Magnetometer measurement mode */
float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_range_ga; /**< ± measurement range in Gauss */
float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ 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_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */ 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 adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ 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 */ float differential_pressure_pa; /**< Airspeed sensor differential pressure */
uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */
}; };
/** /**

Write Preview
Loading…
Cancel
Save