dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Beat Küng 2016-06-24 12:52:16 +02:00 committed by Lorenz Meier
parent c407123a72
commit b4ecc5a8d9
16 changed files with 216 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
msg/sensor_combined.msg
View File

@ -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, # Sensor readings in SI-unit form.
# 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.
# #
# Actual data, this is specific to the type of data which is stored in this struct # These fields are scaled and offset-compensated where possible and do not
# A line containing L0GME will be added by the Python logging code generator to the logged dataset. # change with board revisions and sensor updates.
# #
uint64[3] gyro_timestamp # Gyro timestamps uint64[3] gyro_timestamp # Gyro timestamps
int16[9] gyro_raw # Raw sensor values of angular velocity float32[9] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame
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
uint64[3] gyro_integral_dt # delta time for gyro integral in us 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 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 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 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 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 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_alt_meter # Altitude, already temp. comp.
float32[3] baro_temp_celcius # Temperature in degrees celsius float32[3] baro_temp_celcius # Temperature in degrees celsius
uint64[3] baro_timestamp # Barometer timestamp uint64[3] baro_timestamp # Barometer timestamp
uint32[3] baro_priority # Sensor priority
uint32[3] baro_errcount # Error count in communication

14
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -204,12 +204,14 @@ void frsky_update_topics()
void frsky_send_frame1(int uart) void frsky_send_frame1(int uart)
{ {
/* send formatted frame */ /* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X, float acceleration[3];
roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f)); float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f)); acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f)); 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, frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
sensor_combined->baro_alt_meter[0]); sensor_combined->baro_alt_meter[0]);

15
src/examples/px4_simple_app/px4_simple_app.c
View File

@ -101,15 +101,18 @@ int px4_simple_app_main(int argc, char *argv[])
struct sensor_combined_s raw; struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */ /* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); 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", PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
(double)raw.accelerometer_m_s2[0], (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */ /* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0]; att.roll = sensor_accel[0];
att.pitch = raw.accelerometer_m_s2[1]; att.pitch = sensor_accel[1];
att.yaw = raw.accelerometer_m_s2[2]; att.yaw = sensor_accel[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} }

6
src/lib/terrain_estimation/terrain_estimator.cpp
View File

@ -67,7 +67,11 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
{ {
if (attitude->R_valid) { if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R); matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]); matrix::Vector<float, 3> 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<float, 3> u; matrix::Vector<float, 3> u;
u = R_att * a; u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity _u_z = u(2) + 9.81f; // compensate for gravity

36
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -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); 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) { if (!initialized) {
// XXX disabling init for now // XXX disabling init for now
initialized = true; initialized = true;
// gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[0] += gyro_rad_s[0];
// gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[1] += gyro_rad_s[1];
// gyro_offsets[2] += raw.gyro_rad_s[2]; // gyro_offsets[2] += gyro_rad_s[2];
// offset_count++; // offset_count++;
// if (hrt_absolute_time() - start_time > 3000000LL) { // 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]; sensor_last_timestamp[0] = raw.gyro_timestamp[0];
} }
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[0] = gyro_rad_s[0] - gyro_offsets[0];
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[1] = gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; z_k[2] = gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */ /* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { 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; last_vel_t = 0;
} }
z_k[3] = raw.accelerometer_m_s2[0] - acc(0); matrix::Vector3f raw_accel;
z_k[4] = raw.accelerometer_m_s2[1] - acc(1); float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
z_k[5] = raw.accelerometer_m_s2[2] - acc(2); 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 */ /* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && 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.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5]; att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[0] = raw_accel(0) - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[1] = raw_accel(1) - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); att.g_comp[2] = raw_accel(2) - acc(2);
/* copy offsets */ /* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

31
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -359,30 +359,27 @@ void AttitudeEstimatorQ::task_main()
if (sensors.gyro_timestamp[i] > 0) { if (sensors.gyro_timestamp[i] > 0) {
float gyro[3]; 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++) { //TODO: note: voter will be moved into sensors module
if (sensors.gyro_integral_dt[i] > 0) { //_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75);
} 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]);
} }
/* ignore empty fields */
if (sensors.accelerometer_timestamp[i] > 0) { if (sensors.accelerometer_timestamp[i] > 0) {
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], float acceleration[3];
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); 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) { if (sensors.magnetometer_timestamp[i] > 0) {
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
} }
} }

13
src/modules/commander/calibration_routines.cpp
View File

