src/FLARM/FlarmComputer.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 "FLARM/FlarmComputer.hpp"
  25 #include "FLARM/FlarmDetails.hpp"
  26 #include "NMEA/Info.hpp"
  27 #include "Geo/GeoVector.hpp"
  28
  29 void
  30 FlarmComputer::Process(FlarmData &flarm, const FlarmData &last_flarm,
  31                        const NMEAInfo &basic)
  32 {
  33   // Cleanup old calculation instances
  34   if (basic.time_available)
  35     flarm_calculations.CleanUp(basic.time);
  36
  37   // if (FLARM data is available)
  38   if (!flarm.IsDetected())
  39     return;
  40
  41   fixed north_to_latitude(0);
  42   fixed east_to_longitude(0);
  43
  44   if (basic.location_available) {
  45     // Precalculate relative east and north projection to lat/lon
  46     // for Location calculations of each target
  47     constexpr Angle delta_lat = Angle::Degrees(0.01);
  48     constexpr Angle delta_lon = Angle::Degrees(0.01);
  49
  50     GeoPoint plat = basic.location;
  51     plat.latitude += delta_lat;
  52     GeoPoint plon = basic.location;
  53     plon.longitude += delta_lon;
  54
  55     fixed dlat = basic.location.Distance(plat);
  56     fixed dlon = basic.location.Distance(plon);
  57
  58     if (positive(fabs(dlat)) && positive(fabs(dlon))) {
  59       north_to_latitude = delta_lat.Degrees() / dlat;
  60       east_to_longitude = delta_lon.Degrees() / dlon;
  61     }
  62   }
  63
  64   // for each item in traffic
  65   for (auto &traffic : flarm.traffic.list) {
  66     // if we don't know the target's name yet
  67     if (!traffic.HasName()) {
  68       // lookup the name of this target's id
  69       const TCHAR *fname = FlarmDetails::LookupCallsign(traffic.id);
  70       if (fname != NULL)
  71         traffic.name = fname;
  72     }
  73
  74     // Calculate distance
  75     traffic.distance = SmallHypot(traffic.relative_north,
  76                                   traffic.relative_east);
  77
  78     // Calculate Location
  79     traffic.location_available = basic.location_available;
  80     if (traffic.location_available) {
  81       traffic.location.latitude =
  82           Angle::Degrees(traffic.relative_north * north_to_latitude) +
  83           basic.location.latitude;
  84
  85       traffic.location.longitude =
  86           Angle::Degrees(traffic.relative_east * east_to_longitude) +
  87           basic.location.longitude;
  88     }
  89
  90     // Calculate absolute altitude
  91     traffic.altitude_available = basic.gps_altitude_available;
  92     if (traffic.altitude_available)
  93       traffic.altitude = traffic.relative_altitude + RoughAltitude(basic.gps_altitude);
  94
  95     // Calculate average climb rate
  96     traffic.climb_rate_avg30s_available = traffic.altitude_available;
  97     if (traffic.climb_rate_avg30s_available)
  98       traffic.climb_rate_avg30s =
  99         flarm_calculations.Average30s(traffic.id, basic.time, traffic.altitude);
 100
 101     // The following calculations are only relevant for targets
 102     // where information is missing
 103     if (traffic.track_received && traffic.turn_rate_received &&
 104         traffic.speed_received && traffic.climb_rate_received)
 105       continue;
 106
 107     // Check if the target has been seen before in the last seconds
 108     const FlarmTraffic *last_traffic =
 109       last_flarm.traffic.FindTraffic(traffic.id);
 110     if (last_traffic == NULL || !last_traffic->valid)
 111       continue;
 112
 113     // Calculate the time difference between now and the last contact
 114     fixed dt = traffic.valid.GetTimeDifference(last_traffic->valid);
 115     if (positive(dt)) {
 116       // Calculate the immediate climb rate
 117       if (!traffic.climb_rate_received)
 118         traffic.climb_rate =
 119           (traffic.relative_altitude - last_traffic->relative_altitude) / dt;
 120     } else {
 121       // Since the time difference is zero (or negative)
 122       // we can just copy the old values
 123       if (!traffic.climb_rate_received)
 124         traffic.climb_rate = last_traffic->climb_rate;
 125     }
 126
 127     if (positive(dt) &&
 128         traffic.location_available &&
 129         last_traffic->location_available) {
 130       // Calculate the GeoVector between now and the last contact
 131       GeoVector vec = last_traffic->location.DistanceBearing(traffic.location);
 132
 133       if (!traffic.track_received)
 134         traffic.track = vec.bearing;
 135
 136       // Calculate the turn rate
 137       if (!traffic.turn_rate_received) {
 138         Angle turn_rate = traffic.track - last_traffic->track;
 139         traffic.turn_rate =
 140           turn_rate.AsDelta().Degrees() / dt;
 141       }
 142
 143       // Calculate the speed [m/s]
 144       if (!traffic.speed_received)
 145         traffic.speed = vec.distance / dt;
 146     } else {
 147       // Since the time difference is zero (or negative)
 148       // we can just copy the old values
 149       if (!traffic.track_received)
 150         traffic.track = last_traffic->track;
 151
 152       if (!traffic.turn_rate_received)
 153         traffic.turn_rate = last_traffic->turn_rate;
 154
 155       if (!traffic.speed_received)
 156         traffic.speed = last_traffic->speed;
 157     }
 158   }
 159 }