fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 23:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'ekf_voting_priority'

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-19 10:51:01 +02:00
parent 1412781970 ad2058427d
commit b067a0c094
36 changed files with 1723 additions and 950 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+6 -6
View File
@@ -383,10 +383,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.timestamp) {
if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
update_vect[0] = 1;
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
sensor_last_timestamp[0] = raw.gyro_timestamp[0];
}
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
@@ -394,10 +394,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
update_vect[1] = 1;
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
}
hrt_abstime vel_t = 0;
@@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
/* 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;
sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
}
bool vision_updated = false;
src/modules/attitude_estimator_ekf/module.mk
+1 -1
View File
@@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200
EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
ifeq ($(PX4_TARGET_OS),nuttx)
EXTRACXXFLAGS = -Wframe-larger-than=2400
EXTRACXXFLAGS = -Wframe-larger-than=2600
endif
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+129 -14
View File
@@ -51,7 +51,6 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -59,8 +58,11 @@
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/geo/geo.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -103,6 +105,8 @@ public:
void task_main();
void print();
private:
static constexpr float _dt_max = 0.02;
bool _task_should_exit = false; /**< if true, task should exit */
@@ -121,6 +125,7 @@ private:
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
@@ -130,6 +135,8 @@ private:
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
Vector<3> _gyro;
Vector<3> _accel;
@@ -142,10 +149,23 @@ private:
vehicle_global_position_s _gpos = {};
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;
hrt_abstime _vel_prev_t = 0;
bool _inited = false;
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
int _mavlink_fd = -1;
perf_counter_t _update_perf;
perf_counter_t _loop_perf;
@@ -160,7 +180,15 @@ private:
};
AttitudeEstimatorQ::AttitudeEstimatorQ() {
AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
_lp_roll_rate(250.0f, 20.0f),
_lp_pitch_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_gyro_bias = param_find("ATT_W_GYRO_BIAS");
@@ -168,12 +196,14 @@ 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");
}
/**
* Destructor, also kills task.
*/
AttitudeEstimatorQ::~AttitudeEstimatorQ() {
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
@@ -196,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
attitude_estimator_q::instance = nullptr;
}
int AttitudeEstimatorQ::start() {
int AttitudeEstimatorQ::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
2100,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
@@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() {
return OK;
}
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
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[])
{
attitude_estimator_q::instance->task_main();
}
void AttitudeEstimatorQ::task_main() {
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -236,6 +278,10 @@ void AttitudeEstimatorQ::task_main() {
while (!_task_should_exit) {
int ret = px4_poll(fds, 1, 1000);
if (_mavlink_fd < 0) {
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
@@ -250,11 +296,71 @@ void AttitudeEstimatorQ::task_main() {
// Update sensors
sensor_combined_s sensors;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
_gyro.set(sensors.gyro_rad_s);
_accel.set(sensors.accelerometer_m_s2);
_mag.set(sensors.magnetometer_ga);
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
/* ignore empty fields */
if (sensors.gyro_timestamp[i] > 0) {
float gyro[3];
for (unsigned j = 0; j < 3; j++) {
if (sensors.gyro_integral_dt[i] > 0) {
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
} else {
/* fall back to angular rate */
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
}
}
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
int best_gyro, best_accel, best_mag;
// 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 (_accel.length() < 0.01f || _mag.length() < 0.01f) {
warnx("WARNING: degenerate accel / mag!");
continue;
}
_data_good = true;
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_fd, "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_fd, "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;
}
}
bool gpos_updated;
@@ -311,8 +417,11 @@ void AttitudeEstimatorQ::task_main() {
att.pitch = euler(1);
att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
@@ -328,6 +437,10 @@ void AttitudeEstimatorQ::task_main() {
memcpy(&att.R[0], R.data, sizeof(att.R));
att.R_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());
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
@@ -358,6 +471,7 @@ 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);
}
}
@@ -501,6 +615,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) {
if (!strcmp(argv[1], "status")) {
if (attitude_estimator_q::instance) {
attitude_estimator_q::instance->print();
warnx("running");
return 0;
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+9
View File
@@ -108,3 +108,12 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
* @unit rad/s
*/
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
/**
* Threshold (of RMS) to warn about high vibration levels
*
* @group Attitude Q estimator
* @min 0.001
* @max 100
*/
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);
src/modules/commander/commander.cpp
+1 -1
View File
@@ -1438,7 +1438,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) < FAILSAFE_DEFAULT_TIMEOUT) {
if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where baro was regained */
if (status.barometer_failure) {
status.barometer_failure = false;
src/modules/commander/module.mk
+1 -1
View File
@@ -55,6 +55,6 @@ MODULE_STACKSIZE = 5000
MAXOPTIMIZATION = -Os
ifeq ($(PX4_TARGET_OS),nuttx)
EXTRACXXFLAGS = -Wframe-larger-than=2200
EXTRACXXFLAGS = -Wframe-larger-than=2400
endif
src/modules/controllib/block/BlockParam.cpp
+5
View File
@@ -97,6 +97,11 @@ void BlockParam<T>::update() {
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
}
template <class T>
void BlockParam<T>::commit() {
if (_handle != PARAM_INVALID) param_set(_handle, &_val);
}
template <class T>
BlockParam<T>::~BlockParam() {};
src/modules/controllib/block/BlockParam.hpp
+1
View File
@@ -79,6 +79,7 @@ public:
BlockParam(Block *block, const char *name,
bool parent_prefix = true);
T get();
void commit();
void set(T val);
void update();
virtual ~BlockParam();
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+20 -9
View File
@@ -52,7 +52,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
@@ -70,11 +69,16 @@
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <lib/ecl/validation/data_validator_group.h>
#include "estimator_22states.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
//Forward declaration
class AttPosEKF;
class AttitudePositionEstimatorEKF
class AttitudePositionEstimatorEKF : public control::SuperBlock
{
public:
/**
@@ -168,9 +172,8 @@ private:
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
struct gyro_scale _gyro_offsets[3];
struct accel_scale _accel_offsets[3];
struct mag_scale _mag_offsets[3];
hrt_abstime _last_accel;
hrt_abstime _last_mag;
struct sensor_combined_s _sensor_combined;
@@ -179,6 +182,8 @@ 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 = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_intvl; ///< loop rate counter
@@ -198,14 +203,16 @@ private:
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _last_run;
hrt_abstime _distance_last_valid;
bool _gyro_valid;
bool _accel_valid;
bool _mag_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
@@ -216,6 +223,10 @@ private:
int _mavlink_fd;
control::BlockParamFloat _mag_offset_x;
control::BlockParamFloat _mag_offset_y;
control::BlockParamFloat _mag_offset_z;
struct {
int32_t vel_delay_ms;
int32_t pos_delay_ms;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+241 -273
View File
@@ -68,7 +68,6 @@
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
static uint64_t IMUmsec = 0;
static uint64_t IMUusec = 0;
//Constants
@@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f;
*/
extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
__EXPORT uint64_t getMicros();
uint32_t millis();
uint64_t getMicros();
uint32_t getMillis();
uint32_t millis()
{
return IMUmsec;
return getMillis();
}
uint32_t getMillis()
{
return getMicros() / 1000;
}
uint64_t getMicros()
@@ -115,6 +119,7 @@ AttitudePositionEstimatorEKF *g_estimator = nullptr;
}
AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
SuperBlock(NULL, "EKF"),
_task_should_exit(false),
_task_running(false),
_estimator_task(-1),
@@ -133,84 +138,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_landDetectorSub(-1),
_armedSub(-1),
/* publications */
/* publications */
_att_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att({}),
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_wind({}),
_distance {},
_landDetector {},
_armed {},
_att{},
_gyro{},
_accel{},
_mag{},
_airspeed{},
_baro{},
_vstatus{},
_global_pos{},
_local_pos{},
_gps{},
_wind{},
_distance{},
_landDetector{},
_armed{},
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
_last_accel(0),
_last_mag(0),
_sensor_combined {},
_sensor_combined{},
_pos_ref{},
_filter_ref_offset(0.0f),
_baro_gps_offset(0.0f),
_pos_ref{},
_filter_ref_offset(0.0f),
_baro_gps_offset(0.0f),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* states */
_gps_alt_filt(0.0f),
_baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f),
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_distance_last_valid(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_gyro_main(0),
_accel_main(0),
_mag_main(0),
_ekf_logging(true),
_debug(0),
/* states */
_gps_alt_filt(0.0f),
_baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f),
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_gps_initialized(false),
_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),
_newHgtData(false),
_newAdsData(false),
_newDataMag(false),
_newRangeData(false),
_newHgtData(false),
_newAdsData(false),
_newDataMag(false),
_newRangeData(false),
_mavlink_fd(-1),
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_mavlink_fd(-1),
_mag_offset_x(this, "PE_MAGB_X"),
_mag_offset_y(this, "PE_MAGB_Y"),
_mag_offset_z(this, "PE_MAGB_Z"),
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_LP_att_P(100.0f, 10.0f),
_LP_att_Q(100.0f, 10.0f),
_LP_att_R(100.0f, 10.0f)
_LP_att_P(250.0f, 20.0f),
_LP_att_Q(250.0f, 20.0f),
_LP_att_R(250.0f, 20.0f)
{
_last_run = hrt_absolute_time();
_voter_mag.set_timeout(200000);
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
@@ -237,48 +246,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* fetch initial parameter values */
parameters_update();
/* get offsets */
int fd, res;
for (unsigned s = 0; s < 3; s++) {
char str[30];
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
fd = px4_open(str, O_RDONLY);
if (fd >= 0) {
res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
px4_close(fd);
if (res) {
PX4_WARN("G%u SCALE FAIL", s);
}
}
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
fd = px4_open(str, O_RDONLY);
if (fd >= 0) {
res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
px4_close(fd);
if (res) {
PX4_WARN("A%u SCALE FAIL", s);
}
}
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
fd = px4_open(str, O_RDONLY);
if (fd >= 0) {
res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
px4_close(fd);
if (res) {
PX4_WARN("M%u SCALE FAIL", s);
}
}
}
}
AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
@@ -317,6 +284,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging)
int AttitudePositionEstimatorEKF::parameters_update()
{
/* update C++ param system */
updateParams();
param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
@@ -354,6 +323,13 @@ int AttitudePositionEstimatorEKF::parameters_update()
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
#if 0
// Initially disable loading until
// convergence is flight-test proven
_ekf->magBias.x = _mag_offset_x.get();
_ekf->magBias.y = _mag_offset_y.get();
_ekf->magBias.z = _mag_offset_z.get();
#endif
}
return OK;
@@ -366,12 +342,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll()
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
bool landed = _vstatus.condition_landed;
if (vstatus_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
//Tell EKF that the vehicle is a fixed wing or multi-rotor
// Tell EKF that the vehicle is a fixed wing or multi-rotor
_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
// Save params on landed
if (!landed && _vstatus.condition_landed) {
_mag_offset_x.set(_ekf->magBias.x);
_mag_offset_x.commit();
_mag_offset_y.set(_ekf->magBias.y);
_mag_offset_y.commit();
_mag_offset_z.set(_ekf->magBias.z);
_mag_offset_z.commit();
}
}
}
@@ -541,8 +529,6 @@ void AttitudePositionEstimatorEKF::task_main()
orb_set_interval(_vstatus_sub, 200);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 9);
/* sets also parameters in the EKF object */
parameters_update();
@@ -610,15 +596,14 @@ void AttitudePositionEstimatorEKF::task_main()
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_gyro_main = -1;
_accel_main = -1;
_mag_main = -1;
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
@@ -646,7 +631,7 @@ void AttitudePositionEstimatorEKF::task_main()
* We run the filter only once all data has been fetched
**/
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -836,9 +821,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -1009,7 +994,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_ekf->UpdateStrapdownEquationsNED();
// store the predicted states for subsequent use by measurement fusion
_ekf->StoreStates(IMUmsec);
_ekf->StoreStates(getMillis());
// sum delta angles and time used by covariance prediction
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
@@ -1035,8 +1020,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
@@ -1059,8 +1044,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
@@ -1076,7 +1061,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
_ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms));
// run the fusion step
_ekf->FuseVelposNED();
@@ -1089,7 +1074,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
if (fuseMag) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime,
(IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
(getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
_ekf->magstate.obsIndex = 0;
_ekf->FuseMagnetometer();
@@ -1104,7 +1089,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
(IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
(getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
} else {
@@ -1117,7 +1102,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
// _ekf->rngMea is set in sensor readout already
_ekf->fuseRngData = true;
_ekf->fuseOptFlowData = false;
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
_ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f));
_ekf->OpticalFlowEKF();
_ekf->fuseRngData = false;
}
@@ -1131,7 +1116,7 @@ int AttitudePositionEstimatorEKF::start()
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
SCHED_PRIORITY_MAX - 20,
4800,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@@ -1163,7 +1148,7 @@ void AttitudePositionEstimatorEKF::print_status()
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis());
PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt);
PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt);
PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset,
@@ -1210,6 +1195,13 @@ 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()
@@ -1222,125 +1214,153 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
}
//Update Gyro and Accelerometer
static Vector3f lastAngRate;
static Vector3f lastAccel;
bool accel_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
// Feed validator with recent sensor data
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
}
last_accel = _sensor_combined.accelerometer_timestamp;
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
(void)_voter_gyro.get_best(curr_time, &_gyro_main);
if (_gyro_main >= 0) {
// Use pre-integrated values if possible
if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) {
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]);
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]);
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]);
}
_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
perf_count(_perf_gyro);
}
(void)_voter_accel.get_best(curr_time, &_accel_main);
if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
// Use pre-integrated values if possible
if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) {
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
}
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
}
(void)_voter_mag.get_best(curr_time, &_mag_main);
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_fd, "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_fd, "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;
}
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3;
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
_last_sensor_timestamp = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
_last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
int last_gyro_main = _gyro_main;
// XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made.
#if 0
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_gyro_main = 0;
_gyro_valid = true;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 0;
} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
if (hrt_elapsed_time(&lastprint) > 1000000) {
warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
_gyro_main = 1;
_gyro_valid = true;
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
} else {
_gyro_valid = false;
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
lastprint = hrt_absolute_time();
}
if (last_gyro_main != _gyro_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
if (deltaT > 0.005f) {
dtoverflow5++;
}
if (!_gyro_valid) {
/* keep last value if he hit an out of band value */
lastAngRate = _ekf->angRate;
} else {
perf_count(_perf_gyro);
if (deltaT > 0.01f) {
dtoverflow10++;
}
#endif
if (accel_updated) {
int last_accel_main = _accel_main;
/* fail over to the 2nd accel if we know the first is down */
if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
_accel_main = 0;
} else {
_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
_accel_main = 1;
}
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
if (last_accel_main != _accel_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
_newDataMag = true;
} else {
_newDataMag = false;
}
last_mag = _sensor_combined.magnetometer_timestamp;
_data_good = true;
//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
@@ -1362,7 +1382,6 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
}
bool gps_update;
orb_check(_gps_sub, &gps_update);
@@ -1516,57 +1535,6 @@ void AttitudePositionEstimatorEKF::pollData()
perf_count(_perf_baro);
}
//Update Magnetometer
if (_newDataMag) {
_mag_valid = true;
perf_count(_perf_mag);
int last_mag_main = _mag_main;
Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
_sensor_combined.magnetometer1_ga[2]);
const unsigned mag_timeout_us = 200000;
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag0.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag0.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 0;
} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
mag1.length() > 0.1f) {
_ekf->magData.x = mag1.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag1.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag1.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 1;
} else {
_mag_valid = false;
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
}
}
//Update range data
orb_check(_distance_sub, &_newRangeData);
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+36
View File
@@ -258,6 +258,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Magnetometer X bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
/**
* Magnetometer Y bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
/**
* Magnetometer Z bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
/**
* Threshold for filter initialization.
*
src/modules/mavlink/mavlink_messages.cpp
+12 -12
View File
@@ -643,28 +643,28 @@ protected:
if (_sensor_sub->update(&_sensor_time, &sensor)) {
uint16_t fields_updated = 0;
if (_accel_timestamp != sensor.accelerometer_timestamp) {
if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
_accel_timestamp = sensor.accelerometer_timestamp;
_accel_timestamp = sensor.accelerometer_timestamp[0];
}
if (_gyro_timestamp != sensor.timestamp) {
if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
_gyro_timestamp = sensor.timestamp;
_gyro_timestamp = sensor.gyro_timestamp[0];
}
if (_mag_timestamp != sensor.magnetometer_timestamp) {
if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
_mag_timestamp = sensor.magnetometer_timestamp;
_mag_timestamp = sensor.magnetometer_timestamp[0];
}
if (_baro_timestamp != sensor.baro_timestamp) {
if (_baro_timestamp != sensor.baro_timestamp[0]) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
_baro_timestamp = sensor.baro_timestamp;
_baro_timestamp = sensor.baro_timestamp[0];
}
mavlink_highres_imu_t msg;
@@ -679,10 +679,10 @@ protected:
msg.xmag = sensor.magnetometer_ga[0];
msg.ymag = sensor.magnetometer_ga[1];
msg.zmag = sensor.magnetometer_ga[2];
msg.abs_pressure = sensor.baro_pres_mbar;
msg.diff_pressure = sensor.differential_pressure_pa;
msg.pressure_alt = sensor.baro_alt_meter;
msg.temperature = sensor.baro_temp_celcius;
msg.abs_pressure = sensor.baro_pres_mbar[0];
msg.diff_pressure = sensor.differential_pressure_pa[0];
msg.pressure_alt = sensor.baro_alt_meter[0];
msg.temperature = sensor.baro_temp_celcius[0];
msg.fields_updated = fields_updated;
_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
src/modules/mavlink/mavlink_receiver.cpp
+15 -14
View File
@@ -1410,6 +1410,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_timestamp[0] = timestamp;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -1417,9 +1418,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
hil_sensors.accelerometer_timestamp = timestamp;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
hil_sensors.accelerometer_timestamp[0] = timestamp;
hil_sensors.adc_voltage_v[0] = 0.0f;
hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -1431,19 +1432,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
hil_sensors.magnetometer_timestamp = timestamp;
hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
hil_sensors.magnetometer_timestamp[0] = timestamp;
hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.baro_timestamp = timestamp;
hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
hil_sensors.baro_temp_celcius[0] = imu.temperature;
hil_sensors.baro_timestamp[0] = timestamp;
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
hil_sensors.differential_pressure_timestamp = timestamp;
hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0];
hil_sensors.differential_pressure_timestamp[0] = timestamp;
/* publish combined sensor topic */
if (_sensors_pub == nullptr) {
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -406,7 +406,7 @@ Navigator::task_main()
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set);
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
if (!inside) {
src/modules/position_estimator_inav/position_estimator_inav_main.c
+9 -9
View File
@@ -399,13 +399,13 @@ 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 != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp;
if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp[0];
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
baro_offset += sensor.baro_alt_meter[0];
baro_init_cnt++;
}
@@ -474,7 +474,7 @@ 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 != accel_timestamp) {
if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
if (att.R_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
@@ -496,13 +496,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(acc, 0, sizeof(acc));
}
accel_timestamp = sensor.accelerometer_timestamp;
accel_timestamp = sensor.accelerometer_timestamp[0];
accel_updates++;
}
if (sensor.baro_timestamp != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_timestamp = sensor.baro_timestamp;
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];
baro_updates++;
}
}
src/modules/sdlog2/module.mk
+1 -1
View File
@@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
ifeq ($(PX4_TARGET_OS),nuttx)
EXTRACFLAGS = -Wframe-larger-than=1400
EXTRACFLAGS = -Wframe-larger-than=1600
endif
src/modules/sdlog2/sdlog2.c
+83 -148
View File
@@ -1287,18 +1287,11 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
hrt_abstime gyro_timestamp = 0;
hrt_abstime accelerometer_timestamp = 0;
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
hrt_abstime barometer1_timestamp = 0;
hrt_abstime gyro1_timestamp = 0;
hrt_abstime accelerometer1_timestamp = 0;
hrt_abstime magnetometer1_timestamp = 0;
hrt_abstime gyro2_timestamp = 0;
hrt_abstime accelerometer2_timestamp = 0;
hrt_abstime magnetometer2_timestamp = 0;
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 differential_pressure_timestamp[3] = {0, 0, 0};
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
@@ -1446,144 +1439,86 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
bool write_SENS = false;
bool write_SENS1 = false;
if (buf.sensor.timestamp != gyro_timestamp) {
gyro_timestamp = buf.sensor.timestamp;
write_IMU = true;
for (unsigned i = 0; i < 3; i++) {
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.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
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.baro_timestamp[i] != barometer_timestamp[i]) {
barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
write_SENS = true;
}
if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
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;
}
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0];
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0];
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 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;
}
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i];
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i];
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
}
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true;
}
if (buf.sensor.baro_timestamp != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true;
}
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
write_SENS = true;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
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 = buf.sensor.gyro_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
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 = buf.sensor.differential_pressure_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
barometer1_timestamp = buf.sensor.baro1_timestamp;
write_SENS1 = true;
}
if (write_SENS1) {
log_msg.msg_type = LOG_AIR1_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
// XXX moving to AIR0-AIR2 instead of SENS
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
gyro1_timestamp = buf.sensor.gyro1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
write_IMU1 = true;
}
if (write_IMU1) {
log_msg.msg_type = LOG_IMU1_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
gyro2_timestamp = buf.sensor.gyro2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
write_IMU2 = true;
}
if (write_IMU2) {
log_msg.msg_type = LOG_IMU2_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
}
/* --- ATTITUDE --- */
src/modules/sensors/sensors.cpp
+191 -335
View File
@@ -129,6 +129,8 @@
#define STICK_ON_OFF_LIMIT 0.75f
#define MAG_ROT_VAL_INTERNAL -1
#define SENSOR_COUNT_MAX 3
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -200,25 +202,23 @@ private:
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
bool _armed; /**< arming status of the vehicle */
int _gyro_sub; /**< raw gyro0 data subscription */
int _accel_sub; /**< raw accel0 data subscription */
int _mag_sub; /**< raw mag0 data subscription */
int _gyro1_sub; /**< raw gyro1 data subscription */
int _accel1_sub; /**< raw accel1 data subscription */
int _mag1_sub; /**< raw mag1 data subscription */
int _gyro2_sub; /**< raw gyro2 data subscription */
int _accel2_sub; /**< raw accel2 data subscription */
int _mag2_sub; /**< raw mag2 data subscription */
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 */
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 */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro0 data subscription */
int _baro1_sub; /**< raw baro1 data subscription */
//int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _rc_parameter_map_sub; /**< rc parameter map subscription */
int _manual_control_sub; /**< notification of manual control updates */
int _rc_parameter_map_sub; /**< rc parameter map subscription */
int _manual_control_sub; /**< notification of manual control updates */
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
@@ -228,18 +228,6 @@ private:
orb_advert_t _airspeed_pub; /**< airspeed */
orb_advert_t _diff_pres_pub; /**< differential_pressure */
int32_t _gyro_prio; /**< gyro0 sensor priority */
int32_t _accel_prio; /**< accel0 sensor priority */
int32_t _mag_prio; /**< mag0 sensor priority */
int32_t _gyro1_prio; /**< gyro1 sensor priority */
int32_t _accel1_prio; /**<accel1 sensor priority */
int32_t _mag1_prio; /**< mag1 sensor priority */
int32_t _gyro2_prio; /**< gyro2 sensor priority */
int32_t _accel2_prio; /**< accel2 sensor priority */
int32_t _mag2_prio; /**< mag2 sensor priority */
int32_t _baro_prio; /**< baro0 sensor priority */
int32_t _baro1_prio; /**< baro1 sensor priority */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
@@ -251,7 +239,7 @@ private:
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -377,6 +365,9 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
int init_sensor_class(const struct orb_metadata *meta, int *subs,
unsigned *priorities, unsigned *errcount);
/**
* Update our local parameter cache.
*/
@@ -495,20 +486,18 @@ Sensors::Sensors() :
_sensors_task(-1),
_hil_enabled(false),
_publishing(true),
_armed(false),
/* subscriptions */
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
_gyro1_sub(-1),
_accel1_sub(-1),
_mag1_sub(-1),
_gyro2_sub(-1),
_accel2_sub(-1),
_mag2_sub(-1),
_gyro_sub{-1, -1, -1},
_accel_sub{-1, -1, -1},
_mag_sub{-1, -1, -1},
_baro_sub{-1, -1, -1},
_gyro_count(0),
_accel_count(0),
_mag_count(0),
_baro_count(0),
_rc_sub(-1),
_baro_sub(-1),
_baro1_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_rc_parameter_map_sub(-1),
@@ -523,19 +512,6 @@ Sensors::Sensors() :
_airspeed_pub(nullptr),
_diff_pres_pub(nullptr),
/* sensor priorities */
_gyro_prio(-1),
_accel_prio(-1),
_mag_prio(-1),
_gyro1_prio(-1),
_accel1_prio(-1),
_mag1_prio(-1),
_gyro2_prio(-1),
_accel2_prio(-1),
_mag2_prio(-1),
_baro_prio(-1),
_baro1_prio(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
@@ -546,6 +522,14 @@ Sensors::Sensors() :
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* 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;
}
memset(&_rc, 0, sizeof(_rc));
memset(&_diff_pres, 0, sizeof(_diff_pres));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
@@ -1032,269 +1016,132 @@ Sensors::adc_init()
void
Sensors::accel_poll(struct sensor_combined_s &raw)
{
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
for (unsigned i = 0; i < _accel_count; i++) {
bool accel_updated;
orb_check(_accel_sub[i], &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
raw.accelerometer_m_s2[1] = vect(1);
raw.accelerometer_m_s2[2] = vect(2);
raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw;
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_timestamp = accel_report.timestamp;
raw.accelerometer_priority = _accel_prio;
raw.accelerometer_errcount = accel_report.error_count;
raw.accelerometer_temp = accel_report.temperature;
}
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);
orb_check(_accel1_sub, &accel_updated);
raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
if (accel_updated) {
struct accel_report accel_report;
raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw;
raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer1_m_s2[0] = vect(0);
raw.accelerometer1_m_s2[1] = vect(1);
raw.accelerometer1_m_s2[2] = vect(2);
raw.accelerometer1_raw[0] = accel_report.x_raw;
raw.accelerometer1_raw[1] = accel_report.y_raw;
raw.accelerometer1_raw[2] = accel_report.z_raw;
raw.accelerometer1_timestamp = accel_report.timestamp;
raw.accelerometer1_priority = _accel1_prio;
raw.accelerometer1_errcount = accel_report.error_count;
raw.accelerometer1_temp = accel_report.temperature;
}
orb_check(_accel2_sub, &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer2_m_s2[0] = vect(0);
raw.accelerometer2_m_s2[1] = vect(1);
raw.accelerometer2_m_s2[2] = vect(2);
raw.accelerometer2_raw[0] = accel_report.x_raw;
raw.accelerometer2_raw[1] = accel_report.y_raw;
raw.accelerometer2_raw[2] = accel_report.z_raw;
raw.accelerometer2_timestamp = accel_report.timestamp;
raw.accelerometer2_priority = _accel2_prio;
raw.accelerometer2_errcount = accel_report.error_count;
raw.accelerometer2_temp = accel_report.temperature;
raw.accelerometer_timestamp[i] = accel_report.timestamp;
raw.accelerometer_errcount[i] = accel_report.error_count;
raw.accelerometer_temp[i] = accel_report.temperature;
}
}
}
void
Sensors::gyro_poll(struct sensor_combined_s &raw)
{
bool gyro_updated;
orb_check(_gyro_sub, &gyro_updated);
for (unsigned i = 0; i < _gyro_count; i++) {
bool gyro_updated;
orb_check(_gyro_sub[i], &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);
raw.gyro_rad_s[1] = vect(1);
raw.gyro_rad_s[2] = vect(2);
raw.gyro_rad_s[i * 3 + 0] = vect(0);
raw.gyro_rad_s[i * 3 + 1] = vect(1);
raw.gyro_rad_s[i * 3 + 2] = vect(2);
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
raw.gyro_raw[2] = gyro_report.z_raw;
math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
vect_int = _board_rotation * vect_int;
raw.timestamp = gyro_report.timestamp;
raw.gyro_priority = _gyro_prio;
raw.gyro_errcount = gyro_report.error_count;
raw.gyro_temp = gyro_report.temperature;
}
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);
orb_check(_gyro1_sub, &gyro_updated);
raw.gyro_integral_dt[i] = gyro_report.integral_dt;
if (gyro_updated) {
struct gyro_report gyro_report;
raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw;
raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro1_rad_s[0] = vect(0);
raw.gyro1_rad_s[1] = vect(1);
raw.gyro1_rad_s[2] = vect(2);
raw.gyro1_raw[0] = gyro_report.x_raw;
raw.gyro1_raw[1] = gyro_report.y_raw;
raw.gyro1_raw[2] = gyro_report.z_raw;
raw.gyro1_timestamp = gyro_report.timestamp;
raw.gyro1_priority = _gyro1_prio;
raw.gyro1_errcount = gyro_report.error_count;
raw.gyro1_temp = gyro_report.temperature;
}
orb_check(_gyro2_sub, &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro2_rad_s[0] = vect(0);
raw.gyro2_rad_s[1] = vect(1);
raw.gyro2_rad_s[2] = vect(2);
raw.gyro2_raw[0] = gyro_report.x_raw;
raw.gyro2_raw[1] = gyro_report.y_raw;
raw.gyro2_raw[2] = gyro_report.z_raw;
raw.gyro2_timestamp = gyro_report.timestamp;
raw.gyro2_priority = _gyro2_prio;
raw.gyro2_errcount = gyro_report.error_count;
raw.gyro2_temp = gyro_report.temperature;
raw.gyro_timestamp[i] = gyro_report.timestamp;
if (i == 0) {
raw.timestamp = gyro_report.timestamp;
}
raw.gyro_errcount[i] = gyro_report.error_count;
raw.gyro_temp[i] = gyro_report.temperature;
}
}
}
void
Sensors::mag_poll(struct sensor_combined_s &raw)
{
bool mag_updated;
orb_check(_mag_sub, &mag_updated);
for (unsigned i = 0; i < _mag_count; i++) {
bool mag_updated;
orb_check(_mag_sub[i], &mag_updated);
if (mag_updated) {
struct mag_report mag_report;
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
vect = _mag_rotation[0] * vect;
vect = _mag_rotation[i] * vect;
raw.magnetometer_ga[0] = vect(0);
raw.magnetometer_ga[1] = vect(1);
raw.magnetometer_ga[2] = vect(2);
raw.magnetometer_ga[i * 3 + 0] = vect(0);
raw.magnetometer_ga[i * 3 + 1] = vect(1);
raw.magnetometer_ga[i * 3 + 2] = vect(2);
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;
raw.magnetometer_raw[2] = mag_report.z_raw;
raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw;
raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
raw.magnetometer_timestamp = mag_report.timestamp;
raw.magnetometer_priority = _mag_prio;
raw.magnetometer_errcount = mag_report.error_count;
raw.magnetometer_temp = mag_report.temperature;
}
orb_check(_mag1_sub, &mag_updated);
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
vect = _mag_rotation[1] * vect;
raw.magnetometer1_ga[0] = vect(0);
raw.magnetometer1_ga[1] = vect(1);
raw.magnetometer1_ga[2] = vect(2);
raw.magnetometer1_raw[0] = mag_report.x_raw;
raw.magnetometer1_raw[1] = mag_report.y_raw;
raw.magnetometer1_raw[2] = mag_report.z_raw;
raw.magnetometer1_timestamp = mag_report.timestamp;
raw.magnetometer1_priority = _mag1_prio;
raw.magnetometer1_errcount = mag_report.error_count;
raw.magnetometer1_temp = mag_report.temperature;
}
orb_check(_mag2_sub, &mag_updated);
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
vect = _mag_rotation[2] * vect;
raw.magnetometer2_ga[0] = vect(0);
raw.magnetometer2_ga[1] = vect(1);
raw.magnetometer2_ga[2] = vect(2);
raw.magnetometer2_raw[0] = mag_report.x_raw;
raw.magnetometer2_raw[1] = mag_report.y_raw;
raw.magnetometer2_raw[2] = mag_report.z_raw;
raw.magnetometer2_timestamp = mag_report.timestamp;
raw.magnetometer2_priority = _mag2_prio;
raw.magnetometer2_errcount = mag_report.error_count;
raw.magnetometer2_temp = mag_report.temperature;
raw.magnetometer_timestamp[i] = mag_report.timestamp;
raw.magnetometer_errcount[i] = mag_report.error_count;
raw.magnetometer_temp[i] = mag_report.temperature;
}
}
}
void
Sensors::baro_poll(struct sensor_combined_s &raw)
{
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
for (unsigned i = 0; i < _baro_count; i++) {
bool baro_updated;
orb_check(_baro_sub[i], &baro_updated);
if (baro_updated) {
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar
raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
raw.baro_timestamp = _barometer.timestamp;
raw.baro_priority = _baro_prio;
}
orb_check(_baro1_sub, &baro_updated);
if (baro_updated) {
struct baro_report baro_report;
orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
raw.baro1_timestamp = baro_report.timestamp;
raw.baro1_priority = _baro1_prio;
raw.baro_timestamp[i] = _barometer.timestamp;
}
}
}
@@ -1307,12 +1154,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa;
raw.differential_pressure_timestamp[0] = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
@@ -1320,11 +1167,11 @@ 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));
_airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
@@ -1352,16 +1199,17 @@ Sensors::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
_hil_enabled = true;
_publishing = false;
_armed = vcontrol_mode.flag_armed;
/* switching from HIL to non-HIL mode */
} else if (!_publishing && !_hil_enabled) {
_hil_enabled = false;
_publishing = true;
_armed = vcontrol_mode.flag_armed;
}
}
}
@@ -1391,7 +1239,7 @@ Sensors::parameter_update_poll(bool forced)
unsigned accel_count = 0;
/* run through all gyro sensors */
for (unsigned s = 0; s < 3; s++) {
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
res = ERROR;
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
@@ -1405,7 +1253,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) {
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -1457,7 +1305,7 @@ Sensors::parameter_update_poll(bool forced)
}
/* run through all accel sensors */
for (unsigned s = 0; s < 3; s++) {
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
res = ERROR;
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@@ -1471,7 +1319,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) {
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -1523,7 +1371,7 @@ Sensors::parameter_update_poll(bool forced)
}
/* run through all mag sensors */
for (unsigned s = 0; s < 3; s++) {
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
@@ -1544,7 +1392,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) {
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -2099,6 +1947,26 @@ 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,
unsigned *priorities, unsigned *errcount)
{
unsigned group_count = orb_group_count(meta);
if (group_count > SENSOR_COUNT_MAX) {
group_count = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < group_count; i++) {
if (subs[i] < 0) {
subs[i] = orb_subscribe_multi(meta, i);
orb_priority(subs[i], (int32_t*)&priorities[i]);
}
}
return group_count;
}
void
Sensors::task_main()
{
@@ -2129,74 +1997,47 @@ Sensors::task_main()
return;
}
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
*/
_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
_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));
/*
* get sensor priorities
*/
orb_priority(_gyro_sub, &_gyro_prio);
orb_priority(_accel_sub, &_accel_prio);
orb_priority(_mag_sub, &_mag_prio);
orb_priority(_gyro1_sub, &_gyro1_prio);
orb_priority(_accel1_sub, &_accel1_prio);
orb_priority(_mag1_sub, &_mag1_prio);
orb_priority(_gyro2_sub, &_gyro2_prio);
orb_priority(_accel2_sub, &_accel2_prio);
orb_priority(_mag2_sub, &_mag2_prio);
orb_priority(_baro_sub, &_baro_prio);
orb_priority(_baro1_sub, &_baro1_prio);
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
/*
* do advertisements
*/
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.adc_voltage_v[0] = 0.0f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.adc_voltage_v[3] = 0.0f;
/* set high initial error counts to deselect gyros */
raw.gyro_errcount = 100000;
raw.gyro1_errcount = 100000;
raw.gyro2_errcount = 100000;
/* set high initial error counts to deselect accels */
raw.accelerometer_errcount = 100000;
raw.accelerometer1_errcount = 100000;
raw.accelerometer2_errcount = 100000;
/* set high initial error counts to deselect mags */
raw.magnetometer_errcount = 100000;
raw.magnetometer1_errcount = 100000;
raw.magnetometer2_errcount = 100000;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
@@ -2219,8 +2060,8 @@ Sensors::task_main()
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
fds[0].fd = _gyro_sub;
/* use the gyro to pace output */
fds[0].fd = _gyro_sub[0];
fds[0].events = POLLIN;
_task_should_exit = false;
@@ -2245,6 +2086,21 @@ Sensors::task_main()
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* keep adding sensors as long as we are not armed */
if (!_armed) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
}
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
gyro_poll(raw);
@@ -2253,13 +2109,13 @@ Sensors::task_main()
baro_poll(raw);
/* work out if main gyro timed out and fail over to alternate gyro */
if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) {
if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) {
/* if the secondary failed as well, go to the tertiary */
if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) {
fds[0].fd = _gyro2_sub;
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) {
fds[0].fd = _gyro_sub[2];
} else {
fds[0].fd = _gyro1_sub;
fds[0].fd = _gyro_sub[1];
}
}