@ -273,18 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
for (unsigned i = 0; i < ndim; i++) { for (unsigned i = 0; i < ndim; i++) {
float di = 0.0f; float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
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 d = di - accel_ema[i]; float d = di - accel_ema[i];
accel_ema[i] += d * w; accel_ema[i] += d * w;

23
src/modules/ekf2/ekf2_main.cpp
View File

@ -614,10 +614,15 @@ void Ekf2::task_main()
control_state_s ctrl_state = {}; control_state_s ctrl_state = {};
float gyro_bias[3] = {}; float gyro_bias[3] = {};
_ekf.get_gyro_bias(gyro_bias); _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.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0]; ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1]; ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2]; ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
// Velocity in body frame // Velocity in body frame
float velocity[3]; float velocity[3];
@ -644,7 +649,11 @@ void Ekf2::task_main()
ctrl_state.q[3] = q(3); ctrl_state.q[3] = q(3);
// Acceleration data // Acceleration data
matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]}; matrix::Vector<float, 3> 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]; float accel_bias[3];
_ekf.get_accel_bias(accel_bias); _ekf.get_accel_bias(accel_bias);
@ -705,9 +714,9 @@ void Ekf2::task_main()
att.q[3] = q(3); att.q[3] = q(3);
att.q_valid = true; att.q_valid = true;
att.rollspeed = sensors.gyro_rad_s[0]; att.rollspeed = gyro_rad_s[0];
att.pitchspeed = sensors.gyro_rad_s[1]; att.pitchspeed = gyro_rad_s[1];
att.yawspeed = sensors.gyro_rad_s[2]; att.yawspeed = gyro_rad_s[2];
// publish vehicle attitude data // publish vehicle attitude data
if (_att_pub == nullptr) { if (_att_pub == nullptr) {

74
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -1384,13 +1384,21 @@ void AttitudePositionEstimatorEKF::pollData()
// Feed validator with recent sensor data // 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++) { 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], float gyro_rad_s[3];
_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f;
_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt;
_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt;
_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt;
_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); _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 // Get best measurement values
@ -1399,28 +1407,13 @@ void AttitudePositionEstimatorEKF::pollData()
if (_gyro_main >= 0) { if (_gyro_main >= 0) {
// Use pre-integrated values if possible _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
_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 gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; _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); perf_count(_perf_gyro);
} }
@ -1428,21 +1421,13 @@ void AttitudePositionEstimatorEKF::pollData()
if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
// Use pre-integrated values if possible _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
_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 { float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; _ekf->accel = _ekf->dVelIMU / accel_dt;
_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;
}
_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]; _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[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2])); (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", PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); (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(); lastprint = hrt_absolute_time();
} }

8
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1230,7 +1230,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */ /* 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; 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 */ /* 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> &current_positi
} }
/* Detect launch */ /* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); launchDetector.update(accel_body(0));
/* update our copy of the launch detection state */ /* update our copy of the launch detection state */
launch_detection_state = launchDetector.getLaunchDetected(); launch_detection_state = launchDetector.getLaunchDetected();

6
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -844,7 +844,11 @@ void BlockLocalPositionEstimator::predict()
if (integrate && _sub_att.get().R_valid) { if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R); 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 = R_att * a;
_u(U_az) += 9.81f; // add g _u(U_az) += 9.81f; // add g

16
src/modules/mavlink/mavlink_messages.cpp
View File

@ -752,16 +752,18 @@ protected:
mavlink_highres_imu_t msg; mavlink_highres_imu_t msg;
msg.time_usec = sensor.timestamp; msg.time_usec = sensor.timestamp;
msg.xacc = sensor.accelerometer_m_s2[0]; float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
msg.yacc = sensor.accelerometer_m_s2[1]; msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
msg.zacc = sensor.accelerometer_m_s2[2]; msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
msg.xgyro = sensor.gyro_rad_s[0]; msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
msg.ygyro = sensor.gyro_rad_s[1]; float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f;
msg.zgyro = sensor.gyro_rad_s[2]; 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.xmag = sensor.magnetometer_ga[0];
msg.ymag = sensor.magnetometer_ga[1]; msg.ymag = sensor.magnetometer_ga[1];
msg.zmag = sensor.magnetometer_ga[2]; 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.diff_pressure = differential_pressure.differential_pressure_raw_pa;
msg.pressure_alt = sensor.baro_alt_meter[0]; msg.pressure_alt = sensor.baro_alt_meter[0];
msg.temperature = sensor.baro_temp_celcius[0]; msg.temperature = sensor.baro_temp_celcius[0];

