// $PDVDS,nx,nz,flap,stallratio,netto
static bool
PDVDS(NMEAInputLine &line, NMEAInfo &info)
{
fixed AccelX = line.Read(fixed_zero);
fixed AccelZ = line.Read(fixed_zero);
fixed mag = SmallHypot(AccelX, AccelZ);
info.acceleration.ProvideGLoad(fixed(mag) / 100, true);
/*
double flap = line.Read(0.0);
*/
line.Skip();
info.stall_ratio = line.Read(fixed_zero);
info.stall_ratio_available.Update(info.clock);
fixed value;
if (line.ReadChecked(value))
info.ProvideNettoVario(value / 10);
//hasVega = true;
return true;
}
gcc_pure
static fixed
SimpleSquareDistance(const GeoPoint &a, const GeoPoint &b,
const fixed latitude_cos)
{
return SmallHypot((a.longitude - b.longitude).AsDelta().Native(),
(a.latitude - b.latitude).AsDelta().Native() * latitude_cos);
}
/**
* Attempt to compute airspeed when it is not yet available from:
* 1) dynamic pressure and air density derived from some altitude.
* 2) pitot pressure and static pressure.
* 3) ground speed and wind.
*/
static void
ComputeAirspeed(NMEAInfo &basic, const DerivedInfo &calculated)
{
if (basic.airspeed_available && basic.airspeed_real)
/* got it already */
return;
const auto any_altitude = basic.GetAnyAltitude();
if (!basic.airspeed_available && any_altitude.first) {
fixed dyn; bool available = false;
if (basic.dyn_pressure_available) {
dyn = basic.dyn_pressure.GetHectoPascal();
available = true;
} else if (basic.pitot_pressure_available && basic.static_pressure_available) {
dyn = basic.pitot_pressure.GetHectoPascal() - basic.static_pressure.GetHectoPascal();
available = true;
}
if (available) {
basic.indicated_airspeed = sqrt(fixed(163.2653061) * dyn);
basic.true_airspeed = basic.indicated_airspeed *
AirDensityRatio(any_altitude.second);
basic.airspeed_available.Update(basic.clock);
basic.airspeed_real = true; // Anyway not less real then any other method.
return;
}
}
if (!basic.ground_speed_available || !calculated.wind_available ||
!calculated.flight.flying) {
/* impossible to calculate */
basic.airspeed_available.Clear();
return;
}
fixed TrueAirspeedEstimated = fixed(0);
const SpeedVector wind = calculated.wind;
if (positive(basic.ground_speed) || wind.IsNonZero()) {
fixed x0 = basic.track.fastsine() * basic.ground_speed;
fixed y0 = basic.track.fastcosine() * basic.ground_speed;
x0 += wind.bearing.fastsine() * wind.norm;
y0 += wind.bearing.fastcosine() * wind.norm;
TrueAirspeedEstimated = SmallHypot(x0, y0);
}
basic.true_airspeed = TrueAirspeedEstimated;
basic.indicated_airspeed = TrueAirspeedEstimated;
if (any_altitude.first)
basic.indicated_airspeed /= AirDensityRatio(any_altitude.second);
basic.airspeed_available.Update(basic.clock);
basic.airspeed_real = false;
}
// $PDVDS,nx,nz,flap,stallratio,netto
static bool
PDVDS(NMEAInputLine &line, NMEAInfo &info)
{
const int accel_x = line.Read(0), accel_z = line.Read(0);
fixed mag = SmallHypot(fixed(accel_x), fixed(accel_z));
info.acceleration.ProvideGLoad(mag / 100, true);
/*
double flap = line.Read(0.0);
*/
line.Skip();
info.stall_ratio = line.Read(fixed(0));
info.stall_ratio_available.Update(info.clock);
int value;
if (line.ReadChecked(value))
info.ProvideNettoVario(fixed(value) / 10);
//hasVega = true;
return true;
}
/**
* Constructor given two magnitudes (east and north)
*
* @param x East speed
* @param y North speed
* @return Initialised object
*/
SpeedVector(fixed x, fixed y)
:bearing(Angle::FromXY(y,x).AsBearing()), norm(SmallHypot(x, y)) {}
void
FlarmComputer::Process(FlarmData &flarm, const FlarmData &last_flarm,
const NMEAInfo &basic)
{
// Cleanup old calculation instances
if (basic.time_available)
flarm_calculations.CleanUp(basic.time);
// if (FLARM data is available)
if (!flarm.IsDetected())
return;
fixed north_to_latitude(0);
fixed east_to_longitude(0);
if (basic.location_available) {
// Precalculate relative east and north projection to lat/lon
// for Location calculations of each target
constexpr Angle delta_lat = Angle::Degrees(0.01);
constexpr Angle delta_lon = Angle::Degrees(0.01);
GeoPoint plat = basic.location;
plat.latitude += delta_lat;
GeoPoint plon = basic.location;
plon.longitude += delta_lon;
fixed dlat = basic.location.Distance(plat);
fixed dlon = basic.location.Distance(plon);
if (positive(fabs(dlat)) && positive(fabs(dlon))) {
north_to_latitude = delta_lat.Degrees() / dlat;
east_to_longitude = delta_lon.Degrees() / dlon;
}
}
// for each item in traffic
for (auto &traffic : flarm.traffic.list) {
// if we don't know the target's name yet
if (!traffic.HasName()) {
// lookup the name of this target's id
const TCHAR *fname = FlarmDetails::LookupCallsign(traffic.id);
if (fname != NULL)
traffic.name = fname;
}
// Calculate distance
traffic.distance = SmallHypot(traffic.relative_north,
traffic.relative_east);
// Calculate Location
traffic.location_available = basic.location_available;
if (traffic.location_available) {
traffic.location.latitude =
Angle::Degrees(traffic.relative_north * north_to_latitude) +
basic.location.latitude;
traffic.location.longitude =
Angle::Degrees(traffic.relative_east * east_to_longitude) +
basic.location.longitude;
}
// Calculate absolute altitude
traffic.altitude_available = basic.gps_altitude_available;
if (traffic.altitude_available)
traffic.altitude = traffic.relative_altitude + RoughAltitude(basic.gps_altitude);
// Calculate average climb rate
traffic.climb_rate_avg30s_available = traffic.altitude_available;
if (traffic.climb_rate_avg30s_available)
traffic.climb_rate_avg30s =
flarm_calculations.Average30s(traffic.id, basic.time, traffic.altitude);
// The following calculations are only relevant for targets
// where information is missing
if (traffic.track_received && traffic.turn_rate_received &&
traffic.speed_received && traffic.climb_rate_received)
continue;
// Check if the target has been seen before in the last seconds
const FlarmTraffic *last_traffic =
last_flarm.traffic.FindTraffic(traffic.id);
if (last_traffic == NULL || !last_traffic->valid)
continue;
// Calculate the time difference between now and the last contact
fixed dt = traffic.valid.GetTimeDifference(last_traffic->valid);
if (positive(dt)) {
// Calculate the immediate climb rate
if (!traffic.climb_rate_received)
traffic.climb_rate =
(traffic.relative_altitude - last_traffic->relative_altitude) / dt;
} else {
// Since the time difference is zero (or negative)
// we can just copy the old values
if (!traffic.climb_rate_received)
traffic.climb_rate = last_traffic->climb_rate;
}
if (positive(dt) &&
traffic.location_available &&
last_traffic->location_available) {
// Calculate the GeoVector between now and the last contact
GeoVector vec = last_traffic->location.DistanceBearing(traffic.location);
if (!traffic.track_received)
traffic.track = vec.bearing;
// Calculate the turn rate
if (!traffic.turn_rate_received) {
Angle turn_rate = traffic.track - last_traffic->track;
traffic.turn_rate =
turn_rate.AsDelta().Degrees() / dt;
}
// Calculate the speed [m/s]
if (!traffic.speed_received)
traffic.speed = vec.distance / dt;
} else {
// Since the time difference is zero (or negative)
// we can just copy the old values
if (!traffic.track_received)
traffic.track = last_traffic->track;
if (!traffic.turn_rate_received)
traffic.turn_rate = last_traffic->turn_rate;
if (!traffic.speed_received)
traffic.speed = last_traffic->speed;
}
}
}
