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

Merge pull request #54 from mcsauder/master

Browse Source 
Address C99 compatibility issues by relocating variable initialization. - Replaces PR#50
This commit is contained in:
Siddharth Bharat Purohit 2016-02-23 15:53:04 -08:00
commit 9c7a3f366c
5 changed files with 186 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
EKF/common.h
View File

@ -32,13 +32,14 @@
****************************************************************************/
/**
* @file estimator_base.h
* @file common.h
* Definition of base class for attitude estimators
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
*
*/
namespace estimator
{
struct gps_message {
@ -112,54 +113,100 @@ struct flowSample {
};
struct parameters {
float mag_delay_ms = 0.0f; // magnetometer measurement delay relative to the IMU
float baro_delay_ms = 0.0f; // barometer height measurement delay relative to the IMU
float gps_delay_ms = 200.0f; // GPS measurement delay relative to the IMU
float airspeed_delay_ms = 200.0f; // airspeed measurement delay relative to the IMU
float mag_delay_ms; // magnetometer measurement delay relative to the IMU
float baro_delay_ms; // barometer height measurement delay relative to the IMU
float gps_delay_ms; // GPS measurement delay relative to the IMU
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU
// input noise
float gyro_noise = 1.0e-3f; // IMU angular rate noise used for covariance prediction
float accel_noise = 2.5e-1f; // IMU acceleration noise use for covariance prediction
float gyro_noise; // IMU angular rate noise used for covariance prediction
float accel_noise; // IMU acceleration noise use for covariance prediction
// process noise
float gyro_bias_p_noise = 7.0e-5f; // process noise for IMU delta angle bias prediction
float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction
float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction
float mag_p_noise = 2.5e-2f; // process noise for magnetic field prediction
float wind_vel_p_noise = 1.0e-1f; // process noise for wind velocity prediction
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction
float gyro_scale_p_noise; // process noise for gyro scale factor prediction
float mag_p_noise; // process noise for magnetic field prediction
float wind_vel_p_noise; // process noise for wind velocity prediction
float gps_vel_noise = 5.0e-1f; // observation noise for gps velocity fusion
float gps_pos_noise = 1.0f; // observation noise for gps position fusion
float pos_noaid_noise = 10.0f; // observation noise for non-aiding position fusion
float baro_noise = 3.0f; // observation noise for barometric height fusion
float baro_innov_gate = 3.0f; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate = 3.0f; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
float gps_vel_noise; // observation noise for gps velocity fusion
float gps_pos_noise; // observation noise for gps position fusion
float pos_noaid_noise; // observation noise for non-aiding position fusion
float baro_noise; // observation noise for barometric height fusion
float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations
float mag_heading_noise = 1.7e-1f; // measurement noise used for simple heading fusion
float mag_noise = 5.0e-2f; // measurement noise used for 3-axis magnetoemeter fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
int mag_declination_source = 3; // bitmask used to control the handling of declination data
int mag_fusion_type = 0; // integer used to specify the type of magnetometer fusion used
float mag_heading_noise; // measurement noise used for simple heading fusion
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion
float mag_declination_deg; // magnetic declination in degrees
float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations
float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations
int mag_declination_source; // bitmask used to control the handling of declination data
int mag_fusion_type; // integer used to specify the type of magnetometer fusion used
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float req_hacc = 5.0f; // maximum acceptable horizontal position error
float req_vacc = 8.0f; // maximum acceptable vertical position error
float req_sacc = 1.0f; // maximum acceptable speed error
int req_nsats = 6; // minimum acceptable satellite count
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
int gps_check_mask; // bitmask used to control which GPS quality checks are used
float req_hacc; // maximum acceptable horizontal position error
float req_vacc; // maximum acceptable vertical position error
float req_sacc; // maximum acceptable speed error
int req_nsats; // minimum acceptable satellite count
float req_gdop; // maximum acceptable geometric dilution of precision
float req_hdrift; // maximum acceptable horizontal drift speed
float req_vdrift; // maximum acceptable vertical drift speed
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
mag_delay_ms = 0.0f;
baro_delay_ms = 0.0f;
gps_delay_ms = 200.0f;
airspeed_delay_ms = 200.0f;
// input noise
gyro_noise = 1.0e-3f;
accel_noise = 2.5e-1f;
// process noise
gyro_bias_p_noise = 7.0e-5f;
accel_bias_p_noise = 1.0e-4f;
gyro_scale_p_noise = 3.0e-3f;
mag_p_noise = 2.5e-2f;
wind_vel_p_noise = 1.0e-1f;
gps_vel_noise = 5.0e-1f;
gps_pos_noise = 1.0f;
pos_noaid_noise = 10.0f;
baro_noise = 3.0f;
baro_innov_gate = 3.0f;
posNE_innov_gate = 3.0f;
vel_innov_gate = 3.0f;
mag_heading_noise = 1.7e-1f;
mag_noise = 5.0e-2f;
mag_declination_deg = 0.0f;
heading_innov_gate = 3.0f;
mag_innov_gate = 3.0f;
mag_declination_source = 7;
mag_fusion_type = 0;
gps_check_mask = 21;
req_hacc = 5.0f;
req_vacc = 8.0f;
req_sacc = 1.0f;
req_nsats = 6;
req_gdop = 2.0f;
req_hdrift = 0.3f;
req_vdrift = 0.5f;
}
};
// Bit locations for mag_declination_source
#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to contrain drift when 3-axis fusion is performed
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic

78
EKF/ekf.cpp
View File

@ -42,7 +42,6 @@
#include "ekf.h"
Ekf::Ekf():
_control_status{},
_filter_initialised(false),
_earth_rate_initialised(false),
_fuse_height(false),
@ -54,27 +53,48 @@ Ekf::Ekf():
_time_last_vel_fuse(0),
_time_last_hgt_fuse(0),
_time_last_of_fuse(0),
_vel_pos_innov{},
_mag_innov{},
_heading_innov{},
_vel_pos_innov_var{},
_mag_innov_var{},
_heading_innov_var{}
_last_disarmed_posD(0.0f),
_heading_innov(0.0f),
_heading_innov_var(0.0f),
_mag_declination(0.0f),
_gpsDriftVelN(0.0f),
_gpsDriftVelE(0.0f),
_gps_drift_velD(0.0f),
_gps_velD_diff_filt(0.0f),
_gps_velN_filt(0.0f),
_gps_velE_filt(0.0f),
_last_gps_fail_us(0),
_last_gps_origin_time_us(0),
_gps_alt_ref(0.0f),
_baro_counter(0),
_baro_sum(0.0f),
_mag_counter(0),
_baro_at_alignment(0.0f)
{
_control_status = {};
_control_status_prev = {};
_state = {};
_last_known_posNE.setZero();
_earth_rate_NED.setZero();
_R_prev = matrix::Dcm<float>();
memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov));
memset(_mag_innov, 0, sizeof(_mag_innov));
memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var));
memset(_mag_innov_var, 0, sizeof(_mag_innov_var));
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_last_known_posNE.setZero();
_imu_down_sampled = {};
_q_down_sampled.setZero();
_mag_sum = {};
_delVel_sum = {};
}
Ekf::~Ekf()
{
}
bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
@ -96,7 +116,6 @@ bool Ekf::init(uint64_t timestamp)
_output_new.pos.setZero();
_output_new.quat_nominal = matrix::Quaternion<float>();
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
@ -141,7 +160,7 @@ bool Ekf::update()
// measurement updates
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();
@ -223,7 +242,7 @@ bool Ekf::initialiseFilter(void)
_baro_sum += baro_init.hgt;
}
// check to see if we have enough measruements and return false if not
// check to see if we have enough measurements and return false if not
if (_baro_counter < 10 || _mag_counter < 10) {
return false;
@ -286,7 +305,7 @@ void Ekf::predictState()
}
}
// attitude error state prediciton
// attitude error state prediction
matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
_imu_sample_delayed.delta_ang_dt;
@ -309,10 +328,8 @@ void Ekf::predictState()
constrainStates();
}
bool Ekf::collect_imu(imuSample &imu)
{
imu.delta_ang(0) = imu.delta_ang(0) * _state.gyro_scale(0);
imu.delta_ang(1) = imu.delta_ang(1) * _state.gyro_scale(1);
imu.delta_ang(2) = imu.delta_ang(2) * _state.gyro_scale(2);
@ -320,14 +337,11 @@ bool Ekf::collect_imu(imuSample &imu)
imu.delta_ang -= _state.gyro_bias * imu.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
imu.delta_vel(2) -= _state.accel_z_bias * imu.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
// store the new sample for the complementary filter prediciton
_imu_sample_new = {
.delta_ang = imu.delta_ang,
.delta_vel = imu.delta_vel,
.delta_ang_dt = imu.delta_ang_dt,
.delta_vel_dt = imu.delta_vel_dt,
.time_us = imu.time_us
};
_imu_sample_new.delta_ang = imu.delta_ang;
_imu_sample_new.delta_vel = imu.delta_vel;
_imu_sample_new.delta_ang_dt = imu.delta_ang_dt;
_imu_sample_new.delta_vel_dt = imu.delta_vel_dt;
_imu_sample_new.time_us = imu.time_us;
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
@ -344,13 +358,13 @@ bool Ekf::collect_imu(imuSample &imu)
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
_dt_imu_avg * _imu_ticks >= 0.02f) {
imu = {
.delta_ang = _q_down_sampled.to_axis_angle(),
.delta_vel = _imu_down_sampled.delta_vel,
.delta_ang_dt = _imu_down_sampled.delta_ang_dt,
.delta_vel_dt = _imu_down_sampled.delta_vel_dt,
.time_us = imu.time_us
};
imu.delta_ang = _q_down_sampled.to_axis_angle();
imu.delta_vel = _imu_down_sampled.delta_vel;
imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
imu.time_us = imu.time_us;
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
@ -425,10 +439,8 @@ void Ekf::calculateOutputStates()
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
_vel_corr = (_state.pos - _output_sample_delayed.pos);
}
void Ekf::fuseAirspeed()
{

44
EKF/ekf.h
View File

@ -87,10 +87,10 @@ public:
bool collect_imu(imuSample &imu);
// this is the current status of the filter control modes
filter_control_status_u _control_status = {};
filter_control_status_u _control_status;
// this is the previous status of the filter control modes - used to detect mode transitions
filter_control_status_u _control_status_prev = {};
filter_control_status_u _control_status_prev;
// get the ekf WGS-84 origin position and height and the system time it was last set
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
@ -112,7 +112,7 @@ private:
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec)
uint64_t _time_last_pos_fuse; // time the last fusion of horizontal position measurements was performed (usec)
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
@ -133,7 +133,7 @@ private:
float _mag_innov_var[3]; // earth magnetic field innovation variance
float _heading_innov_var; // heading measurement innovation variance
float _mag_declination = 0.0f; // magnetic declination used by reset and fusion functions (rad)
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
// complementary filter states
Vector3f _delta_angle_corr; // delta angle correction vector
@ -143,25 +143,25 @@ private:
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
// variables used for the GPS quality checks
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
float _gpsDriftVelN; // GPS north position derivative (m/s)
float _gpsDriftVelE; // GPS east position derivative (m/s)
float _gps_drift_velD; // GPS down position derivative (m/s)
float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s)
float _gps_velN_filt; // GPS filtered North velocity (m/s)
float _gps_velE_filt; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec)
float _gps_alt_ref; // WGS-84 height (m)
// Variables used to initialise the filter states
uint8_t _baro_counter = 0; // number of baro samples averaged
float _baro_sum = 0.0f; // summed baro measurement
uint8_t _mag_counter = 0; // number of magnetometer samples averaged
Vector3f _mag_sum = {}; // summed magnetometer measurement
Vector3f _delVel_sum = {}; // summed delta velocity
float _baro_at_alignment; // baro offset relative to alignment position
uint8_t _baro_counter; // number of baro samples averaged
float _baro_sum; // summed baro measurement
uint8_t _mag_counter; // number of magnetometer samples averaged
Vector3f _mag_sum; // summed magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity
float _baro_at_alignment; // baro offset relative to alignment position
gps_check_fail_status_u _gps_check_fail_status;
@ -245,15 +245,15 @@ private:
// Determine if we are airborne or motors are armed
void calculateVehicleStatus();
// return the square of two foating point numbers - used in autocoded sections
// return the square of two floating point numbers - used in auto coded sections
inline float sq(float var)
{
return var * var;
}
// zero the specified range of rows in the state covariance matricx
// zero the specified range of rows in the state covariance matrix
void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// zero the specified range of columns in the state covariance matricx
// zero the specified range of columns in the state covariance matrix
void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
};

