test/src/RunWindEKF.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #include "Computer/Settings.hpp"
  25 #include "Computer/CirclingComputer.hpp"
  26 #include "Computer/Wind/WindEKFGlue.hpp"
  27 #include "Formatter/TimeFormatter.hpp"
  28 #include "OS/Args.hpp"
  29 #include "DebugReplay.hpp"
  30
  31 #include <stdio.h>
  32
  33 int main(int argc, char **argv)
  34 {
  35   Args args(argc, argv, "DRIVER FILE");
  36   DebugReplay *replay = CreateDebugReplay(args);
  37   if (replay == NULL)
  38     return EXIT_FAILURE;
  39
  40   args.ExpectEnd();
  41
  42   printf("# time wind_bearing (deg) wind_speed (m/s) grndspeed (m/s) tas (m/s) bearing (deg)\n");
  43
  44   CirclingSettings circling_settings;
  45   circling_settings.SetDefaults();
  46
  47   CirclingComputer circling_computer;
  48   circling_computer.Reset();
  49
  50   WindEKFGlue wind_ekf;
  51   wind_ekf.Reset();
  52
  53   while (replay->Next()) {
  54     const MoreData &data = replay->Basic();
  55     const DerivedInfo &calculated = replay->Calculated();
  56
  57     circling_computer.TurnRate(replay->SetCalculated(),
  58                                data, calculated.flight);
  59
  60     WindEKFGlue::Result result =
  61       wind_ekf.Update(data, replay->Calculated());
  62     if (result.quality > 0) {
  63       TCHAR time_buffer[32];
  64       FormatTime(time_buffer, data.time);
  65
  66       _tprintf(_T("%s %d %g %g %g %d\n"), time_buffer,
  67                (int)result.wind.bearing.Degrees(),
  68                (double)result.wind.norm,
  69                (double)data.ground_speed,
  70                (double)data.true_airspeed,
  71                (int)data.track.Degrees());
  72     }
  73   }
  74
  75   delete replay;
  76 }