traffic_info.c

Go to the documentation of this file.

/*
 * Copyright (C) Pascal Brisset, Antoine Drouin (2008), Kirk Scheper (2016)
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include "modules/multi/traffic_info.h"
 
#include "generated/airframe.h"     // AC_ID
#include "generated/flight_plan.h"  // NAV_MSL0
 
#include "modules/datalink/datalink.h"
#include "modules/datalink/telemetry.h"
#include "pprzlink/dl_protocol.h"   // datalink messages
#include "pprzlink/messages.h"      // telemetry messages
 
#include "state.h"
#include "math/pprz_geodetic_utm.h"
#include "math/pprz_geodetic_wgs84.h"
 
#if TRAFFIC_INFO_USE_LOG
#include "modules/loggers/logger_utils.h"
#if !USE_CHIBIOS_RTOS
static FILE* pprzLogFile = NULL;
 
/* Set the default log path to bebop storage */
#ifndef TRAFFIC_INFO_FILE_PATH
#define TRAFFIC_INFO_FILE_PATH /data/ftp/internal_000/acinfo
#endif
 
#endif // !USE_CHIBIOS_RTOS
#endif // TRAFFIC_INFO_USE_LOG
 
/* number of ac being tracked */
uint8_t ti_acs_idx;
/* index of ac in the list of traffic info aircraft (ti_acs) */
uint8_t ti_acs_id[NB_ACS_ID];
/* container for available traffic info */
struct acInfo ti_acs[NB_ACS];
 
/* Geoid height (msl) over ellipsoid [mm] */
int32_t geoid_height;
 
/* Send ACINFO_LLA message from the datalink class */
static void send_acinfo_lla(struct transport_tx *trans, struct link_device *dev)
{
  int16_t course = (int16_t)DeciDegOfRad(stateGetHorizontalSpeedDir_f());
  struct LlaCoor_i* lla = stateGetPositionLla_i();
  uint32_t itow = gps.tow;
  uint16_t speed = (uint16_t)(stateGetHorizontalSpeedNorm_f()*10.f);
  int16_t climb = (int16_t)(stateGetSpeedEnu_f()->z*10.f);
  int32_t alt = (int32_t)(lla->alt/10);
  uint8_t ac_id = AC_ID;
 
  // broadcast GPS message
  struct pprzlink_msg msg;
  msg.trans = trans;
  msg.dev = dev;
  msg.sender_id = AC_ID;
  msg.receiver_id = PPRZLINK_MSG_BROADCAST;
  msg.component_id = 0;
  pprzlink_msg_send_ACINFO_LLA(&msg,
      &course,
      &lla->lat, &lla->lon, &alt,
      &itow, &speed, &climb, &ac_id);
 
#if TRAFFIC_INFO_USE_LOG
  struct LlaCoor_f* lla_f = stateGetPositionLla_f();
  struct EnuCoor_f* enu_f = stateGetPositionEnu_f();
  if (LogFileIsOpen(pprzLogFile)) {
    LogWrite(pprzLogFile, "S,%d,%d,%.7f,%.7f,%.3f,%.3f,%.3f,%.3f,%d,0\n", // 0 at the end to have same length than receive log lines
        msg.sender_id, msg.receiver_id,
        DegOfRad(lla_f->lat), DegOfRad(lla_f->lon), lla_f->alt,
        enu_f->x, enu_f->y, enu_f->z,
        itow);
  }
#endif
}
 
void traffic_info_init(void)
{
  memset(ti_acs_id, 0, NB_ACS_ID);
 
  ti_acs_id[0] = 0;  // ground station
  ti_acs_id[AC_ID] = 1;
  ti_acs[ti_acs_id[AC_ID]].ac_id = AC_ID;
  ti_acs_idx = 2;
 
  geoid_height = NAV_MSL0;
 
#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACINFO_LLA, send_acinfo_lla);
#endif
}
 
static void update_geoid_height(void) {
  if(bit_is_set(gps.valid_fields, GPS_VALID_HMSL_BIT) && bit_is_set(gps.valid_fields, GPS_VALID_POS_LLA_BIT))
  {
    geoid_height = gps.lla_pos.alt - gps.hmsl;
  } else if(bit_is_set(gps.valid_fields, GPS_VALID_POS_LLA_BIT))
  {
    geoid_height = wgs84_ellipsoid_to_geoid_i(gps.lla_pos.lat, gps.lla_pos.lon);
  } /* default is just keep last available height */
}
 
