WindEKFGlue::Result
WindEKFGlue::Update(const NMEAInfo &basic, const DerivedInfo &derived)
{
// @todo accuracy: correct TAS for vertical speed if dynamic pullup
// reset if flight hasnt started or airspeed instrument not available
if (!derived.flight.flying ||
!basic.airspeed_available || !basic.airspeed_real ||
basic.true_airspeed < fixed_one) {
reset();
return Result(0);
}
// temporary manoeuvering, dont append this point
if ((fabs(derived.turn_rate) > fixed(20)) ||
(fabs(basic.acceleration.g_load - fixed_one) > fixed(0.3))) {
blackout(basic.time);
return Result(0);
}
if (in_blackout(basic.time))
return Result(0);
// clear blackout
blackout((unsigned)-1);
float V = basic.true_airspeed;
float dynamic_pressure = V*V;
float gps_vel[2];
fixed gps_east, gps_north;
basic.track.sin_cos(gps_east, gps_north);
gps_vel[0] = (float)(gps_east * basic.ground_speed);
gps_vel[1] = (float)(gps_north * basic.ground_speed);
float dT = 1.0;
StatePrediction(gps_vel, dT);
Correction(dynamic_pressure, gps_vel);
// CovariancePrediction(dT);
const float* x = get_state();
Result res;
static int j=0;
j++;
if (j%10==0)
res.quality = 1;
else
res.quality = 0;
res.wind = SpeedVector(fixed(-x[0]), fixed(-x[1]));
return res;
}
static void
TestAll()
{
TestWind(SpeedVector(Angle::Zero(), fixed(0)));
TestWind(SpeedVector(Angle::Zero(), fixed(2)));
TestWind(SpeedVector(Angle::Zero(), fixed(5)));
TestWind(SpeedVector(Angle::Zero(), fixed(10)));
TestWind(SpeedVector(Angle::Zero(), fixed(15)));
TestWind(SpeedVector(Angle::Zero(), fixed(30)));
}
FinalGlideBarWindow(const FinalGlideBarLook &look, const TaskLook &task_look)
:renderer(look, task_look),
state(GeoVector(fixed(100), Angle::Zero()),
fixed(1000), fixed(1000),
SpeedVector(Angle::Zero(), fixed(0)))
{
glide_polar = GlidePolar(fixed(0));
calculated.task_stats.total.solution_remaining =
MacCready::Solve(glide_settings, glide_polar, state);
calculated.task_stats.total.solution_mc0 =
MacCready::Solve(glide_settings, glide_polar, state);
calculated.task_stats.task_valid = true;
}
void
WindStore::NewWind(const NMEAInfo &info, DerivedInfo &derived,
Vector &wind) const
{
fixed mag = hypot(wind.x, wind.y);
Angle bearing;
if (wind.y == fixed_zero && wind.x == fixed_zero)
bearing = Angle::Zero();
else
bearing = Angle::Radians(atan2(wind.y, wind.x));
if (mag < fixed(30)) { // limit to reasonable values
derived.estimated_wind = SpeedVector(bearing.AsBearing(), mag);
derived.estimated_wind_available.Update(update_clock);
} else {
// TODO code: give warning, wind estimate bogus or very strong!
}
#ifdef DEBUG_WIND
LogDebug(_T("%f %f 0 # wind estimate\n"), wind.x, wind.y);
#endif
}
void
WindStore::NewWind(const NMEAInfo &info, DerivedInfo &derived,
Vector &wind) const
{
auto mag = wind.Magnitude();
Angle bearing;
if (wind.y == 0 && wind.x == 0)
bearing = Angle::Zero();
else
bearing = Angle::FromXY(wind.x, wind.y);
if (mag < 30) { // limit to reasonable values
derived.estimated_wind = SpeedVector(bearing.AsBearing(), mag);
derived.estimated_wind_available.Update(update_clock);
} else {
// TODO code: give warning, wind estimate bogus or very strong!
}
#ifdef DEBUG_WIND
LogDebug(_T("%f %f 0 # wind estimate\n"), wind.x, wind.y);
#endif
}
CirclingWind::Result
CirclingWind::NewSample(const MoreData &info, const CirclingInfo &circling)
{
if (!circling.circling) {
Reset();
return Result(0);
}
if (!active) {
active = true;
/* reset the circlecounter for each flightmode change; the
important thing to measure is the number of turns in this
thermal only. */
circle_count = 0;
current_circle = Angle::Zero();
last_track_available.Clear();
last_ground_speed_available.Clear();
samples.clear();
}
if (last_track_available.FixTimeWarp(info.track_available, 30) ||
last_ground_speed_available.FixTimeWarp(info.ground_speed_available, 30))
/* time warp: start from scratch */
Reset();
if (!info.track_available.Modified(last_track_available) ||
!info.ground_speed_available.Modified(last_ground_speed_available))
/* no updated GPS fix */
return Result(0);
const bool previous_track_available = last_track_available;
last_track_available = info.track_available;
last_ground_speed_available = info.ground_speed_available;
// Circle detection
if (previous_track_available)
current_circle += (info.track - last_track).AsDelta().Absolute();
last_track = info.track;
const bool fullCircle = current_circle >= Angle::FullCircle();
if (fullCircle) {
//full circle made!
current_circle -= Angle::FullCircle();
circle_count++; //increase the number of circles flown (used
//to determine the quality)
}
if (!samples.full()) {
Sample &sample = samples.append();
sample.time = info.clock;
sample.vector = SpeedVector(info.track, info.ground_speed);
} else {
// TODO code: give error, too many wind samples
// or use circular buffer
}
Result result(0);
if (fullCircle) { //we have completed a full circle!
if (!samples.full())
// calculate the wind for this circle, only if it is valid
result = CalcWind();
samples.clear();
}
return result;
}
/**
* Return the vector with the bearing rotated by 180 degrees.
*/
gcc_pure
SpeedVector Reciprocal() const {
return SpeedVector(bearing.Reciprocal(), norm);
}
/**
* Returns the null vector.
*/
static constexpr SpeedVector Zero() {
return SpeedVector(Angle::Zero(), fixed(0));
}
RoutePlannerGlue::RoutePlannerGlue(const GlidePolar& polar,
const Airspaces& master):
terrain(NULL),
m_planner(polar, SpeedVector(Angle::degrees(fixed_zero), fixed_zero), master)
{
}
