fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 20:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move voting into sensors module

Browse Source 
- voting is now at a central place instead of duplicated within the
  estimators
  -> this also means that estimators that did not do voting so far,
     now have voting, like ekf2
- estimators requiring more than that can still subscribe to the raw
  sensors
- allows sensors_combined to be 3 times smaller
  - reduces logger, memcpy (cache) & RAM overhead
- all modules requiring only 1 or 2 sensor values now automatically get
  the voted result
- this also adds voting to baro
This commit is contained in:
Beat Küng
2016-07-07 11:35:50 +02:00
committed by Lorenz Meier
parent c50d267bfb
commit d846ad5dac
23 changed files with 560 additions and 632 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+14 -14
View File
@@ -724,39 +724,39 @@ protected:
if (_sensor_sub->update(&_sensor_time, &sensor)) {
uint16_t fields_updated = 0;
if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
if (_accel_timestamp != sensor.accelerometer_timestamp) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
_accel_timestamp = sensor.accelerometer_timestamp[0];
_accel_timestamp = sensor.accelerometer_timestamp;
}
if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
if (_gyro_timestamp != sensor.timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
_gyro_timestamp = sensor.gyro_timestamp[0];
_gyro_timestamp = sensor.timestamp;
}
if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
if (_mag_timestamp != sensor.magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
_mag_timestamp = sensor.magnetometer_timestamp[0];
_mag_timestamp = sensor.magnetometer_timestamp;
}
if (_baro_timestamp != sensor.baro_timestamp[0]) {
if (_baro_timestamp != sensor.baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
_baro_timestamp = sensor.baro_timestamp[0];
_baro_timestamp = sensor.baro_timestamp;
}
_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
mavlink_highres_imu_t msg;
msg.time_usec = sensor.timestamp;
float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = sensor.accelerometer_integral_dt / 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;
float gyro_dt = sensor.gyro_integral_dt / 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;
@@ -765,8 +765,8 @@ protected:
msg.zmag = sensor.magnetometer_ga[2];
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];
msg.pressure_alt = sensor.baro_alt_meter;
msg.temperature = sensor.baro_temp_celcius;
msg.fields_updated = fields_updated;
mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
@@ -1014,7 +1014,7 @@ protected:
/* fall back to baro altitude */
sensor_combined_s sensor;
(void)_sensor_sub->update(&_sensor_time, &sensor);
msg.alt = sensor.baro_alt_meter[0];
msg.alt = sensor.baro_alt_meter;
}
msg.climb = -pos.vel_d;
@@ -3121,7 +3121,7 @@ protected:
msg.time_usec = hrt_absolute_time();
msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN;
msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN;
msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN;
msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN;
src/modules/mavlink/mavlink_receiver.cpp
+8 -10
View File
@@ -1673,16 +1673,14 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
struct sensor_combined_s hil_sensors = {};
hil_sensors.timestamp = timestamp;
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;
_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_integral_dt = dt * 1e6f;
hil_sensors.timestamp = timestamp;
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;
@@ -1690,17 +1688,17 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
_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_timestamp[0] = timestamp;
hil_sensors.accelerometer_integral_dt = dt * 1e6f;
hil_sensors.accelerometer_timestamp = timestamp;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_timestamp[0] = timestamp;
hil_sensors.magnetometer_timestamp = timestamp;
hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
hil_sensors.baro_temp_celcius[0] = imu.temperature;
hil_sensors.baro_timestamp[0] = timestamp;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.baro_timestamp = timestamp;
/* publish combined sensor topic */
if (_sensors_pub == nullptr) {