Пример #1
0
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;
}
Пример #2
0
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)));
}
Пример #3
0
  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;
  }
Пример #4
0
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

}
Пример #5
0
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

}
Пример #6
0
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;
}
Пример #7
0
 /**
  * Return the vector with the bearing rotated by 180 degrees.
  */
 gcc_pure
 SpeedVector Reciprocal() const {
   return SpeedVector(bearing.Reciprocal(), norm);
 }
Пример #8
0
 /**
  * Returns the null vector.
  */
 static constexpr SpeedVector Zero() {
   return SpeedVector(Angle::Zero(), fixed(0));
 }
Пример #9
0
RoutePlannerGlue::RoutePlannerGlue(const GlidePolar& polar,
                                   const Airspaces& master):
  terrain(NULL),
  m_planner(polar, SpeedVector(Angle::degrees(fixed_zero), fixed_zero), master)
{
}