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]; + 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; - 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]); + //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; + float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f; + _ekf->angRate = _ekf->dAngIMU / gyro_dt; - 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; - } - - _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();