fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 16:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_combined cleanup: remove many unneeded fields

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+24 -12
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);
}
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));
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+14 -17
View File
@@ -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);
}
}
src/modules/commander/calibration_routines.cpp
+1 -12
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++) {
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;
src/modules/ekf2/ekf2_main.cpp
+16 -7
View File
@@ -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<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];
_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) {
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+24 -50
View File
@@ -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();
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+6 -2
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
/* 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> &current_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();
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -1
View File
@@ -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
src/modules/mavlink/mavlink_messages.cpp
+9 -7
View File
@@ -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];
src/modules/mavlink/mavlink_receiver.cpp
+6 -26
View File
@@ -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;
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+6 -5
View File
@@ -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];
}
}
src/modules/sdlog2/sdlog2.c
+12 -10
View File
@@ -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;
src/modules/sensors/sensors.cpp
+67 -65
View File
@@ -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();