From b4ecc5a8d961d31ef7eb66b8f5ea097568e914f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 24 Jun 2016 12:52:16 +0200 Subject: [PATCH] sensor_combined cleanup: remove many unneeded fields Decreases the message size from 780 to 280 bytes. In particular, all modules using sensor_combined must use the integral now. The sensor value can easily be reconstructed by dividing with dt. Voters now need to be moved into sensors module, because error count and priority is removed from the topic. Any module that requires additional data from a sensor can subscribe to the raw sensor topics. At two places, values are set to zero instead of subscribing to the raw sensors (with the assumption that no one reads them): - mavlink mavlink_highres_imu_t::abs_pressure - sdlog2: sensor temperatures --- msg/sensor_combined.msg | 49 +------ src/drivers/frsky_telemetry/frsky_data.c | 14 +- src/examples/px4_simple_app/px4_simple_app.c | 15 +- .../terrain_estimation/terrain_estimator.cpp | 6 +- .../attitude_estimator_ekf_main.cpp | 36 +++-- .../attitude_estimator_q_main.cpp | 33 ++--- .../commander/calibration_routines.cpp | 13 +- src/modules/ekf2/ekf2_main.cpp | 23 ++- .../ekf_att_pos_estimator_main.cpp | 74 ++++------ .../fw_pos_control_l1_main.cpp | 8 +- .../BlockLocalPositionEstimator.cpp | 6 +- src/modules/mavlink/mavlink_messages.cpp | 16 ++- src/modules/mavlink/mavlink_receiver.cpp | 34 +---- .../position_estimator_inav_main.cpp | 11 +- src/modules/sdlog2/sdlog2.c | 22 +-- src/modules/sensors/sensors.cpp | 132 +++++++++--------- 16 files changed, 218 insertions(+), 274 deletions(-) diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 206258e31d..790cdf3c68 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -1,63 +1,22 @@ -# Definition of the sensor_combined uORB topic. - -int32 MAGNETOMETER_MODE_NORMAL = 0 -int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 -int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 - -uint32 SENSOR_PRIO_MIN = 0 -uint32 SENSOR_PRIO_VERY_LOW = 25 -uint32 SENSOR_PRIO_LOW = 50 -uint32 SENSOR_PRIO_DEFAULT = 75 -uint32 SENSOR_PRIO_HIGH = 100 -uint32 SENSOR_PRIO_VERY_HIGH = 125 -uint32 SENSOR_PRIO_MAX = 255 - -# Sensor readings in raw and SI-unit form. # -# These values are read from the sensors. Raw values are in sensor-specific units, -# the scaled values are in SI-units, as visible from the ending of the variable -# or the comments. The use of the SI fields is in general advised, as these fields -# are scaled and offset-compensated where possible and do not change with board -# revisions and sensor updates. +# Sensor readings in SI-unit form. # -# Actual data, this is specific to the type of data which is stored in this struct -# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# These fields are scaled and offset-compensated where possible and do not +# change with board revisions and sensor updates. # uint64[3] gyro_timestamp # Gyro timestamps -int16[9] gyro_raw # Raw sensor values of angular velocity -float32[9] gyro_rad_s # Angular velocity in radian per seconds -uint32[3] gyro_priority # Sensor priority -float32[9] gyro_integral_rad # delta angle in radians +float32[9] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame uint64[3] gyro_integral_dt # delta time for gyro integral in us -uint32[3] gyro_errcount # Error counter for gyro 0 -float32[3] gyro_temp # Temperature of gyro 0 -int16[9] accelerometer_raw # Raw acceleration in NED body frame -float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 uint64[3] accelerometer_integral_dt # delta time for accel integral in us -int16[3] accelerometer_mode # Accelerometer measurement mode -float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 uint64[3] accelerometer_timestamp # Accelerometer timestamp -uint32[3] accelerometer_priority # Sensor priority -uint32[3] accelerometer_errcount # Error counter for accel 0 -float32[3] accelerometer_temp # Temperature of accel 0 -int16[9] magnetometer_raw # Raw magnetic field in NED body frame float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss -int16[3] magnetometer_mode # Magnetometer measurement mode -float32[3] magnetometer_range_ga # measurement range in Gauss -float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor uint64[3] magnetometer_timestamp # Magnetometer timestamp -uint32[3] magnetometer_priority # Sensor priority -uint32[3] magnetometer_errcount # Error counter for mag 0 -float32[3] magnetometer_temp # Temperature of mag 0 -float32[3] baro_pres_mbar # Barometric pressure, already temp. comp. float32[3] baro_alt_meter # Altitude, already temp. comp. float32[3] baro_temp_celcius # Temperature in degrees celsius uint64[3] baro_timestamp # Barometer timestamp -uint32[3] baro_priority # Sensor priority -uint32[3] baro_errcount # Error count in communication diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index a5304603fd..246ad91d42 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -204,12 +204,14 @@ void frsky_update_topics() void frsky_send_frame1(int uart) { /* send formatted frame */ - frsky_send_data(uart, FRSKY_ID_ACCEL_X, - roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, - roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f)); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, - roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f)); + float acceleration[3]; + float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f; + acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt; + acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt; + acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt; + frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, sensor_combined->baro_alt_meter[0]); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 23cd124130..a382c2224a 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -101,15 +101,18 @@ int px4_simple_app_main(int argc, char *argv[]) struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); + float sensor_accel[3]; + float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f; + sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt; + sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt; + sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt; PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]); /* set att and publish this information for other apps */ - att.roll = raw.accelerometer_m_s2[0]; - att.pitch = raw.accelerometer_m_s2[1]; - att.yaw = raw.accelerometer_m_s2[2]; + att.roll = sensor_accel[0]; + att.pitch = sensor_accel[1]; + att.yaw = sensor_accel[2]; orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 20bdabf6e5..d8c88e618d 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -67,7 +67,11 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); - matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector a; + float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f; + a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt; + a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt; + a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt; matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity 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 4aa6abca72..fa22ad165f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -395,13 +395,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } + float gyro_rad_s[3]; + float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f; + gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt; + gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt; + gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt; + if (!initialized) { // XXX disabling init for now initialized = true; - // gyro_offsets[0] += raw.gyro_rad_s[0]; - // gyro_offsets[1] += raw.gyro_rad_s[1]; - // gyro_offsets[2] += raw.gyro_rad_s[2]; + // gyro_offsets[0] += gyro_rad_s[0]; + // gyro_offsets[1] += gyro_rad_s[1]; + // gyro_offsets[2] += gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { @@ -427,9 +433,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } - z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + z_k[0] = gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { @@ -470,9 +476,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) last_vel_t = 0; } - z_k[3] = raw.accelerometer_m_s2[0] - acc(0); - z_k[4] = raw.accelerometer_m_s2[1] - acc(1); - z_k[5] = raw.accelerometer_m_s2[2] - acc(2); + matrix::Vector3f raw_accel; + float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f; + raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt; + raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt; + raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt; + + z_k[3] = raw_accel(0) - acc(0); + z_k[4] = raw_accel(1) - acc(1); + z_k[5] = raw_accel(2) - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && @@ -616,9 +628,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); - att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); - att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + att.g_comp[0] = raw_accel(0) - acc(0); + att.g_comp[1] = raw_accel(1) - acc(1); + att.g_comp[2] = raw_accel(2) - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index b2b6c6bdda..90ed78025c 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -359,30 +359,27 @@ void AttitudeEstimatorQ::task_main() if (sensors.gyro_timestamp[i] > 0) { float gyro[3]; - - for (unsigned j = 0; j < 3; j++) { - if (sensors.gyro_integral_dt[i] > 0) { - gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); - - } else { - /* fall back to angular rate */ - gyro[j] = sensors.gyro_rad_s[i * 3 + j]; - } - } - - _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); + float gyro_dt = sensors.gyro_integral_dt[i] / 1e6; + gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt; + gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt; + gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt; + + //TODO: note: voter will be moved into sensors module + //_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); + _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75); } - /* ignore empty fields */ if (sensors.accelerometer_timestamp[i] > 0) { - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + float acceleration[3]; + float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f; + acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; + acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; + acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; + _voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75); } - /* ignore empty fields */ if (sensors.magnetometer_timestamp[i] > 0) { - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75); } } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index b351ac1bd3..39dc65155e 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -273,18 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, for (unsigned i = 0; i < ndim; i++) { - float di = 0.0f; - switch (i) { - case 0: - di = sensor.accelerometer_m_s2[0]; - break; - case 1: - di = sensor.accelerometer_m_s2[1]; - break; - case 2: - di = sensor.accelerometer_m_s2[2]; - break; - } + float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f); float d = di - accel_ema[i]; accel_ema[i] += d * w; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index e1f746e58d..ddc471e234 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -614,10 +614,15 @@ void Ekf2::task_main() control_state_s ctrl_state = {}; float gyro_bias[3] = {}; _ekf.get_gyro_bias(gyro_bias); + float gyro_rad_s[3]; + float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f; + gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0]; + gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1]; + gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2]; ctrl_state.timestamp = hrt_absolute_time(); - ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0]; - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1]; - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2]; + ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]); // Velocity in body frame float velocity[3]; @@ -644,7 +649,11 @@ void Ekf2::task_main() ctrl_state.q[3] = q(3); // Acceleration data - matrix::Vector acceleration = {&sensors.accelerometer_m_s2[0]}; + matrix::Vector acceleration; + float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f; + acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt; + acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt; + acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt; float accel_bias[3]; _ekf.get_accel_bias(accel_bias); @@ -705,9 +714,9 @@ void Ekf2::task_main() att.q[3] = q(3); att.q_valid = true; - att.rollspeed = sensors.gyro_rad_s[0]; - att.pitchspeed = sensors.gyro_rad_s[1]; - att.yawspeed = sensors.gyro_rad_s[2]; + att.rollspeed = gyro_rad_s[0]; + att.pitchspeed = gyro_rad_s[1]; + att.yawspeed = gyro_rad_s[2]; // publish vehicle attitude data if (_att_pub == nullptr) { diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4fde0bea22..ac19b192fb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1384,13 +1384,21 @@ void AttitudePositionEstimatorEKF::pollData() // Feed validator with recent sensor data + //TODO: note, we will move voters into sensors module for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { - _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], - _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); - _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], - _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); - _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], - _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + float gyro_rad_s[3]; + float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f; + gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt; + gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt; + gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt; + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75); + float acceleration[3]; + float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f; + acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; + acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; + acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75); } // Get best measurement values @@ -1399,28 +1407,13 @@ void AttitudePositionEstimatorEKF::pollData() if (_gyro_main >= 0) { - // Use pre-integrated values if possible - if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { - _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; - _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; - _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; - } else { - float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; - - if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { - deltaT = dt_gyro; - _ekf->dtIMU = deltaT; - } - - _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; - _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; - _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; - } + float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f; + _ekf->angRate = _ekf->dAngIMU / gyro_dt; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; perf_count(_perf_gyro); } @@ -1428,21 +1421,13 @@ void AttitudePositionEstimatorEKF::pollData() if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { - // Use pre-integrated values if possible - if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { - _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; - _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; - _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; - } else { - _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; - _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU; - } + float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f; + _ekf->accel = _ekf->dVelIMU / accel_dt; - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; } @@ -1518,20 +1503,9 @@ void AttitudePositionEstimatorEKF::pollData() (double)(_sensor_combined.accelerometer_integral_m_s[1]), (double)(_sensor_combined.accelerometer_integral_m_s[2])); - PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); - PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); - PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", - (double)_sensor_combined.gyro_rad_s[0], - (double)_sensor_combined.gyro_rad_s[1], - (double)_sensor_combined.gyro_rad_s[2]); - lastprint = hrt_absolute_time(); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0788056104..3fddf2c7b6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1230,7 +1230,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_body; + float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f; + accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt; + accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt; + accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt; math::Vector<3> accel_earth = _R_nb * accel_body; /* tell TECS to update its state, but let it know when it cannot actually control the plane */ @@ -1703,7 +1707,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + launchDetector.update(accel_body(0)); /* update our copy of the launch detection state */ launch_detection_state = launchDetector.getLaunchDetected(); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index fb9c8ea712..493a12e997 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -844,7 +844,11 @@ void BlockLocalPositionEstimator::predict() if (integrate && _sub_att.get().R_valid) { Matrix3f R_att(_sub_att.get().R); - Vector3f a(_sub_sensor.get().accelerometer_m_s2); + Vector3f a; + float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f; + a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt; + a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt; + a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt; _u = R_att * a; _u(U_az) += 9.81f; // add g diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a20d99294b..a699974ca1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -752,16 +752,18 @@ protected: mavlink_highres_imu_t msg; msg.time_usec = sensor.timestamp; - msg.xacc = sensor.accelerometer_m_s2[0]; - msg.yacc = sensor.accelerometer_m_s2[1]; - msg.zacc = sensor.accelerometer_m_s2[2]; - msg.xgyro = sensor.gyro_rad_s[0]; - msg.ygyro = sensor.gyro_rad_s[1]; - msg.zgyro = sensor.gyro_rad_s[2]; + float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f; + msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt; + msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt; + msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt; + float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f; + msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt; + msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt; + msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt; msg.xmag = sensor.magnetometer_ga[0]; msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; - msg.abs_pressure = sensor.baro_pres_mbar[0]; + msg.abs_pressure = 0; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = sensor.baro_alt_meter[0]; msg.temperature = sensor.baro_temp_celcius[0]; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c7baa5da05..0d62fb2307 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1675,49 +1675,29 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.timestamp = timestamp; - hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; - hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; - hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - 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_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; - memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro)); + _hil_prev_gyro[0] = imu.xgyro; + _hil_prev_gyro[1] = imu.ygyro; + _hil_prev_gyro[2] = imu.zgyro; hil_sensors.gyro_integral_dt[0] = dt * 1e6f; hil_sensors.gyro_timestamp[0] = timestamp; - hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; - - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; - memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel)); + _hil_prev_accel[0] = imu.xacc; + _hil_prev_accel[1] = imu.yacc; + _hil_prev_accel[2] = imu.zacc; hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; - hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 hil_sensors.accelerometer_timestamp[0] = timestamp; - hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH; - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; - hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; - hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16 - hil_sensors.magnetometer_mode[0] = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f; hil_sensors.magnetometer_timestamp[0] = timestamp; - hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH; - hil_sensors.baro_pres_mbar[0] = imu.abs_pressure; hil_sensors.baro_alt_meter[0] = imu.pressure_alt; hil_sensors.baro_temp_celcius[0] = imu.temperature; hil_sensors.baro_timestamp[0] = timestamp; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 191b5be099..96f02298a1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -504,17 +504,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { - /* correct accel bias */ - sensor.accelerometer_m_s2[0] -= acc_bias[0]; - sensor.accelerometer_m_s2[1] -= acc_bias[1]; - sensor.accelerometer_m_s2[2] -= acc_bias[2]; + float sensor_accel[3]; + float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f; + sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0]; + sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1]; + sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; + acc[i] += PX4_R(att.R, i, j) * sensor_accel[j]; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 261b319f6e..9d0245dcaf 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1671,18 +1671,20 @@ int sdlog2_thread_main(int argc, char *argv[]) break; } - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; + float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt; + float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i]; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i]; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i]; + log_msg.body.log_IMU.temp_gyro = 0; + log_msg.body.log_IMU.temp_acc = 0; + log_msg.body.log_IMU.temp_mag = 0; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1699,7 +1701,7 @@ int sdlog2_thread_main(int argc, char *argv[]) break; } - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; + log_msg.body.log_SENS.baro_pres = 0; log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; log_msg.body.log_SENS.diff_pres = 0; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 35fbb2ed7f..e5fe8a7ecf 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -252,6 +252,10 @@ private: Battery _battery; /**< Helper lib to publish battery_status topic. */ + float _latest_baro_pressure[SENSOR_COUNT_MAX]; + hrt_abstime _last_accel_timestamp[SENSOR_COUNT_MAX]; + hrt_abstime _last_gyro_timestamp[SENSOR_COUNT_MAX]; + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -393,8 +397,7 @@ private: } _parameter_handles; /**< handles for interesting parameters */ - int init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount); + int init_sensor_class(const struct orb_metadata *meta, int *subs); /** * Update our local parameter cache. @@ -564,6 +567,9 @@ Sensors::Sensors() : memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); + memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure)); + memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); + memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1023,29 +1029,34 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + if (accel_report.integral_dt != 0) { + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.accelerometer_m_s2[i * 3 + 0] = vect(0); - raw.accelerometer_m_s2[i * 3 + 1] = vect(1); - raw.accelerometer_m_s2[i * 3 + 2] = vect(2); + raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); + raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); + raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); - math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); - vect_int = _board_rotation * vect_int; + raw.accelerometer_integral_dt[i] = accel_report.integral_dt; - raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); - raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); - raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); + } else { + //this is undesirable: a driver did not set the integral, so we have to construct it ourselves + math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z); + vect_val = _board_rotation * vect_val; - raw.accelerometer_integral_dt[i] = accel_report.integral_dt; + if (_last_accel_timestamp[i] == 0) { + _last_accel_timestamp[i] = accel_report.timestamp - 1000; + } - raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw; - raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; - raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; + raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i]; + _last_accel_timestamp[i] = accel_report.timestamp; + float dt = raw.accelerometer_integral_dt[i] / 1.e6f; + raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt; + raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt; + raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt; + } raw.accelerometer_timestamp[i] = accel_report.timestamp; - raw.accelerometer_errcount[i] = accel_report.error_count; - raw.accelerometer_temp[i] = accel_report.temperature; } } } @@ -1062,34 +1073,38 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + if (gyro_report.integral_dt != 0) { + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.gyro_rad_s[i * 3 + 0] = vect(0); - raw.gyro_rad_s[i * 3 + 1] = vect(1); - raw.gyro_rad_s[i * 3 + 2] = vect(2); + raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); + raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); + raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); - math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); - vect_int = _board_rotation * vect_int; + raw.gyro_integral_dt[i] = gyro_report.integral_dt; + raw.gyro_timestamp[i] = gyro_report.timestamp; - raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); - raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); - raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); + } else { + //this is undesirable: a driver did not set the integral, so we have to construct it ourselves + math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z); + vect_val = _board_rotation * vect_val; - raw.gyro_integral_dt[i] = gyro_report.integral_dt; + if (_last_gyro_timestamp[i] == 0) { + _last_gyro_timestamp[i] = gyro_report.timestamp - 1000; + } - raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw; - raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; - raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; + raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i]; + _last_gyro_timestamp[i] = gyro_report.timestamp; + float dt = raw.gyro_integral_dt[i] / 1.e6f; + raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt; + raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt; + raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt; - raw.gyro_timestamp[i] = gyro_report.timestamp; + } if (i == 0) { raw.timestamp = gyro_report.timestamp; } - - raw.gyro_errcount[i] = gyro_report.error_count; - raw.gyro_temp[i] = gyro_report.temperature; } } } @@ -1114,13 +1129,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_ga[i * 3 + 1] = vect(1); raw.magnetometer_ga[i * 3 + 2] = vect(2); - raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw; - raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw; - raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw; - raw.magnetometer_timestamp[i] = mag_report.timestamp; - raw.magnetometer_errcount[i] = mag_report.error_count; - raw.magnetometer_temp[i] = mag_report.temperature; } } } @@ -1136,7 +1145,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); - raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar + _latest_baro_pressure[i] = _barometer.pressure; raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius @@ -1172,12 +1181,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + //FIXME: we just use the baro pressure from the first baro. we should do voting instead. _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f, + _latest_baro_pressure[0] * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f, + _latest_baro_pressure[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -2050,8 +2060,7 @@ Sensors::task_main_trampoline(int argc, char *argv[]) } int -Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount) +Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs) { unsigned group_count = orb_group_count(meta); @@ -2062,7 +2071,6 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, for (unsigned i = 0; i < group_count; i++) { if (subs[i] < 0) { subs[i] = orb_subscribe_multi(meta, i); - orb_priority(subs[i], (int32_t *)&priorities[i]); } } @@ -2106,14 +2114,13 @@ Sensors::task_main() unsigned bcount_prev = _baro_count; - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, raw.gyro_priority, raw.gyro_errcount); + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); - _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, raw.magnetometer_priority, raw.magnetometer_errcount); + _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub); - _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, raw.accelerometer_priority, - raw.accelerometer_errcount); + _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub); - _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, raw.baro_priority, raw.baro_errcount); + _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub); if (gcount_prev != _gyro_count || mcount_prev != _mag_count || @@ -2178,8 +2185,7 @@ Sensors::task_main() * then attempt to subscribe once again */ if (_gyro_count == 0) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, - raw.gyro_priority, raw.gyro_errcount); + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); fds[0].fd = _gyro_sub[0]; } @@ -2237,17 +2243,13 @@ Sensors::task_main() * when not adding sensors poll for param updates */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, - raw.gyro_priority, raw.gyro_errcount); + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); - _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, - raw.magnetometer_priority, raw.magnetometer_errcount); + _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub); - _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, - raw.accelerometer_priority, raw.accelerometer_errcount); + _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub); - _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, - raw.baro_priority, raw.baro_errcount); + _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub); _last_config_update = hrt_absolute_time();