dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #330 from CarlOlsson/pr-add_get_wind_vel_var 

EKF: added get_wind_velocity_var function
This commit is contained in:
Paul Riseborough 2017-10-12 09:12:54 +11:00 committed by GitHub
parent 6cb99ce8a8 8200e4b218
commit 68bad48598
3 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/airspeed_fusion.cpp
View File

@ -224,6 +224,12 @@ void Ekf::get_wind_velocity(float *wind)
wind[1] = _state.wind_vel(1); wind[1] = _state.wind_vel(1);
} }
void Ekf::get_wind_velocity_var(float *wind_var)
{
wind_var[0] = P[22][22];
wind_var[1] = P[23][23];
}
void Ekf::get_true_airspeed(float *tas) void Ekf::get_true_airspeed(float *tas)
{ {
float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));

3
EKF/ekf.h
View File

@ -112,6 +112,9 @@ public:
// get the wind velocity in m/s // get the wind velocity in m/s
void get_wind_velocity(float *wind); void get_wind_velocity(float *wind);
// get the wind velocity var
void get_wind_velocity_var(float *wind_var);
// get the true airspeed in m/s // get the true airspeed in m/s
void get_true_airspeed(float *tas); void get_true_airspeed(float *tas);

2
EKF/estimator_interface.h
View File

@ -93,6 +93,8 @@ public:
virtual void get_wind_velocity(float *wind) = 0; virtual void get_wind_velocity(float *wind) = 0;
virtual void get_wind_velocity_var(float *wind_var) = 0;
virtual void get_true_airspeed(float *tas) = 0; virtual void get_true_airspeed(float *tas) = 0;
virtual void get_covariances(float *covariances) = 0; virtual void get_covariances(float *covariances) = 0;