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

Merge pull request #316 from PX4/pr-ekfTrueAirspeed 

EKF: Add true airspeed accessor
This commit is contained in:
Paul Riseborough 2017-08-11 08:18:04 +10:00 committed by GitHub
parent 967720a9d7 35ffd55481
commit 3983ac23fa
4 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/airspeed_fusion.cpp
View File

@ -225,6 +225,12 @@ void Ekf::get_wind_velocity(float *wind)
wind[1] = _state.wind_vel(1);
}
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)));
memcpy(tas, &tempvar, sizeof(float));
}
/*
* Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
*/

3
EKF/ekf.h
View File

@ -112,6 +112,9 @@ public:
// get the wind velocity in m/s
void get_wind_velocity(float *wind);
// get the true airspeed in m/s
void get_true_airspeed(float *tas);
// get the diagonal elements of the covariance matrix
void get_covariances(float *covariances);

2
EKF/estimator_interface.h
View File

@ -93,6 +93,8 @@ public:
virtual void get_wind_velocity(float *wind) = 0;
virtual void get_true_airspeed(float *tas) = 0;
virtual void get_covariances(float *covariances) = 0;
// gets the variances for the NED velocity states

12
EKF/mag_fusion.cpp
View File

@ -537,12 +537,12 @@ void Ekf::fuseHeading()
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
Dcmf R_to_earth;
float sy = sin(yaw);
float cy = cos(yaw);
float sp = sin(pitch);
float cp = cos(pitch);
float sr = sin(roll);
float cr = cos(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);
float sp = sinf(pitch);
float cp = cosf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
R_to_earth(0,0) = cy*cp-sy*sp*sr;
R_to_earth(0,1) = -sy*cr;
R_to_earth(0,2) = cy*sp+sy*cp*sr;