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

added interface for parameters

Browse Source
This commit is contained in:
Roman 2015-12-26 07:14:57 +01:00 committed by Roman Bapst
parent 9b7afa2600
commit 86df68e404
2 changed files with 32 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
EKF/estimator_base.cpp
View File

@ -144,7 +144,6 @@ void EstimatorBase::setMagData(uint64_t time_usec, float *data)
magSample mag_sample_new = {};
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_mag = time_usec;
@ -260,27 +259,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
_state.quat_nominal.setZero();
_state.quat_nominal(0) = 1.0f;
_params.mag_delay_ms = 0;
_params.baro_delay_ms = 0;
_params.gps_delay_ms = 200;
_params.airspeed_delay_ms = 0;
_params.requiredEph = 500;
_params.requiredEpv = 800;
_params.gyro_noise = 1e-3f;
_params.accel_noise = 1e-1f;
_params.gyro_bias_p_noise = 1e-5f;
_params.accel_bias_p_noise = 1e-3f;
_params.gyro_scale_p_noise = 1e-4f;
_params.mag_p_noise = 1e-2f;
_params.wind_vel_p_noise = 0.05f;
_params.gps_vel_noise = 0.05f;
_params.gps_pos_noise = 1.0f;
_params.baro_noise = 0.1f;
_params.mag_heading_noise = 3e-2f;
_params.mag_declination_deg = 0.0f;
_params.heading_innov_gate = 0.5f;
_dt_imu_avg = 0.0f;
_imu_time_last = time_usec;

59
EKF/estimator_base.h
View File

@ -58,6 +58,34 @@ struct gps_message {
bool vel_ned_valid; // GPS ground speed is valid
};
struct parameters {
float mag_delay_ms = 0.0f;
float baro_delay_ms = 0.0f;
float gps_delay_ms = 200.0f;
float airspeed_delay_ms = 200.0f;
float requiredEph = 10.0f;
float requiredEpv = 20.0f;
// input noise
float gyro_noise = 0.001f;
float accel_noise = 0.1f;
// process noise
float gyro_bias_p_noise = 1e-5f;
float accel_bias_p_noise = 1e-3f;
float gyro_scale_p_noise = 1e-4f;
float mag_p_noise = 1e-2f;
float wind_vel_p_noise = 0.05f;
float gps_vel_noise = 0.05f;
float gps_pos_noise = 1.0f;
float baro_noise = 0.1f;
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees
float heading_innov_gate = 0.5f; // innovation gate for heading innovation test
};
class EstimatorBase
{
public:
@ -87,6 +115,9 @@ public:
// set optical flow data
void setOpticalFlowData(uint64_t time_usec, float *data);
// return a address to the parameters struct
// in order to give access to the application
parameters *getParamHandle() {return &_params;}
protected:
typedef matrix::Vector<float, 2> Vector2f;
@ -155,33 +186,7 @@ protected:
uint64_t time_us;
};
struct {
uint32_t mag_delay_ms;
uint32_t baro_delay_ms;
uint32_t gps_delay_ms;
uint32_t airspeed_delay_ms;
float requiredEph;
float requiredEpv;
float gyro_noise;
float accel_noise;
// process noise
float gyro_bias_p_noise;
float accel_bias_p_noise;
float gyro_scale_p_noise;
float mag_p_noise;
float wind_vel_p_noise;
float gps_vel_noise;
float gps_pos_noise;
float baro_noise;
float mag_heading_noise; // measurement noise used for simple heading fusion
float mag_declination_deg; // magnetic declination in degrees
float heading_innov_gate; // innovation gate for heading innovation test
} _params;
parameters _params; // filter parameters
static const uint8_t OBS_BUFFER_LENGTH = 10;
static const uint8_t IMU_BUFFER_LENGTH = 30;