bool parse_acinfo_dl(uint8_t *buf)
{
  uint8_t sender_id = SenderIdOfPprzMsg(buf);
  uint8_t msg_id = IdOfPprzMsg(buf);
  uint8_t class_id = pprzlink_get_msg_class_id(buf);
  uint32_t itow = 0;
 
  /* handle telemetry message */
#if PPRZLINK_DEFAULT_VER == 2
  if (class_id == DL_telemetry_CLASS_ID) {
#else
  if (sender_id > 0) {
#endif
    switch (msg_id) {
      case DL_GPS_SMALL: {
        uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(buf);
 
        // decode compressed values
        int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
        if (course & 0x400) {
          course |= 0xF800;  // fix for twos complements
        }
        course *= 2; // scale course by resolution
        int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
        if (gspeed & 0x400) {
          gspeed |= 0xF800;  // fix for twos complements
        }
        int16_t climb = (int16_t)(multiplex_speed & 0x3FF); // bits 9-0 z climb speed in cm/s
        if (climb & 0x200) {
          climb |= 0xFC00;  // fix for twos complements
        }
        itow = gps_tow_from_sys_ticks(sys_time.nb_tick);
 
        set_ac_info_lla(sender_id,
                        DL_GPS_SMALL_lat(buf),
                        DL_GPS_SMALL_lon(buf),
                        (int32_t)DL_GPS_SMALL_alt(buf) * 10,
                        course,
                        gspeed,
                        climb,
                        itow);
      }
      break;
      case DL_GPS: {
        itow = DL_GPS_itow(buf);
        set_ac_info_utm(sender_id,
                    DL_GPS_utm_east(buf),
                    DL_GPS_utm_north(buf),
                    DL_GPS_alt(buf),
                    DL_GPS_utm_zone(buf),
                    DL_GPS_course(buf),
                    DL_GPS_speed(buf),
                    DL_GPS_climb(buf),
                    itow);
      }
      break;
      case DL_GPS_LLA: {
        itow = DL_GPS_LLA_itow(buf);
        set_ac_info_lla(sender_id,
                        DL_GPS_LLA_lat(buf),
                        DL_GPS_LLA_lon(buf),
                        DL_GPS_LLA_alt(buf),
                        DL_GPS_LLA_course(buf),
                        DL_GPS_LLA_speed(buf),
                        DL_GPS_LLA_climb(buf),
                        itow);
      }
      break;
      case DL_GPS_INT: {
        struct LtpDef_i def = {0};
        struct EcefCoor_i ecef_pos = {
          .x = DL_GPS_INT_ecef_x(buf),
          .y = DL_GPS_INT_ecef_y(buf),
          .z = DL_GPS_INT_ecef_z(buf),
        };
        struct EcefCoor_i ecef_vel = {
          .x = DL_GPS_INT_ecef_xd(buf),
          .y = DL_GPS_INT_ecef_yd(buf),
          .z = DL_GPS_INT_ecef_zd(buf),
        };
        struct NedCoor_i ned_vel;
        ltp_def_from_ecef_i(&def, &ecef_pos);
        ned_of_ecef_vect_i(&ned_vel, &def, &ecef_vel);
        struct NedCoor_f ned_vel_f;
        VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel);
        int16_t course = (int16_t)(10.f * DegOfRad(atan2f(ned_vel_f.y, ned_vel_f.x))); // decideg
        uint16_t gspeed = (uint16_t)(CM_OF_M(FLOAT_VECT2_NORM(ned_vel_f)));
        int16_t climb = (int16_t)(CM_OF_M(-ned_vel_f.z));
        itow = DL_GPS_INT_tow(buf);
        set_ac_info_lla(sender_id,
                        DL_GPS_INT_lat(buf),
                        DL_GPS_INT_lon(buf),
                        DL_GPS_INT_alt(buf),
                        course,
                        gspeed,
                        climb,
                        itow);
      }
      break;
      default:
        return FALSE;
    }
  /* handle datalink message */
  } else if (class_id == DL_datalink_CLASS_ID) {
    switch (msg_id) {
      case DL_ACINFO: {
        sender_id = DL_ACINFO_ac_id(buf); // may overwrite GCS id
        itow = DL_ACINFO_itow(buf);
        set_ac_info_utm(sender_id,
                        DL_ACINFO_utm_east(buf),
                        DL_ACINFO_utm_north(buf),
                        DL_ACINFO_alt(buf) * 10,
                        DL_ACINFO_utm_zone(buf),
                        DL_ACINFO_course(buf),
                        DL_ACINFO_speed(buf),
                        DL_ACINFO_climb(buf),
                        itow);
      }
      break;
      case DL_ACINFO_LLA: {
        sender_id = DL_ACINFO_LLA_ac_id(buf); // may overwrite GCS id
        itow = DL_ACINFO_LLA_itow(buf);
        set_ac_info_lla(sender_id,
                  DL_ACINFO_LLA_lat(buf),
                  DL_ACINFO_LLA_lon(buf),
                  DL_ACINFO_LLA_alt(buf) * 10,
                  DL_ACINFO_LLA_course(buf),
                  DL_ACINFO_LLA_speed(buf),
                  DL_ACINFO_LLA_climb(buf),
                  itow);
      }
      break;
      default:
        return FALSE;
    }
  } else {
    return FALSE; // unsupported class
  }
#if TRAFFIC_INFO_USE_LOG
  struct LlaCoor_f* lla_f = acInfoGetPositionLla_f(sender_id);
  struct EnuCoor_f* enu_f = acInfoGetPositionEnu_f(sender_id);
  if (LogFileIsOpen(pprzLogFile)) {
    LogWrite(pprzLogFile, "R,%d,%d,%.7f,%.7f,%.3f,%.3f,%.3f,%.3f,%d,%d\n",
        sender_id, AC_ID,
        DegOfRad(lla_f->lat), DegOfRad(lla_f->lon), lla_f->alt,
        enu_f->x, enu_f->y, enu_f->z,
        itow, gps.tow);
  }
#endif
  return TRUE;
}
 
 
void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone, uint16_t course,
                 uint16_t gspeed, uint16_t climb, uint32_t itow)
{
  if (ti_acs_idx < NB_ACS) {
    if (id > 0 && ti_acs_id[id] == 0) {    // new aircraft id
      ti_acs_id[id] = ti_acs_idx++;
      ti_acs[ti_acs_id[id]].ac_id = id;
    }
 
    if (itow < ti_acs[ti_acs_id[id]].itow) {
      return; // don't update on old data
    }
 
    ti_acs[ti_acs_id[id]].status = 0;
 
    uint16_t my_zone = stateGetUtmOrigin_f()->zone;
    if (utm_zone == my_zone) {
      ti_acs[ti_acs_id[id]].utm_pos_i.east = utm_east;
      ti_acs[ti_acs_id[id]].utm_pos_i.north = utm_north;
      ti_acs[ti_acs_id[id]].utm_pos_i.alt = alt;
      ti_acs[ti_acs_id[id]].utm_pos_i.zone = utm_zone;
      SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_UTM_I);
 
    } else { // store other uav in utm extended zone
      struct UtmCoor_i utm = {.east = utm_east, .north = utm_north, .alt = alt, .zone = utm_zone};
      struct LlaCoor_i lla;
      lla_of_utm_i(&lla, &utm);
      LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
      SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_LLA_I);
 
      utm.zone = my_zone;
      utm_of_lla_i(&utm, &lla);
 
      UTM_COPY(ti_acs[ti_acs_id[id]].utm_pos_i, utm);
      SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_UTM_I);
    }
 
    ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
    ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
    ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
    SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_VEL_LOCAL_F);
 
    ti_acs[ti_acs_id[id]].itow = itow;
  }
}
 
