Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 18 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
File diff suppressed because it is too large Load Diff