zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Add required declarations for optical flow

master
Paul Riseborough 9 years ago
parent
75b22c95ac
commit
dca186c6e8
3 changed files with 243 additions and 93 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 178
      EKF/common.h
  2. 113
      EKF/ekf.h
  3. 45
      EKF/estimator_interface.h

178
EKF/common.h
Unescape View File

@ -64,6 +64,13 @@ typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion; typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> Matrix3f; typedef matrix::Matrix<float, 3, 3> Matrix3f;
struct flow_message {
uint8_t quality; // Quality of Flow data
Vector2f flowdata; // Flow data received
Vector2f gyrodata; // Gyro data from flow sensor
uint32_t dt; // integration time of flow samples
};
struct outputSample { struct outputSample {
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
Vector3f vel; // NED velocity estimate in earth frame in m/s Vector3f vel; // NED velocity estimate in earth frame in m/s
@ -107,43 +114,90 @@ struct airspeedSample {
}; };
struct flowSample { struct flowSample {
Vector2f flowRadXY; uint8_t quality; // quality indicator between 0 and 255
Vector2f flowRadXYcomp; Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
uint64_t time_us; Vector2f flowRadXYcomp; // measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive
float dt; // amount of integration time (sec)
uint64_t time_us; // timestamp in microseconds of the integration period mid-point
}; };
enum vdist_sensor_type_t {
VDIST_SENSOR_RANGE,
VDIST_SENSOR_BARO,
VDIST_SENSOR_GPS,
VDIST_SENSOR_NONE
};
// 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 constrain drift when 3-axis fusion is performed
// Bit locations for fusion_mode
#define MASK_USE_GPS (1<<0) // set to true to use GPS data
#define MASK_USE_OF (1<<1) // set to true to use optical flow data
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
struct parameters { struct parameters {
float mag_delay_ms; // magnetometer measurement delay relative to the IMU // measurement source control
float baro_delay_ms; // barometer height measurement delay relative to the IMU int fusion_mode;
float gps_delay_ms; // GPS measurement delay relative to the IMU int vdist_sensor_type;
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU
// measurement time delays
float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec)
float baro_delay_ms; // barometer height measurement delay relative to the IMU (msec)
float gps_delay_ms; // GPS measurement delay relative to the IMU (msec)
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec)
float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
float range_delay_ms; // range finder measurement delay relative to the IMU (msec)
// input noise // input noise
float gyro_noise; // IMU angular rate noise used for covariance prediction float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise; // IMU acceleration noise use for covariance prediction float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec)
// process noise // process noise
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec)
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec)
float gyro_scale_p_noise; // process noise for gyro scale factor prediction float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A)
float mag_p_noise; // process noise for magnetic field prediction float mag_p_noise; // process noise for magnetic field prediction (Guass/sec)
float wind_vel_p_noise; // process noise for wind velocity prediction float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec)
float terrain_p_noise; // process noise for terrain offset (m/sec)
float gps_vel_noise; // observation noise for gps velocity fusion
float gps_pos_noise; // observation noise for gps position fusion // position and velocity fusion
float pos_noaid_noise; // observation noise for non-aiding position fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec)
float baro_noise; // observation noise for barometric height fusion float gps_pos_noise; // observation noise for gps position fusion (m)
float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations float pos_noaid_noise; // observation noise for non-aiding position fusion (m)
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations float baro_noise; // observation noise for barometric height fusion (m)
float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations float baro_innov_gate; // barometric height innovation consistency gate size (STD)
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size (STD)
float mag_heading_noise; // measurement noise used for simple heading fusion float vel_innov_gate; // GPS velocity innovation consistency gate size (STD)
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion
float mag_declination_deg; // magnetic declination in degrees // magnetometer fusion
float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations float mag_heading_noise; // measurement noise used for simple heading fusion (rad)
float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion (Gauss)
int mag_declination_source; // bitmask used to control the handling of declination data float mag_declination_deg; // magnetic declination (degrees)
int mag_fusion_type; // integer used to specify the type of magnetometer fusion used float heading_innov_gate; // heading fusion innovation consistency gate size (STD)
float mag_innov_gate; // magnetometer fusion innovation consistency gate size (STD)
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
// range finder fusion
float range_noise; // observation noise for range finder measurements (m)
float range_innov_gate; // range finder fusion innovation consistency gate size (STD)
float rng_gnd_clearance; // minimum valid value for range when on ground (m)
// optical flow fusion
float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int flow_qual_min; // minimum acceptable quality integer from the flow sensor
float flow_innov_gate; // optical flow fusion innovation consistency gate size (STD)
float flow_rate_max; // maximum valid optical flow rate (rad/sec)
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is // 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 // good enough to set a local origin and commence aiding
@ -159,10 +213,17 @@ struct parameters {
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters() parameters()
{ {
// measurement source control
fusion_mode = MASK_USE_OF;
vdist_sensor_type = VDIST_SENSOR_BARO;
// measurement time delays
mag_delay_ms = 0.0f; mag_delay_ms = 0.0f;
baro_delay_ms = 0.0f; baro_delay_ms = 0.0f;
gps_delay_ms = 200.0f; gps_delay_ms = 200.0f;
airspeed_delay_ms = 200.0f; airspeed_delay_ms = 200.0f;
flow_delay_ms = 60.0f;
range_delay_ms = 200.0f;
// input noise // input noise
gyro_noise = 1.0e-3f; gyro_noise = 1.0e-3f;
@ -174,7 +235,9 @@ struct parameters {
gyro_scale_p_noise = 3.0e-3f; gyro_scale_p_noise = 3.0e-3f;
mag_p_noise = 2.5e-2f; mag_p_noise = 2.5e-2f;
wind_vel_p_noise = 1.0e-1f; wind_vel_p_noise = 1.0e-1f;
terrain_p_noise = 0.5f;
// position and velocity fusion
gps_vel_noise = 5.0e-1f; gps_vel_noise = 5.0e-1f;
gps_pos_noise = 1.0f; gps_pos_noise = 1.0f;
pos_noaid_noise = 10.0f; pos_noaid_noise = 10.0f;
@ -183,15 +246,28 @@ struct parameters {
posNE_innov_gate = 3.0f; posNE_innov_gate = 3.0f;
vel_innov_gate = 3.0f; vel_innov_gate = 3.0f;
mag_heading_noise = 5.0e-1f; // magnetometer fusion
mag_heading_noise = 1.7e-1f;
mag_noise = 5.0e-2f; mag_noise = 5.0e-2f;
mag_declination_deg = 0.0f; mag_declination_deg = 0.0f;
heading_innov_gate = 3.0f; heading_innov_gate = 3.0f;
mag_innov_gate = 3.0f; mag_innov_gate = 3.0f;
mag_declination_source = 3;
mag_declination_source = 7;
mag_fusion_type = 0; mag_fusion_type = 0;
// range finder fusion
range_noise = 0.1f;
range_innov_gate = 5.0f;
rng_gnd_clearance = 0.1f;
// optical flow fusion
flow_noise = 0.15f;
flow_noise_qual_min = 0.5f;
flow_qual_min = 1;
flow_innov_gate = 3.0f;
flow_rate_max = 2.5f;
// GPS quality checks
gps_check_mask = 21; gps_check_mask = 21;
req_hacc = 5.0f; req_hacc = 5.0f;
req_vacc = 8.0f; req_vacc = 8.0f;
@ -203,17 +279,6 @@ struct parameters {
} }
}; };
// 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 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
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
struct stateSample { struct stateSample {
Vector3f ang_error; // attitude axis angle error (error state formulation) Vector3f ang_error; // attitude axis angle error (error state formulation)
Vector3f vel; // NED velocity in earth frame in m/s Vector3f vel; // NED velocity in earth frame in m/s
@ -259,17 +324,20 @@ union gps_check_fail_status_u {
// bitmask containing filter control status // bitmask containing filter control status
union filter_control_status_u { union filter_control_status_u {
struct { struct {
uint8_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete uint16_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete
uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete uint16_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
uint8_t gps : 1; // 2 - true if GPS measurements are being fused uint16_t gps : 1; // 2 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused uint16_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused uint16_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
uint8_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused uint16_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
uint8_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused uint16_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused uint16_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 8 - true when the vehicle is airborne uint16_t in_air : 1; // 8 - true when the vehicle is airborne
uint8_t armed : 1; // 9 - true when the vehicle motors are armed uint16_t armed : 1; // 9 - true when the vehicle motors are armed
uint8_t wind : 1; // 10 - true when wind velocity is being estimated uint16_t wind : 1; // 10 - true when wind velocity is being estimated
uint16_t baro_hgt : 1; // 11 - true when baro height is being fused as a primary height reference
uint16_t rng_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference
uint16_t gps_hgt : 1; // 15 - true when range finder height is being fused as a primary height reference
} flags; } flags;
uint16_t value; uint16_t value;
}; };

113
EKF/ekf.h
Unescape View File

@ -76,6 +76,18 @@ public:
// gets the innovation variance of the heading measurement // gets the innovation variance of the heading measurement
void get_heading_innov_var(float *heading_innov_var); void get_heading_innov_var(float *heading_innov_var);
// gets the innovation variance of the flow measurement
void get_flow_innov_var(float flow_innov_var[2]);
// gets the innovation of the flow measurement
void get_flow_innov(float flow_innov[2]);
// gets the innovation variance of the HAGL measurement
void get_hagl_innov_var(float *hagl_innov_var);
// gets the innovation of the HAGL measurement
void get_hagl_innov(float *hagl_innov);
// get the state vector at the delayed time horizon // get the state vector at the delayed time horizon
void get_state_delayed(float *state); void get_state_delayed(float *state);
@ -98,6 +110,17 @@ public:
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning); void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
void get_vel_var(Vector3f &vel_var);
void get_pos_var(Vector3f &pos_var);
// return true if the global position estimate is valid
bool global_position_is_valid();
// return true if the etimate is valid
// return the estimated terrain vertical position relative to the NED origin
bool get_terrain_vert_pos(float *ret);
private: private:
static const uint8_t _k_num_states = 24; static const uint8_t _k_num_states = 24;
@ -105,13 +128,15 @@ private:
stateSample _state; // state struct of the ekf running at the delayed time horizon stateSample _state; // state struct of the ekf running at the delayed time horizon
bool _filter_initialised; bool _filter_initialised; // true when the EKF sttes and covariances been initialised
bool _earth_rate_initialised; bool _earth_rate_initialised; // true when we know the earth rotatin rate (requires GPS)
bool _fuse_height; // baro height data should be fused bool _fuse_height; // baro height data should be fused
bool _fuse_pos; // gps position data should be fused bool _fuse_pos; // gps position data should be fused
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
bool _fuse_flow; // flow measurement should be fused
bool _fuse_hagl_data; // if true then range data will be fused to estimate terrain height
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
@ -131,45 +156,64 @@ private:
float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update
float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos
float _mag_innov[3]; // earth magnetic field innovations float _mag_innov[3]; // earth magnetic field innovations
float _mag_innov_var[3]; // earth magnetic field innovation variance
float _heading_innov; // heading measurement innovation float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance
Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion
float _tilt_err_length_filt; // filtered length of _tilt_err_vec
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos // optical flow processing
float _mag_innov_var[3]; // earth magnetic field innovation variance float _flow_innov[2]; // flow measurement innovation
float _heading_innov_var; // heading measurement innovation variance float _flow_innov_var[2]; // flow innovation variance
Vector2f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs
Vector2f _imu_del_ang_of; // bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates
float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over
float _mag_declination; // magnetic declination used by reset and fusion functions (rad) float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
// complementary filter states // complementary filter states
Vector3f _delta_angle_corr; // delta angle correction vector Vector3f _delta_angle_corr; // delta angle correction vector
Vector3f _delta_vel_corr; // delta velocity correction vector Vector3f _delta_vel_corr; // delta velocity correction vector
Vector3f _vel_corr; // velocity correction vector Vector3f _vel_corr; // velocity correction vector
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
// variables used for the GPS quality checks // variables used for the GPS quality checks
float _gpsDriftVelN; // GPS north position derivative (m/s) float _gpsDriftVelN; // GPS north position derivative (m/s)
float _gpsDriftVelE; // GPS east position derivative (m/s) float _gpsDriftVelE; // GPS east position derivative (m/s)
float _gps_drift_velD; // GPS down 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_velD_diff_filt; // GPS filtered Down velocity (m/s)
float _gps_velN_filt; // GPS filtered North velocity (m/s) float _gps_velN_filt; // GPS filtered North velocity (m/s)
float _gps_velE_filt; // GPS filtered East 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 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 // Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec) uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec)
float _gps_alt_ref; // WGS-84 height (m) float _gps_alt_ref; // WGS-84 height (m)
// Variables used to initialise the filter states // Variables used to initialise the filter states
uint8_t _baro_counter; // number of baro samples averaged uint8_t _hgt_counter; // number of baro samples averaged
float _baro_sum; // summed baro measurement float _hgt_sum; // summed baro measurement
uint8_t _mag_counter; // number of magnetometer samples averaged uint8_t _mag_counter; // number of magnetometer samples averaged
Vector3f _mag_sum; // summed magnetometer measurement Vector3f _mag_sum; // summed magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity Vector3f _delVel_sum; // summed delta velocity
float _baro_at_alignment; // baro offset relative to alignment position float _hgt_at_alignment; // baro offset relative to alignment position
gps_check_fail_status_u _gps_check_fail_status; gps_check_fail_status_u _gps_check_fail_status;
// Terrain height state estimation
float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
float _terrain_var; // variance of terrain position estimate (m^2)
float _hagl_innov; // innovation of the last height above terrain measurement (m)
float _hagl_innov_var; // innovation variance for the last height above terrain measurement (m^2)
uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks
bool _terrain_initialised; // true when the terrain estimator has been intialised
// update the real time complementary filter states. This includes the prediction // update the real time complementary filter states. This includes the prediction
// and the correction step // and the correction step
void calculateOutputStates(); void calculateOutputStates();
@ -201,15 +245,28 @@ private:
// fuse airspeed measurement // fuse airspeed measurement
void fuseAirspeed(); void fuseAirspeed();
// fuse range measurements
void fuseRange();
// fuse velocity and position measurements (also barometer height) // fuse velocity and position measurements (also barometer height)
void fuseVelPosHeight(); void fuseVelPosHeight();
// reset velocity states of the ekf // reset velocity states of the ekf
void resetVelocity(); void resetVelocity();
// fuse optical flow line of sight rate measurements
void fuseOptFlow();
// calculate optical flow bias errors
void calcOptFlowBias();
// initialise the terrain vertical position estimator
// return true if the initialisation is successful
bool initHagl();
// predict the terrain vertical position state and variance
void predictHagl();
// update the terrain vertical position estimate using a height above ground measurement from the range finder
void fuseHagl();
// reset the heading and magnetic field states using the declination and magnetometer measurements // reset the heading and magnetic field states using the declination and magnetometer measurements
// return true if successful // return true if successful
bool resetMagHeading(Vector3f &mag_init); bool resetMagHeading(Vector3f &mag_init);

45
EKF/estimator_interface.h
Unescape View File

@ -81,6 +81,22 @@ public:
virtual void get_covariances(float *covariances) = 0; virtual void get_covariances(float *covariances) = 0;
// get the ekf WGS-84 origin position 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_vel_var(Vector3f &vel_var) = 0;
virtual void get_pos_var(Vector3f &pos_var) = 0;
// gets the innovation variance of the flow measurement
virtual void get_flow_innov_var(float flow_innov_var[2]) = 0;
// gets the innovation of the flow measurement
virtual void get_flow_innov(float flow_innov[2]) = 0;
// gets the innovation variance of the HAGL measurement
virtual void get_hagl_innov_var(float *flow_innov_var) = 0;
// gets the innovation of the HAGL measurement
virtual void get_hagl_innov(float *flow_innov_var) = 0;
// get the ekf WGS-84 origin positoin 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; virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@ -99,7 +115,7 @@ public:
virtual bool collect_range(uint64_t time_usec, float *data) { return true; } virtual bool collect_range(uint64_t time_usec, float *data) { return true; }
virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; } virtual bool collect_opticalflow(uint64_t time_usec, flow_message *flow) { return true; }
// set delta angle imu data // set delta angle imu data
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
@ -121,7 +137,7 @@ public:
void setRangeData(uint64_t time_usec, float *data); void setRangeData(uint64_t time_usec, float *data);
// set optical flow data // set optical flow data
void setOpticalFlowData(uint64_t time_usec, float *data); void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
// return a address to the parameters struct // return a address to the parameters struct
// in order to give access to the application // in order to give access to the application
@ -133,7 +149,15 @@ public:
// set vehicle landed status data // set vehicle landed status data
void set_in_air_status(bool in_air) {_in_air = in_air;} void set_in_air_status(bool in_air) {_in_air = in_air;}
bool position_is_valid(); // return true if the global position estimate is valid
virtual bool global_position_is_valid() = 0;
// return true if the estimate is valid
// return the estimated terrain vertical position relative to the NED origin
virtual bool get_terrain_vert_pos(float *ret) = 0;
// return true if the local position estimate is valid
bool local_position_is_valid();
void copy_quaternion(float *quat) void copy_quaternion(float *quat)
@ -196,13 +220,13 @@ protected:
bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground 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 _in_air; // we assume vehicle is in the air, set by the given landing detector
bool _NED_origin_initialised; bool _NED_origin_initialised = false;
bool _gps_speed_valid; bool _gps_speed_valid = false;
float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s) float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s)
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m)
float _gps_hpos_accuracy; // GPS receiver reported 1-sigma horizontal accuracy (m) float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
float _gps_origin_eph; // horizontal position uncertainty of the GPS origin float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
float _gps_origin_epv; // vertical position uncertainty of the GPS origin struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
bool _mag_healthy; // computed by mag innovation test bool _mag_healthy; // computed by mag innovation test
float _yaw_test_ratio; // yaw innovation consistency check ratio float _yaw_test_ratio; // yaw innovation consistency check ratio
@ -226,6 +250,7 @@ protected:
uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds
uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_range; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_optflow;
fault_status_t _fault_status; fault_status_t _fault_status;

Write Preview
Loading…
Cancel
Save