void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
                     int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow)
{
  if (ti_acs_idx < NB_ACS) {
    if (id > 0 && ti_acs_id[id] == 0) {
      ti_acs_id[id] = ti_acs_idx++;
      ti_acs[ti_acs_id[id]].ac_id = id;
    }
 
    if (itow < ti_acs[ti_acs_id[id]].itow) {
      return; // don't update on old data
    }
 
    ti_acs[ti_acs_id[id]].status = 0;
 
    struct LlaCoor_i lla = {.lat = lat, .lon = lon, .alt = alt};
    LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
    SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_LLA_I);
 
    ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
    ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
    ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
    SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_VEL_LOCAL_F);
 
    ti_acs[ti_acs_id[id]].itow = itow;
  }
}
 
/* Conversion funcitons for computing ac position in requested reference frame from
 * any other available reference frame
 */
 
/* compute UTM position of aircraft with ac_id (int) */
void acInfoCalcPositionUtm_i(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
  {
    return;
  }
 
  /* LLA_i -> UTM_i is more accurate than from UTM_f */
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
  {
    // use my zone as reference, i.e zone extend
    ti_acs[ac_nr].utm_pos_i.zone = stateGetUtmOrigin_f()->zone;
    utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
    update_geoid_height();
    ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
  {
    UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
  {
    // use my zone as reference, i.e zone extend
    ti_acs[ac_nr].utm_pos_i.zone = stateGetUtmOrigin_f()->zone;
    utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
    update_geoid_height();
    ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
    SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
    UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
}
 
/* compute LLA position of aircraft with ac_id (int) */
void acInfoCalcPositionLla_i(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
  {
    LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
  {
    lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
    update_geoid_height();
    ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
  {
    lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
    update_geoid_height();
    ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
    SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
    LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
}
 
/* compute ENU position of aircraft with ac_id (int) */
void acInfoCalcPositionEnu_i(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
  {
    ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f);
  }
  else if (state.ned_initialized_i)
  {
    if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
    {
      struct EnuCoor_i enu;
      enu_of_lla_point_i(&enu, stateGetNedOrigin_i(), acInfoGetPositionLla_i(ac_id));
      // convert ENU pos from cm to BFP with INT32_POS_FRAC
      enu.x = POS_BFP_OF_REAL(enu.x) / 100;
      enu.y = POS_BFP_OF_REAL(enu.y) / 100;
      enu.z = POS_BFP_OF_REAL(enu.z) / 100;
      ti_acs[ac_nr].enu_pos_i = enu;
    } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
    {
      enu_of_lla_point_f(&ti_acs[ac_nr].enu_pos_f, stateGetNedOrigin_f(), acInfoGetPositionLla_f(ac_id));
      SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
      ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f)
    }
  } else if (state.utm_initialized_f)
  {
    /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
    ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), *stateGetUtmOrigin_f());
    SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
    ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f);
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
}
 
/* compute UTM position of aircraft with ac_id (float) */
void acInfoCalcPositionUtm_f(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
  {
    UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
  {
    // use my zone as reference, i.e zone extend
    ti_acs[ac_nr].utm_pos_i.zone = stateGetUtmOrigin_f()->zone;
    utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
    update_geoid_height();
    ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
    SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
    UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
  {
    /* not very accurate with float ~5cm */
    ti_acs[ac_nr].utm_pos_f.zone = stateGetUtmOrigin_f()->zone;
    utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
    update_geoid_height();
    ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
}
 
/* compute LLA position of aircraft with ac_id (float) */
void acInfoCalcPositionLla_f(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
  {
    LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
  {
    lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
    update_geoid_height();
    ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
    SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
    LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
  {
    lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
    update_geoid_height();
    ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
}
 
/* compute ENU position of aircraft with ac_id (float) */
void acInfoCalcPositionEnu_f(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
  {
    ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
  }
  else if (state.ned_initialized_i)
  {
    if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
    {
      enu_of_lla_point_f(&ti_acs[ac_nr].enu_pos_f, stateGetNedOrigin_f(), acInfoGetPositionLla_f(ac_id));
    } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
    {
      struct EnuCoor_i enu;
      enu_of_lla_point_i(&enu, stateGetNedOrigin_i(), acInfoGetPositionLla_i(ac_id));
      // convert ENU pos from cm to BFP with INT32_POS_FRAC
      enu.x = POS_BFP_OF_REAL(enu.x) / 100;
      enu.y = POS_BFP_OF_REAL(enu.y) / 100;
      enu.z = POS_BFP_OF_REAL(enu.z) / 100;
      ti_acs[ac_nr].enu_pos_i = enu;
      SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
      ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
    }
  } else if (state.utm_initialized_f)
  {
    /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
    ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), *stateGetUtmOrigin_f());
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
}
 
/* compute ENU velocity of aircraft with ac_id (int) */
void acInfoCalcVelocityEnu_i(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
  {
    SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
  } else {
    ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
    ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
    ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
    SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
    SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I);
}
 
/* compute ENU position of aircraft with ac_id (float) */
void acInfoCalcVelocityEnu_f(uint8_t ac_id)
{
  uint8_t ac_nr = ti_acs_id[ac_id];
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
  {
    return;
  }
 
  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
  {
    SPEEDS_FLOAT_OF_BFP(ti_acs[ac_nr].enu_vel_f, ti_acs[ac_nr].enu_vel_i);
  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_LOCAL_F)) {
    ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
    ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
    ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
  }
  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
}
 
void traffic_info_log_start(void)
{
#if TRAFFIC_INFO_USE_LOG
  LogOpen(pprzLogFile, STRINGIFY(TRAFFIC_INFO_FILE_PATH), NULL);
#endif
}
 
void traffic_info_log_stop(void)
{
#if TRAFFIC_INFO_USE_LOG
  if (LogFileIsOpen(pprzLogFile)) {
    LogClose(pprzLogFile);
  }
#endif
}