fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 06:37:35 +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-06-25 12:28:02 +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
msg/sensor_combined.msg
+11 -11
View File
@@ -5,18 +5,18 @@
# change with board revisions and sensor updates.
#
uint64[3] gyro_timestamp # Gyro timestamps
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
# gyro timstamp is equal to the timestamp of the message
float32[3] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame
uint64 gyro_integral_dt # delta time for gyro integral in us
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_timestamp # Accelerometer timestamp
uint64 accelerometer_timestamp # Accelerometer timestamp
float32[3] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2
uint64 accelerometer_integral_dt # delta time for accel integral in us
float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss
uint64[3] magnetometer_timestamp # Magnetometer timestamp
uint64 magnetometer_timestamp # Magnetometer timestamp
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss
float32[3] baro_alt_meter # Altitude, already temp. comp.
float32[3] baro_temp_celcius # Temperature in degrees celsius
uint64[3] baro_timestamp # Barometer timestamp
uint64 baro_timestamp # Barometer timestamp
float32 baro_alt_meter # Altitude, already temp. comp.
float32 baro_temp_celcius # Temperature in degrees celsius
src/drivers/frsky_telemetry/frsky_data.c
+4 -4
View File
@@ -205,7 +205,7 @@ void frsky_send_frame1(int uart)
{
/* send formatted frame */
float acceleration[3];
float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = sensor_combined->accelerometer_integral_dt / 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;
@@ -214,12 +214,12 @@ void frsky_send_frame1(int uart)
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]);
sensor_combined->baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f));
roundf(frac(sensor_combined->baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(sensor_combined->baro_temp_celcius[0]));
roundf(sensor_combined->baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery_status->voltage_v * 10.0f));
src/drivers/frsky_telemetry/sPort_data.c
+1 -1
View File
@@ -236,7 +236,7 @@ void sPort_send_CUR(int uart)
void sPort_send_ALT(int uart)
{
/* send data */
uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]);
uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter);
sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
}
src/drivers/hott/messages.cpp
+2 -2
View File
@@ -149,12 +149,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500);
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
src/examples/px4_simple_app/px4_simple_app.c
+1 -1
View File
@@ -102,7 +102,7 @@ int px4_simple_app_main(int argc, char *argv[])
/* 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;
float accel_dt = raw.accelerometer_integral_dt / 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;
src/lib/terrain_estimation/terrain_estimator.cpp
+1 -1
View File
@@ -68,7 +68,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a;
float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = sensor->accelerometer_integral_dt / 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;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+8 -8
View File
@@ -396,7 +396,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
float gyro_rad_s[3];
float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f;
float gyro_dt = raw.gyro_integral_dt / 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;
@@ -427,10 +427,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.gyro_timestamp[0];
sensor_last_timestamp[0] = raw.timestamp;
}
z_k[0] = gyro_rad_s[0] - gyro_offsets[0];
@@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[2] = gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
hrt_abstime vel_t = 0;
@@ -477,7 +477,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
matrix::Vector3f raw_accel;
float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = raw.accelerometer_integral_dt / 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;
@@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[5] = raw_accel(2) - acc(2);
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
/* check that the mag vector is > 0 */
fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
update_vect[2] = 1;
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
bool vision_updated = false;
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+30 -143
View File
@@ -67,7 +67,6 @@
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/geo/geo.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -139,7 +138,6 @@ private:
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
param_t airspeed_mode;
} _params_handles; /**< handles for interesting parameters */
@@ -152,8 +150,6 @@ private:
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
float _vibration_warning_threshold = 2.0f;
hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
int _airspeed_mode = 0;
@@ -177,10 +173,6 @@ private:
Vector<3> _vel_prev;
Vector<3> _pos_acc;
DataValidatorGroup _voter_gyro;
DataValidatorGroup _voter_accel;
DataValidatorGroup _voter_mag;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate;
math::LowPassFilter2p _lp_pitch_rate;
@@ -190,8 +182,6 @@ private:
bool _inited = false;
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
orb_advert_t _mavlink_log_pub = nullptr;
@@ -215,15 +205,10 @@ private:
AttitudeEstimatorQ::AttitudeEstimatorQ() :
_vel_prev(0, 0, 0),
_pos_acc(0, 0, 0),
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
_lp_roll_rate(250.0f, 30.0f),
_lp_pitch_rate(250.0f, 30.0f),
_lp_yaw_rate(250.0f, 20.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
@@ -232,7 +217,6 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
}
@@ -286,12 +270,6 @@ int AttitudeEstimatorQ::start()
void AttitudeEstimatorQ::print()
{
warnx("gyro status:");
_voter_gyro.print();
warnx("accel status:");
_voter_accel.print();
warnx("mag status:");
_voter_mag.print();
}
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
@@ -346,129 +324,40 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
int best_gyro = 0;
int best_accel = 0;
int best_mag = 0;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
if (sensors.timestamp > 0) {
float gyro_dt = sensors.gyro_integral_dt / 1e6;
_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
}
/* ignore empty fields */
if (sensors.gyro_timestamp[i] > 0) {
if (sensors.accelerometer_timestamp > 0) {
float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
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;
//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);
}
if (sensors.accelerometer_timestamp[i] > 0) {
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);
}
if (sensors.magnetometer_timestamp[i] > 0) {
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
if (_accel.length() < 0.01f) {
warnx("WARNING: degenerate accel!");
continue;
}
}
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
_accel.set(_voter_accel.get_best(curr_time, &best_accel));
_mag.set(_voter_mag.get_best(curr_time, &best_mag));
if (sensors.magnetometer_timestamp > 0) {
_mag(0) = sensors.magnetometer_ga[0];
_mag(1) = sensors.magnetometer_ga[1];
_mag(2) = sensors.magnetometer_ga[2];
if (_accel.length() < 0.01f) {
warnx("WARNING: degenerate accel!");
continue;
}
if (_mag.length() < 0.01f) {
warnx("WARNING: degenerate mag!");
continue;
if (_mag.length() < 0.01f) {
warnx("WARNING: degenerate mag!");
continue;
}
}
_data_good = true;
if (!_failsafe) {
uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_mag.failover_count() > 0) {
_failsafe = true;
flags = _voter_mag.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_failsafe) {
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
// mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
// (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
// (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
// (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
}
}
// Update vision and motion capture heading
@@ -596,10 +485,6 @@ void AttitudeEstimatorQ::task_main()
memcpy(&att.q[0], _q.data, sizeof(att.q));
att.q_valid = true;
att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
/* the instance count is not used here */
int att_inst;
orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
@@ -656,12 +541,9 @@ void AttitudeEstimatorQ::task_main()
}
{
struct estimator_status_s est = {};
//struct estimator_status_s est = {};
est.timestamp = sensors.timestamp;
est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
//est.timestamp = sensors.timestamp;
/* the instance count is not used here */
//int est_inst;
@@ -672,6 +554,12 @@ void AttitudeEstimatorQ::task_main()
}
}
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
orb_unsubscribe(_sensors_sub);
orb_unsubscribe(_vision_sub);
orb_unsubscribe(_mocap_sub);
@@ -706,7 +594,6 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.acc_comp, &acc_comp_int);
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
param_get(_params_handles.airspeed_mode, &_airspeed_mode);
}
src/modules/commander/calibration_routines.cpp
+1 -1
View File
@@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
for (unsigned i = 0; i < ndim; i++) {
float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
float d = di - accel_ema[i];
accel_ema[i] += d * w;
src/modules/commander/commander.cpp
+1 -1
View File
@@ -1788,7 +1788,7 @@ int commander_thread_main(int argc, char *argv[])
* vertical separation from other airtraffic the operator has to know when the
* barometer is inoperational.
* */
if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where baro was regained */
if (status_flags.barometer_failure) {
status_flags.barometer_failure = false;
src/modules/ekf2/ekf2_main.cpp
+15 -15
View File
@@ -505,14 +505,14 @@ void Ekf2::task_main()
}
// push imu data into estimator
_ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
// read mag data
_ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
_ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga);
// read baro data
_ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
_ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter);
// read gps data if available
if (gps_updated) {
@@ -615,7 +615,7 @@ void Ekf2::task_main()
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;
float gyro_dt = sensors.gyro_integral_dt / 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];
@@ -650,7 +650,7 @@ void Ekf2::task_main()
// Acceleration data
matrix::Vector<float, 3> acceleration;
float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = sensors.accelerometer_integral_dt / 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;
@@ -811,7 +811,7 @@ void Ekf2::task_main()
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)
if (_vehicle_global_position_pub == nullptr) {
_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
@@ -905,15 +905,15 @@ void Ekf2::task_main()
if (publish_replay_message) {
struct ekf2_replay_s replay = {};
replay.time_ref = now;
replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
replay.baro_timestamp = sensors.baro_timestamp[0];
memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
replay.gyro_integral_dt = sensors.gyro_integral_dt;
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
replay.magnetometer_timestamp = sensors.magnetometer_timestamp;
replay.baro_timestamp = sensors.baro_timestamp;
memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
sizeof(replay.accelerometer_integral_m_s));
memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter[0];
memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter;
// only write gps data if we had a gps update.
if (gps_updated) {
src/modules/ekf2_replay/ekf2_replay_main.cpp
+5 -5
View File
@@ -377,10 +377,10 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref;
parseMessage(data, dest_ptr, type);
_sensors.timestamp = replay_part1.time_ref;
_sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt;
_sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt;
_sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp;
_sensors.baro_timestamp[0] = replay_part1.baro_timestamp;
_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
_sensors.baro_timestamp = replay_part1.baro_timestamp;
_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
@@ -390,7 +390,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;
_sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter;
_sensors.baro_alt_meter = replay_part1.baro_alt_meter;
_part1_counter_ref = _message_counter;
} else if (type == LOG_RPL2_MSG) {
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
-11
View File
@@ -71,7 +71,6 @@
#include <geo/geo.h>
#include <terrain_estimation/terrain_estimator.h>
#include <systemlib/perf_counter.h>
#include <lib/ecl/validation/data_validator_group.h>
#include "estimator_22states.h"
#include <controllib/blocks.hpp>
@@ -191,8 +190,6 @@ private:
float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
float _vibration_warning_threshold = 2.0f;
hrt_abstime _vibration_warning_timestamp = 0;
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_intvl; ///< loop rate counter
@@ -213,15 +210,7 @@ private:
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _distance_last_valid;
DataValidatorGroup _voter_gyro;
DataValidatorGroup _voter_accel;
DataValidatorGroup _voter_mag;
int _gyro_main; ///< index of the main gyroscope
int _accel_main; ///< index of the main accelerometer
int _mag_main; ///< index of the main magnetometer
bool _data_good; ///< all required filter data is ok
bool _failsafe; ///< failsafe on one of the sensors
bool _vibration_warning; ///< high vibration levels detected
bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
bool _was_landed; ///< indicates if was landed in previous iteration
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+25 -111
View File
@@ -205,15 +205,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_filter_start_time(0),
_last_sensor_timestamp(hrt_absolute_time()),
_distance_last_valid(0),
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
_gyro_main(-1),
_accel_main(-1),
_mag_main(-1),
_data_good(false),
_failsafe(false),
_vibration_warning(false),
_ekf_logging(true),
_debug(0),
_was_landed(true),
@@ -236,8 +228,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_LP_att_Q(250.0f, 20.0f),
_LP_att_R(250.0f, 20.0f)
{
_voter_mag.set_timeout(200000);
_terrain_estimator = new TerrainEstimator();
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
@@ -620,19 +610,9 @@ void AttitudePositionEstimatorEKF::task_main()
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* HIL is slow, set permissive timeouts */
_voter_gyro.set_timeout(500000);
_voter_accel.set_timeout(500000);
_voter_mag.set_timeout(500000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set sensors to de-initialized state */
_gyro_main = -1;
_accel_main = -1;
_mag_main = -1;
_baro_init = false;
_gps_initialized = false;
@@ -664,7 +644,8 @@ void AttitudePositionEstimatorEKF::task_main()
* We run the filter only once all data has been fetched
**/
if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
&& _sensor_combined.magnetometer_timestamp) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1343,13 +1324,6 @@ void AttitudePositionEstimatorEKF::print_status()
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
PX4_INFO("gyro status:");
_voter_gyro.print();
PX4_INFO("accel status:");
_voter_accel.print();
PX4_INFO("mag status:");
_voter_mag.print();
}
void AttitudePositionEstimatorEKF::pollData()
@@ -1382,97 +1356,37 @@ void AttitudePositionEstimatorEKF::pollData()
/* fill in last data set */
_ekf->dtIMU = deltaT;
// Feed validator with recent sensor data
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
//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++) {
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);
}
float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
_ekf->angRate = _ekf->dAngIMU / gyro_dt;
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
(void)_voter_gyro.get_best(curr_time, &_gyro_main);
perf_count(_perf_gyro);
if (_gyro_main >= 0) {
if (_last_accel != _sensor_combined.accelerometer_timestamp) {
_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->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
_ekf->angRate = _ekf->dAngIMU / gyro_dt;
perf_count(_perf_gyro);
}
(void)_voter_accel.get_best(curr_time, &_accel_main);
if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
_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];
float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
_ekf->accel = _ekf->dVelIMU / accel_dt;
_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
_last_accel = _sensor_combined.accelerometer_timestamp;
}
(void)_voter_mag.get_best(curr_time, &_mag_main);
Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
if (_mag_main >= 0) {
Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
_sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
/* fail over to the 2nd mag if we know the first is down */
if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
_ekf->magData.x = mag.x;
_ekf->magData.y = mag.y;
_ekf->magData.z = mag.z;
_newDataMag = true;
_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
perf_count(_perf_mag);
}
}
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
// mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
// (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
// (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
// (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
_ekf->magData.x = mag.x;
_ekf->magData.y = mag.y;
_ekf->magData.z = mag.z;
_newDataMag = true;
_last_mag = _sensor_combined.magnetometer_timestamp;
perf_count(_perf_mag);
}
_last_sensor_timestamp = _sensor_combined.timestamp;
@@ -1481,8 +1395,8 @@ void AttitudePositionEstimatorEKF::pollData()
// leave this in as long as larger improvements are still being made.
#if 0
float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f;
float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+1 -1
View File
@@ -1231,7 +1231,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* filter speed and altitude for controller */
math::Vector<3> accel_body;
float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = _sensor_combined.accelerometer_integral_dt / 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;
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+2 -2
View File
@@ -744,7 +744,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _validTZ;
_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
_pub_gpos.update();
}
}
@@ -845,7 +845,7 @@ void BlockLocalPositionEstimator::predict()
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a;
float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 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;
src/modules/local_position_estimator/sensors/baro.cpp
+1 -1
View File
@@ -41,7 +41,7 @@ int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
{
//measure
y.setZero();
y(0) = _sub_sensor.get().baro_alt_meter[0];
y(0) = _sub_sensor.get().baro_alt_meter;
_baroStats.update(y);
_time_last_baro = _timeStamp;
return OK;
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) {
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -500,7 +500,7 @@ Navigator::task_main()
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+11 -11
View File
@@ -425,14 +425,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp[0];
if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp;
baro_wait_for_sample_time = hrt_absolute_time();
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
baro_offset += sensor.baro_alt_meter[0];
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
@@ -502,10 +502,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
if (sensor.accelerometer_timestamp != accel_timestamp) {
if (att.R_valid) {
float sensor_accel[3];
float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
float accel_dt = sensor.accelerometer_integral_dt / 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];
@@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(acc, 0, sizeof(acc));
}
accel_timestamp = sensor.accelerometer_timestamp[0];
accel_timestamp = sensor.accelerometer_timestamp;
accel_updates++;
}
if (sensor.baro_timestamp[0] != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
baro_timestamp = sensor.baro_timestamp[0];
if (sensor.baro_timestamp != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_timestamp = sensor.baro_timestamp;
baro_updates++;
}
}
@@ -1363,7 +1363,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.terrain_alt_valid = false;
}
global_pos.pressure_alt = sensor.baro_alt_meter[0];
global_pos.pressure_alt = sensor.baro_alt_meter;
if (vehicle_global_position_pub == NULL) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
src/modules/sdlog2/sdlog2.c
+48 -70
View File
@@ -1370,10 +1370,10 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
hrt_abstime gyro_timestamp[3] = {0, 0, 0};
hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
hrt_abstime barometer_timestamp[3] = {0, 0, 0};
hrt_abstime gyro_timestamp = 0;
hrt_abstime accelerometer_timestamp = 0;
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
@@ -1634,80 +1634,58 @@ int sdlog2_thread_main(int argc, char *argv[])
// but we need to copy it again because we are re-using the buffer.
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
for (unsigned i = 0; i < 3; i++) {
bool write_IMU = false;
bool write_SENS = false;
bool write_IMU = false;
bool write_SENS = false;
if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
write_IMU = true;
}
if (buf.sensor.timestamp != gyro_timestamp) {
gyro_timestamp = buf.sensor.timestamp;
write_IMU = true;
}
if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
write_IMU = true;
}
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true;
}
if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
write_SENS = true;
}
if (buf.sensor.baro_timestamp != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true;
}
if (write_IMU) {
switch (i) {
case 0:
log_msg.msg_type = LOG_IMU_MSG;
break;
case 1:
log_msg.msg_type = LOG_IMU1_MSG;
break;
case 2:
log_msg.msg_type = LOG_IMU2_MSG;
break;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
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 = 0;
log_msg.body.log_IMU.temp_acc = 0;
log_msg.body.log_IMU.temp_mag = 0;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt;
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt;
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt;
float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f;
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt;
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt;
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt;
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
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);
}
if (write_SENS) {
switch (i) {
case 0:
log_msg.msg_type = LOG_SENS_MSG;
break;
case 1:
log_msg.msg_type = LOG_AIR1_MSG;
break;
case 2:
continue;
break;
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
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;
log_msg.body.log_SENS.diff_pres_filtered = 0;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
log_msg.body.log_SENS.baro_pres = 0;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
log_msg.body.log_SENS.diff_pres = 0;
log_msg.body.log_SENS.diff_pres_filtered = 0;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
/* --- VTOL VEHICLE STATUS --- */
src/modules/sensors/sensors.cpp
+369 -207
View File
@@ -76,6 +76,7 @@
#include <drivers/drv_px4flow.h>
#include <systemlib/airspeed.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -85,6 +86,7 @@
#include <conversion/rotation.h>
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
@@ -164,6 +166,9 @@ public:
*/
int start();
void print_status();
private:
static const unsigned _rc_max_chan_count =
input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
@@ -200,19 +205,33 @@ private:
int _sensors_task; /**< task handle for sensor task */
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
bool _publishing; /**< if true, we are publishing sensor data (in HIL mode, we don't) */
bool _armed; /**< arming status of the vehicle */
int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */
int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */
int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */
int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */
int _actuator_ctrl_0_sub; /**< attitude controls sub */
unsigned _gyro_count; /**< raw gyro data count */
unsigned _accel_count; /**< raw accel data count */
unsigned _mag_count; /**< raw mag data count */
unsigned _baro_count; /**< raw baro data count */
struct SensorData {
SensorData()
: subscription_count(0),
voter(SENSOR_COUNT_MAX),
last_failover_count(0)
{
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
subscription[i] = -1;
}
}
int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
int subscription_count;
DataValidatorGroup voter;
unsigned int last_failover_count;
};
SensorData _gyro;
SensorData _accel;
SensorData _mag;
SensorData _baro;
int _actuator_ctrl_0_sub; /**< attitude controls sub */
int _rc_sub; /**< raw rc channels data subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
@@ -227,6 +246,7 @@ private:
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
orb_advert_t _diff_pres_pub; /**< differential_pressure */
orb_advert_t _mavlink_log_pub;
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -234,7 +254,6 @@ private:
struct rc_channels_s _rc; /**< r/c channel data */
struct battery_status_s _battery_status; /**< battery status */
struct baro_report _barometer; /**< barometer data */
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
@@ -245,9 +264,12 @@ 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];
float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
float _last_best_baro_pressure; /**< pressure from last best baro */
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
hrt_abstime _vibration_warning_timestamp;
bool _vibration_warning;
struct {
float min[_rc_max_chan_count];
@@ -320,6 +342,8 @@ private:
float baro_qnh;
float vibration_warning_threshold;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -387,10 +411,12 @@ private:
param_t baro_qnh;
param_t vibe_thresh; /**< vibration threshold */
} _parameter_handles; /**< handles for interesting parameters */
int init_sensor_class(const struct orb_metadata *meta, int *subs);
void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data);
/**
* Update our local parameter cache.
@@ -495,6 +521,18 @@ private:
*/
void adc_poll(struct sensor_combined_s &raw);
/**
* Check & handle failover of a sensor
* @return true if a switch occured (could be for a non-critical reason)
*/
bool check_failover(SensorData &sensor, const char *sensor_name);
/**
* check vibration levels and output a warning if they're high
* @return true on high vibration
*/
bool check_vibration();
/**
* Shim for calling task_main from task_create.
*/
@@ -521,10 +559,6 @@ Sensors::Sensors() :
_hil_enabled(false),
_publishing(true),
_armed(false),
_gyro_count(0),
_accel_count(0),
_mag_count(0),
_baro_count(0),
_rc_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
@@ -539,6 +573,7 @@ Sensors::Sensors() :
_battery_pub(nullptr),
_airspeed_pub(nullptr),
_diff_pres_pub(nullptr),
_mavlink_log_pub(nullptr),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
@@ -546,23 +581,20 @@ Sensors::Sensors() :
_param_rc_values{},
_board_rotation{},
_mag_rotation{}
_mag_rotation{},
_last_best_baro_pressure(0.f),
_vibration_warning_timestamp(0),
_vibration_warning(false)
{
/* initialize subscriptions */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
_gyro_sub[i] = -1;
_accel_sub[i] = -1;
_mag_sub[i] = -1;
_baro_sub[i] = -1;
}
_mag.voter.set_timeout(200000);
memset(&_rc, 0, sizeof(_rc));
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));
memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
/* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -661,6 +693,8 @@ Sensors::Sensors() :
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
_parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
// These are parameters for which QGroundControl always expects to be returned in a list request.
// We do a param_find here to force them into the list.
(void)param_find("RC_CHAN_CNT");
@@ -993,6 +1027,8 @@ Sensors::parameters_update()
#endif
param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold);
return ret;
}
@@ -1014,43 +1050,72 @@ Sensors::adc_init()
void
Sensors::accel_poll(struct sensor_combined_s &raw)
{
for (unsigned i = 0; i < _accel_count; i++) {
bool got_update = false;
for (unsigned i = 0; i < _accel.subscription_count; i++) {
bool accel_updated;
orb_check(_accel_sub[i], &accel_updated);
orb_check(_accel.subscription[i], &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report);
if (accel_report.timestamp == 0) {
continue; //ignore invalid data
}
got_update = true;
math::Vector<3> sensor_value;
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_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);
_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0);
_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
_last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt;
float dt = accel_report.integral_dt / 1.e6f;
sensor_value = vect_int / dt;
} 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;
if (_last_accel_timestamp[i] == 0) {
_last_accel_timestamp[i] = accel_report.timestamp - 1000;
sensor_value = vect_val;
if (_last_sensor_data[i].accelerometer_timestamp == 0) {
_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000;
}
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;
_last_sensor_data[i].accelerometer_integral_dt =
accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp;
float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
}
raw.accelerometer_timestamp[i] = accel_report.timestamp;
_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp;
_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
accel_report.error_count, _accel.priority[i]);
}
}
if (got_update) {
int best_index;
_accel.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0];
raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp;
}
}
}
@@ -1058,47 +1123,72 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
void
Sensors::gyro_poll(struct sensor_combined_s &raw)
{
for (unsigned i = 0; i < _gyro_count; i++) {
bool got_update = false;
for (unsigned i = 0; i < _gyro.subscription_count; i++) {
bool gyro_updated;
orb_check(_gyro_sub[i], &gyro_updated);
orb_check(_gyro.subscription[i], &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report);
if (gyro_report.timestamp == 0) {
continue; //ignore invalid data
}
got_update = true;
math::Vector<3> sensor_value;
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_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);
_last_sensor_data[i].gyro_integral_rad[0] = vect_int(0);
_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
raw.gyro_integral_dt[i] = gyro_report.integral_dt;
raw.gyro_timestamp[i] = gyro_report.timestamp;
_last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt;
float dt = gyro_report.integral_dt / 1.e6f;
sensor_value = vect_int / dt;
} 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;
if (_last_gyro_timestamp[i] == 0) {
_last_gyro_timestamp[i] = gyro_report.timestamp - 1000;
sensor_value = vect_val;
if (_last_sensor_data[i].timestamp == 0) {
_last_sensor_data[i].timestamp = gyro_report.timestamp - 1000;
}
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;
_last_sensor_data[i].gyro_integral_dt =
gyro_report.timestamp - _last_sensor_data[i].timestamp;
float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;
_last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt;
}
if (i == 0) {
raw.timestamp = gyro_report.timestamp;
}
_last_sensor_data[i].timestamp = gyro_report.timestamp;
_gyro.voter.put(i, gyro_report.timestamp, sensor_value.data,
gyro_report.error_count, _gyro.priority[i]);
}
}
if (got_update) {
int best_index;
_gyro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0];
raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1];
raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
raw.timestamp = _last_sensor_data[best_index].timestamp;
}
}
}
@@ -1106,24 +1196,44 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
void
Sensors::mag_poll(struct sensor_combined_s &raw)
{
for (unsigned i = 0; i < _mag_count; i++) {
bool got_update = false;
for (unsigned i = 0; i < _mag.subscription_count; i++) {
bool mag_updated;
orb_check(_mag_sub[i], &mag_updated);
orb_check(_mag.subscription[i], &mag_updated);
if (mag_updated) {
struct mag_report mag_report;
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
orb_copy(ORB_ID(sensor_mag), _mag.subscription[i], &mag_report);
if (mag_report.timestamp == 0) {
continue; //ignore invalid data
}
got_update = true;
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
vect = _mag_rotation[i] * vect;
raw.magnetometer_ga[i * 3 + 0] = vect(0);
raw.magnetometer_ga[i * 3 + 1] = vect(1);
raw.magnetometer_ga[i * 3 + 2] = vect(2);
_last_sensor_data[i].magnetometer_ga[0] = vect(0);
_last_sensor_data[i].magnetometer_ga[1] = vect(1);
_last_sensor_data[i].magnetometer_ga[2] = vect(2);
raw.magnetometer_timestamp[i] = mag_report.timestamp;
_last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp;
_mag.voter.put(i, mag_report.timestamp, vect.data,
mag_report.error_count, _mag.priority[i]);
}
}
if (got_update) {
int best_index;
_mag.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp;
}
}
}
@@ -1131,19 +1241,43 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
void
Sensors::baro_poll(struct sensor_combined_s &raw)
{
for (unsigned i = 0; i < _baro_count; i++) {
bool got_update = false;
for (unsigned i = 0; i < _baro.subscription_count; i++) {
bool baro_updated;
orb_check(_baro_sub[i], &baro_updated);
orb_check(_baro.subscription[i], &baro_updated);
if (baro_updated) {
struct baro_report baro_report;
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report);
_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
if (baro_report.timestamp == 0) {
continue; //ignore invalid data
}
raw.baro_timestamp[i] = _barometer.timestamp;
got_update = true;
math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
_last_sensor_data[i].baro_alt_meter = baro_report.altitude;
_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
_last_baro_pressure[i] = baro_report.pressure;
_last_sensor_data[i].baro_timestamp = baro_report.timestamp;
_baro.voter.put(i, baro_report.timestamp, vect.data,
baro_report.error_count, _baro.priority[i]);
}
}
if (got_update) {
int best_index;
_baro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp;
_last_best_baro_pressure = _last_baro_pressure[best_index];
}
}
}
@@ -1158,7 +1292,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
@@ -1175,13 +1309,12 @@ 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 + _latest_baro_pressure[0] * 1e2f,
_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _last_best_baro_pressure * 1e2f,
_last_best_baro_pressure * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _last_best_baro_pressure * 1e2f,
_last_best_baro_pressure * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
@@ -1544,14 +1677,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
if (res) {
return false;
} else {
return true;
}
return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
#else
/* On QURT, the params are read directly in the respective wrappers. */
@@ -1565,14 +1691,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
if (res) {
return false;
} else {
return true;
}
return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
#else
/* On QURT, the params are read directly in the respective wrappers. */
@@ -1586,14 +1705,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
if (res) {
return false;
} else {
return true;
}
return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
#else
/* On QURT, the params are read directly in the respective wrappers. */
@@ -2045,14 +2157,72 @@ Sensors::rc_poll()
}
}
bool
Sensors::check_failover(SensorData &sensor, const char *sensor_name)
{
if (sensor.last_failover_count != sensor.voter.failover_count()) {
uint32_t flags = sensor.voter.failover_state();
if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
//we switched due to a non-critical reason. No need to panic.
PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index());
} else {
mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failure :%s%s%s%s%s!",
sensor_name,
sensor.voter.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
sensor.last_failover_count = sensor.voter.failover_count();
return true;
}
return false;
}
bool
Sensors::check_vibration()
{
bool ret = false;
hrt_abstime cur_time = hrt_absolute_time();
if (!_vibration_warning && (_gyro.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
_accel.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
_mag.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = cur_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000 * 1000) {
_vibration_warning = true;
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _gyro.voter.get_vibration_factor(cur_time)),
(int)(100 * _accel.voter.get_vibration_factor(cur_time)),
(int)(100 * _mag.voter.get_vibration_factor(cur_time)));
ret = true;
}
} else {
_vibration_warning_timestamp = 0;
}
return ret;
}
void
Sensors::task_main_trampoline(int argc, char *argv[])
{
sensors::g_sensors->task_main();
}
int
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
void
Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data)
{
unsigned group_count = orb_group_count(meta);
@@ -2061,12 +2231,19 @@ 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);
if (sensor_data.subscription[i] < 0) {
sensor_data.subscription[i] = orb_subscribe_multi(meta, i);
int32_t priority;
orb_priority(sensor_data.subscription[i], &priority);
sensor_data.priority[i] = (uint8_t)priority;
}
int32_t priority;
orb_priority(sensor_data.subscription[i], &priority);
sensor_data.priority[i] = (uint8_t)priority;
}
return group_count;
sensor_data.subscription_count = group_count;
}
void
@@ -2090,84 +2267,79 @@ Sensors::task_main()
struct sensor_combined_s raw = {};
/* ensure no overflows can occur */
static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
/*
* do subscriptions
*/
init_sensor_class(ORB_ID(sensor_gyro), _gyro);
unsigned gcount_prev = _gyro_count;
init_sensor_class(ORB_ID(sensor_mag), _mag);
unsigned mcount_prev = _mag_count;
init_sensor_class(ORB_ID(sensor_accel), _accel);
unsigned acount_prev = _accel_count;
init_sensor_class(ORB_ID(sensor_baro), _baro);
unsigned bcount_prev = _baro_count;
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
if (gcount_prev != _gyro_count ||
mcount_prev != _mag_count ||
acount_prev != _accel_count ||
bcount_prev != _baro_count) {
/* reload calibration params */
parameter_update_poll(true);
}
/* reload calibration params */
parameter_update_poll(true);
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
/*
* do advertisements
*/
raw.timestamp = hrt_absolute_time();
raw.timestamp = 0;
_battery.reset(&_battery_status);
/* get a set of initial values */
accel_poll(raw);
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
diff_pres_poll(raw);
parameter_update_poll(true /* forced */);
rc_parameter_map_poll(true /* forced */);
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
/* use the gyro to pace output */
fds[0].fd = _gyro_sub[0];
fds[0].events = POLLIN;
int num_poll_fds = 0;
_task_should_exit = false;
raw.timestamp = 0;
uint64_t _last_config_update = hrt_absolute_time();
while (!_task_should_exit) {
/* use the gyro(s) to pace output */
if (num_poll_fds != _gyro.subscription_count) { //happens the first time we enter, or when new gyro added
num_poll_fds = _gyro.subscription_count;
for (int i = 0; i < _gyro.subscription_count; ++i) {
fds[i].fd = _gyro.subscription[i];
fds[i].events = POLLIN;
}
}
/* wait for up to 50ms for data */
int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 50);
int pret = px4_poll(fds, num_poll_fds, 50);
/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
@@ -2176,11 +2348,12 @@ Sensors::task_main()
/* if the polling operation failed because no gyro sensor is available yet,
* then attempt to subscribe once again
*/
if (_gyro_count == 0) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
fds[0].fd = _gyro_sub[0];
if (_gyro.subscription_count == 0) {
init_sensor_class(ORB_ID(sensor_gyro), _gyro);
}
usleep(1000);
continue;
}
@@ -2189,60 +2362,38 @@ Sensors::task_main()
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
* a mandatory sensor) */
gyro_poll(raw);
accel_poll(raw);
mag_poll(raw);
baro_poll(raw);
// FIXME TODO: this needs more thinking, otherwise we spam the console and keep switching.
/* Work out if main gyro timed out and fail over to alternate gyro.
* However, don't do this if the secondary is not available. */
if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) {
if (fds[0].fd == _gyro_sub[0]) {
PX4_WARN("gyro0 has timed out");
}
/* If the secondary failed as well, go to the tertiary, also only if available. */
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) {
fds[0].fd = _gyro_sub[2];
if (!_hil_enabled) {
PX4_WARN("failing over to third gyro");
}
} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
fds[0].fd = _gyro_sub[1];
if (!_hil_enabled) {
PX4_WARN("failing over to second gyro");
}
}
}
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing && raw.timestamp > 0) {
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
check_failover(_accel, "Accel");
check_failover(_gyro, "Gyro");
check_failover(_mag, "Mag");
check_failover(_baro, "Baro");
//check_vibration(); //disabled for now, as it does not seem to be reliable
}
/* keep adding sensors as long as we are not armed,
* 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);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
init_sensor_class(ORB_ID(sensor_gyro), _gyro);
init_sensor_class(ORB_ID(sensor_mag), _mag);
init_sensor_class(ORB_ID(sensor_accel), _accel);
init_sensor_class(ORB_ID(sensor_baro), _baro);
_last_config_update = hrt_absolute_time();
} else {
@@ -2260,22 +2411,20 @@ Sensors::task_main()
perf_end(_loop_perf);
}
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
if (_gyro_sub[i] >= 0) {
orb_unsubscribe(_gyro_sub[i]);
}
for (unsigned i = 0; i < _gyro.subscription_count; i++) {
orb_unsubscribe(_gyro.subscription[i]);
}
if (_accel_sub[i] >= 0) {
orb_unsubscribe(_accel_sub[i]);
}
for (unsigned i = 0; i < _accel.subscription_count; i++) {
orb_unsubscribe(_accel.subscription[i]);
}
if (_mag_sub[i] >= 0) {
orb_unsubscribe(_mag_sub[i]);
}
for (unsigned i = 0; i < _mag.subscription_count; i++) {
orb_unsubscribe(_mag.subscription[i]);
}
if (_baro_sub[i] >= 0) {
orb_unsubscribe(_baro_sub[i]);
}
for (unsigned i = 0; i < _baro.subscription_count; i++) {
orb_unsubscribe(_baro.subscription[i]);
}
orb_unsubscribe(_rc_sub);
@@ -2316,6 +2465,19 @@ Sensors::start()
return OK;
}
void Sensors::print_status()
{
PX4_INFO("gyro status:");
_gyro.voter.print();
PX4_INFO("accel status:");
_accel.voter.print();
PX4_INFO("mag status:");
_mag.voter.print();
PX4_INFO("baro status:");
_baro.voter.print();
}
int sensors_main(int argc, char *argv[])
{
if (argc < 2) {
@@ -2360,7 +2522,7 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (sensors::g_sensors) {
PX4_INFO("is running");
sensors::g_sensors->print_status();
return 0;
} else {