32
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1675,49 +1675,29 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.timestamp = timestamp; 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[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[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; 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_integral_dt[0] = dt * 1e6f;
hil_sensors.gyro_timestamp[0] = timestamp; 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[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[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; 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_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_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[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag; 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_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_alt_meter[0] = imu.pressure_alt;
hil_sensors.baro_temp_celcius[0] = imu.temperature; hil_sensors.baro_temp_celcius[0] = imu.temperature;
hil_sensors.baro_timestamp[0] = timestamp; hil_sensors.baro_timestamp[0] = timestamp;

11
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -504,17 +504,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
if (att.R_valid) { if (att.R_valid) {
/* correct accel bias */ float sensor_accel[3];
sensor.accelerometer_m_s2[0] -= acc_bias[0]; float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
sensor.accelerometer_m_s2[2] -= acc_bias[2]; 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 */ /* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
acc[i] = 0.0f; acc[i] = 0.0f;
for (int j = 0; j < 3; j++) { 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];
} }
} }

22
src/modules/sdlog2/sdlog2.c
View File

@ -1671,18 +1671,20 @@ int sdlog2_thread_main(int argc, char *argv[])
break; break;
} }
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f;
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt;
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt;
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt;
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f;
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; 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_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_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.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_gyro = 0;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i]; log_msg.body.log_IMU.temp_acc = 0;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i]; log_msg.body.log_IMU.temp_mag = 0;
LOGBUFFER_WRITE_AND_COUNT(IMU); LOGBUFFER_WRITE_AND_COUNT(IMU);
} }
@ -1699,7 +1701,7 @@ int sdlog2_thread_main(int argc, char *argv[])
break; 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_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.baro_temp = buf.sensor.baro_temp_celcius[i];
log_msg.body.log_SENS.diff_pres = 0; log_msg.body.log_SENS.diff_pres = 0;

132
src/modules/sensors/sensors.cpp
View File

@ -252,6 +252,10 @@ private:
Battery _battery; /**< Helper lib to publish battery_status topic. */ 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 { struct {
float min[_rc_max_chan_count]; float min[_rc_max_chan_count];
float trim[_rc_max_chan_count]; float trim[_rc_max_chan_count];
@ -393,8 +397,7 @@ private:
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
int init_sensor_class(const struct orb_metadata *meta, int *subs, int init_sensor_class(const struct orb_metadata *meta, int *subs);
uint32_t *priorities, uint32_t *errcount);
/** /**
* Update our local parameter cache. * Update our local parameter cache.
@ -564,6 +567,9 @@ Sensors::Sensors() :
memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_diff_pres, 0, sizeof(_diff_pres));
memset(&_parameters, 0, sizeof(_parameters)); memset(&_parameters, 0, sizeof(_parameters));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); 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 */ /* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) { 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); orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); if (accel_report.integral_dt != 0) {
vect = _board_rotation * vect; 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_integral_m_s[i * 3 + 0] = vect_int(0);
raw.accelerometer_m_s2[i * 3 + 1] = vect(1); raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
raw.accelerometer_m_s2[i * 3 + 2] = vect(2); 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); raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
vect_int = _board_rotation * vect_int;
raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); } else {
raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); //this is undesirable: a driver did not set the integral, so we have to construct it ourselves
raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); 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_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i];
raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; _last_accel_timestamp[i] = accel_report.timestamp;
raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; 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_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); orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); if (gyro_report.integral_dt != 0) {
vect = _board_rotation * vect; 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_integral_rad[i * 3 + 0] = vect_int(0);
raw.gyro_rad_s[i * 3 + 1] = vect(1); raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
raw.gyro_rad_s[i * 3 + 2] = vect(2); 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); raw.gyro_integral_dt[i] = gyro_report.integral_dt;
vect_int = _board_rotation * vect_int; raw.gyro_timestamp[i] = gyro_report.timestamp;
raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); } else {
raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); //this is undesirable: a driver did not set the integral, so we have to construct it ourselves
raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); 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_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i];
raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; _last_gyro_timestamp[i] = gyro_report.timestamp;
raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; 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) { if (i == 0) {
raw.timestamp = gyro_report.timestamp; 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 + 1] = vect(1);
raw.magnetometer_ga[i * 3 + 2] = vect(2); 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_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); 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_alt_meter[i] = _barometer.altitude; // Altitude in meters
raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius 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, _airspeed.indicated_airspeed_m_s = math::max(0.0f,
calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); 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, _airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, _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, calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius; _airspeed.air_temperature_celsius = air_temperature_celsius;
@ -2050,8 +2060,7 @@ Sensors::task_main_trampoline(int argc, char *argv[])
} }
int int
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
uint32_t *priorities, uint32_t *errcount)
{ {
unsigned group_count = orb_group_count(meta); 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++) { for (unsigned i = 0; i < group_count; i++) {
if (subs[i] < 0) { if (subs[i] < 0) {
subs[i] = orb_subscribe_multi(meta, i); 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; 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, _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
raw.accelerometer_errcount);
_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 || if (gcount_prev != _gyro_count ||
mcount_prev != _mag_count || mcount_prev != _mag_count ||
@ -2178,8 +2185,7 @@ Sensors::task_main()
* then attempt to subscribe once again * then attempt to subscribe once again
*/ */
if (_gyro_count == 0) { if (_gyro_count == 0) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
raw.gyro_priority, raw.gyro_errcount);
fds[0].fd = _gyro_sub[0]; fds[0].fd = _gyro_sub[0];
} }
@ -2237,17 +2243,13 @@ Sensors::task_main()
* when not adding sensors poll for param updates * when not adding sensors poll for param updates
*/ */
if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
raw.gyro_priority, raw.gyro_errcount);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
raw.magnetometer_priority, raw.magnetometer_errcount);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
raw.accelerometer_priority, raw.accelerometer_errcount);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
raw.baro_priority, raw.baro_errcount);
_last_config_update = hrt_absolute_time(); _last_config_update = hrt_absolute_time();