24
EKF/estimator_interface.cpp
View File

@ -47,8 +47,30 @@
#include "mathlib.h"
EstimatorInterface::EstimatorInterface()
EstimatorInterface::EstimatorInterface():
_dt_imu_avg(0.0f),
_imu_ticks(0),
_imu_updated(false),
_initialised(false),
_vehicle_armed(false),
_in_air(false),
_NED_origin_initialised(false),
_gps_speed_valid(false),
_gps_speed_accuracy(0.0f),
_mag_healthy(false),
_yaw_test_ratio(0.0f),
_time_last_imu(0),
_time_last_gps(0),
_time_last_mag(0),
_time_last_baro(0),
_time_last_range(0),
_time_last_airspeed(0),
_mag_declination_gps(0.0f),
_mag_declination_to_save_deg(0.0f)
{
_pos_ref = {};
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
}
EstimatorInterface::~EstimatorInterface()

31
EKF/estimator_interface.h
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file estimator_base.h
* @file estimator_interface.h
* Definition of base class for attitude estimators
*
* @author Roman Bast <bapstroman@gmail.com>
@ -80,10 +80,9 @@ public:
virtual void get_covariances(float *covariances) = 0;
// get the ekf WGS-84 origin positoin and height and the system time it was last set
// get the ekf WGS-84 origin position and height and the system time it was last set
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
@ -104,6 +103,7 @@ public:
// set magnetometer data
void setMagData(uint64_t time_usec, float *data);
//void setMagData(uint64_t time_usec, struct magSample *mag);
// set gps data
void setGpsData(uint64_t time_usec, struct gps_message *gps);
@ -188,17 +188,17 @@ protected:
uint64_t _imu_ticks; // counter for imu updates
bool _imu_updated = false; // true if the ekf should update (completed downsampling process)
bool _initialised = false; // true if ekf interface instance (data buffering) is initialised
bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground
bool _in_air = false; // we assume vehicle is in the air, set by the given landing detector
bool _imu_updated; // true if the ekf should update (completed downsampling process)
bool _initialised; // true if the ekf interface instance (data buffering) is initialized
bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground
bool _in_air; // we assume vehicle is in the air, set by the given landing detector
bool _NED_origin_initialised = false;
bool _gps_speed_valid = false;
float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s)
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
bool _NED_origin_initialised;
bool _gps_speed_valid;
float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s)
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
bool _mag_healthy = false; // computed by mag innovation test
bool _mag_healthy; // computed by mag innovation test
float _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
@ -221,7 +221,6 @@ protected:
uint64_t _time_last_range; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
fault_status_t _fault_status;
// allocate data buffers and intialise interface variables
@ -230,8 +229,6 @@ protected:
// free buffer memory
void unallocate_buffers();
float _mag_declination_gps =
0.0f; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_declination_to_save_deg = 0.0f; // magnetic declination to save to EKF2_MAG_DECL (deg)
float _mag_declination_gps; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_declination_to_save_deg; // magnetic declination to save to EKF2_MAG_DECL (deg)
};