static void
ComputeFlarmTeam(const GeoPoint &location, const GeoPoint &reference_location,
const TrafficList &traffic_list, const FlarmId target_id,
TeamInfo &teamcode_info)
{
const FlarmTraffic *traffic = traffic_list.FindTraffic(target_id);
if (traffic == NULL || !traffic->location_available) {
teamcode_info.flarm_teammate_code_current = false;
return;
}
// Set Teammate location to FLARM contact location
teamcode_info.teammate_location = traffic->location;
teamcode_info.teammate_vector = location.DistanceBearing(traffic->location);
teamcode_info.teammate_available = true;
// Calculate distance and bearing from teammate to reference waypoint
GeoVector v = reference_location.DistanceBearing(traffic->location);
// Calculate TeamCode and save it in Calculated
teamcode_info.flarm_teammate_code.Update(v.bearing, v.distance);
teamcode_info.flarm_teammate_code_available = true;
teamcode_info.flarm_teammate_code_current = true;
}
/**
* Adds data from the specified object, unless already present in
* this one.
*/
void Complement(const TrafficList &add) {
if (IsEmpty() && !add.IsEmpty())
*this = add;
}
void SetTarget(const FlarmId &id) {
SetTarget(data.FindTraffic(id));
}
void SetTarget(const FlarmTraffic *traffic) {
SetTarget(traffic != NULL ? (int)data.TrafficIndex(traffic) : -1);
}
void Expire(fixed clock) {
error.Expire(clock);
version.Expire(clock);
status.Expire(clock);
traffic.Expire(clock);
}
void Complement(const FlarmData &add) {
error.Complement(add.error);
version.Complement(add.version);
status.Complement(add.status);
traffic.Complement(add.traffic);
}
void Clear() {
error.Clear();
version.Clear();
status.Clear();
traffic.Clear();
}
bool IsDetected() const {
return status.available || !traffic.IsEmpty();
}
void
ParsePFLAA(NMEAInputLine &line, TrafficList &flarm, double clock)
{
flarm.modified.Update(clock);
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
FlarmTraffic traffic;
traffic.alarm_level = (FlarmTraffic::AlarmType)
line.Read((int)FlarmTraffic::AlarmType::NONE);
double value;
bool stealth = false;
if (!line.ReadChecked(value))
// Relative North is required !
return;
traffic.relative_north = value;
if (!line.ReadChecked(value))
// Relative East is required !
return;
traffic.relative_east = value;
if (!line.ReadChecked(value))
// Relative Altitude is required !
return;
traffic.relative_altitude = value;
line.Skip(); /* id type */
// 5 id, 6 digit hex
char id_string[16];
line.Read(id_string, 16);
traffic.id = FlarmId::Parse(id_string, nullptr);
Angle track;
traffic.track_received = ReadBearing(line, track);
if (!traffic.track_received) {
// Field is empty in stealth mode
stealth = true;
traffic.track = Angle::Zero();
} else
traffic.track = track;
traffic.turn_rate_received = line.ReadChecked(value);
if (!traffic.turn_rate_received) {
// Field is empty in stealth mode
traffic.turn_rate = 0;
} else
traffic.turn_rate = value;
traffic.speed_received = line.ReadChecked(value);
if (!traffic.speed_received) {
// Field is empty in stealth mode
stealth = true;
traffic.speed = 0;
} else
traffic.speed = value;
traffic.climb_rate_received = line.ReadChecked(value);
if (!traffic.climb_rate_received) {
// Field is empty in stealth mode
stealth = true;
traffic.climb_rate = 0;
} else
traffic.climb_rate = value;
traffic.stealth = stealth;
unsigned type = line.Read(0);
if (type > 15 || type == 14)
traffic.type = FlarmTraffic::AircraftType::UNKNOWN;
else
traffic.type = (FlarmTraffic::AircraftType)type;
FlarmTraffic *flarm_slot = flarm.FindTraffic(traffic.id);
if (flarm_slot == nullptr) {
flarm_slot = flarm.AllocateTraffic();
if (flarm_slot == nullptr)
// no more slots available
return;
flarm_slot->Clear();
flarm_slot->id = traffic.id;
flarm.new_traffic.Update(clock);
}
// set time of fix to current time
flarm_slot->valid.Update(clock);
flarm_slot->Update(traffic);
}
