Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: start with a wind estimate of 3m/s, when no direct measurement

Browse Source 
this will cope better with users with low roll/pitch gains, to ensure
they get enough control on takeoff

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2014-04-13 19:42:23 +10:00
parent d745dc2b6f
commit 4abc2999a6
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -95,6 +95,10 @@ extern const AP_HAL::HAL& hal;
#define earthRate 0.000072921f // earth rotation rate (rad/sec)
// when the wind estimation first starts with no airspeed sensor,
// assume 3m/s to start
#define STARTUP_WIND_SPEED 3.0f
// Define tuning parameters
const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
@ -3142,16 +3146,18 @@ void NavEKF::alignYawGPS()
}
}
// This function is used to do a forced alignment of the wind velocity states so that they are set to the reciprocal of
// the ground speed and scaled to 6 m/s. This is used when launching a fly-forward vehicle without an airspeed sensor
// on the assumption that launch will be into wind and 6 is representative global average at height
// http://maps.google.com/gallery/details?id=zJuaSgXp_WLc.kTBytKPmNODY&hl=en
// This function is used to do a forced alignment of the wind velocity
// states so that they are set to the reciprocal of the ground speed
// and scaled to STARTUP_WIND_SPEED m/s. This is used when launching a
// fly-forward vehicle without an airspeed sensor on the assumption
// that launch will be into wind and STARTUP_WIND_SPEED is
// representative of typical launch wind
void NavEKF::setWindVelStates()
{
float gndSpd = sqrtf(sq(states[4]) + sq(states[5]));
if (gndSpd > 4.0f) {
// set the wind states to be the reciprocal of the velocity and scale to 6 m/s
float scaleFactor = 6.0f / gndSpd;
float scaleFactor = STARTUP_WIND_SPEED / gndSpd;
states[14] = - states[4] * scaleFactor;
states[15] = - states[5] * scaleFactor;
// reinitialise the wind state covariances