fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved initialization to object constructors to allow C99 compiler compatibility.

Browse Source
This commit is contained in:
mcsauder
2016-02-18 03:21:04 -07:00
parent 96c0e18acb
commit 437f6ca5fb
5 changed files with 260 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.h
+30 -34
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>
@@ -41,7 +41,7 @@
#include <stdint.h>
#include <matrix/matrix/math.hpp>
#include <lib/geo/geo.h>
#include <geo/geo.h>
#include "RingBuffer.h"
#include "common.h"
@@ -81,10 +81,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; }
@@ -105,6 +104,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);
@@ -165,15 +165,15 @@ public:
protected:
parameters _params; // filter parameters
parameters _params; // filter parameters
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
float _dt_imu_avg; // average imu update period in s
float _dt_imu_avg; // average imu update period in s
imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon
imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon
// measurement samples capturing measurements on the delayed time horizon
magSample _mag_sample_delayed;
@@ -183,26 +183,25 @@ protected:
airspeedSample _airspeed_sample_delayed;
flowSample _flow_sample_delayed;
outputSample _output_sample_delayed; // filter output on the delayed time horizon
outputSample _output_new; // filter output on the non-delayed time horizon
imuSample _imu_sample_new; // imu sample capturing the newest imu data
outputSample _output_sample_delayed; // filter output on the delayed time horizon
outputSample _output_new; // filter output on the non-delayed time horizon
imuSample _imu_sample_new; // imu sample capturing the newest imu data
uint64_t _imu_ticks; // counter for imu updates
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
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
// data buffer instances
@@ -215,14 +214,13 @@ protected:
RingBuffer<flowSample> _flow_buffer;
RingBuffer<outputSample> _output_buffer;
uint64_t _time_last_imu; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds
uint64_t _time_last_mag; // timestamp of last magnetometer 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_imu; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds
uint64_t _time_last_mag; // timestamp of last magnetometer 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_airspeed; // timestamp of last airspeed measurement in microseconds
fault_status_t _fault_status;
// allocate data buffers and intialise interface variables
@@ -231,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)
};