diff --git a/conf/messages.xml b/conf/messages.xml
index f073969cc2..da9cda5111 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -856,6 +856,7 @@
+
diff --git a/diff.diff b/diff.diff
new file mode 100644
index 0000000000..5de33f7296
--- /dev/null
+++ b/diff.diff
@@ -0,0 +1,2699 @@
+diff --git a/conf/abi.xml b/conf/abi.xml
+index ac16c25..be5739d 100644
+--- a/conf/abi.xml
++++ b/conf/abi.xml
+@@ -72,6 +72,12 @@
+
+
+
++
++
++
++
++
++
+
+
+
+diff --git a/conf/airframes/BR/conf.xml b/conf/airframes/BR/conf.xml
+index 1612531..16019fd 100644
+--- a/conf/airframes/BR/conf.xml
++++ b/conf/airframes/BR/conf.xml
+@@ -88,6 +88,17 @@
+ gui_color="blue"
+ />
+
++
+
+
+-
+-
+-
++
++
++
+
+
+
+@@ -161,13 +161,6 @@
+
+
+
+-
+-
+
+
+
+@@ -183,8 +176,6 @@
+
+
+
+-
+-
+
+
+
+@@ -210,17 +201,15 @@
+
+
+
+-
+-
+-
+-
++
+
+
+
+
+-
+-
++
++
+
++
+
+
+
+diff --git a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml
+index 99c55f2..298fd77 100644
+--- a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml
++++ b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml
+@@ -173,13 +173,6 @@
+
+
+
+-
+-
+
+
+
+diff --git a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml
+index b5d7e19..0732035 100644
+--- a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml
++++ b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml
+@@ -102,13 +102,6 @@
+
+
+
+-
+-
+
+
+
+diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
+index c0baaac..b24a80f 100644
+--- a/conf/firmwares/rotorcraft.makefile
++++ b/conf/firmwares/rotorcraft.makefile
+@@ -48,7 +48,10 @@ $(TARGET).CFLAGS += -DPERIPHERALS_AUTO_INIT
+ $(TARGET).srcs += mcu.c
+ $(TARGET).srcs += $(SRC_ARCH)/mcu_arch.c
+
+-# frequency of main periodic
++#
++# frequencies of main periodic
++#
++
+ PERIODIC_FREQUENCY ?= 512
+ $(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
+
+diff --git a/conf/firmwares/subsystems/fixedwing/navigation.makefile b/conf/firmwares/subsystems/fixedwing/navigation.makefile
+index 223c780..90285b3 100644
+--- a/conf/firmwares/subsystems/fixedwing/navigation.makefile
++++ b/conf/firmwares/subsystems/fixedwing/navigation.makefile
+@@ -7,6 +7,7 @@
+
+ $(TARGET).CFLAGS += -DNAV
+ $(TARGET).srcs += $(SRC_FIRMWARE)/nav.c
++#$(TARGET).srcs += subsystems/navigation/common_nav.c
+ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
+ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
+ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c
+diff --git a/conf/firmwares/subsystems/rotorcraft/navigation.makefile b/conf/firmwares/subsystems/rotorcraft/navigation.makefile
+index 49e29ab..9b712b7 100644
+--- a/conf/firmwares/subsystems/rotorcraft/navigation.makefile
++++ b/conf/firmwares/subsystems/rotorcraft/navigation.makefile
+@@ -2,5 +2,7 @@
+
+ $(TARGET).CFLAGS += -DUSE_NAVIGATION
+ $(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c
++#$(TARGET).srcs += subsystems/navigation/common_nav.c
+ $(TARGET).srcs += subsystems/navigation/waypoints.c
+ $(TARGET).srcs += subsystems/navigation/common_flight_plan.c
++$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
+diff --git a/conf/joystick/hobbyking.xml b/conf/joystick/hobbyking.xml
+index b778a15..6180870 100644
+--- a/conf/joystick/hobbyking.xml
++++ b/conf/joystick/hobbyking.xml
+@@ -38,7 +38,7 @@ The basis of steering is the standard signs of aerospace convention
+
+
+
+-
++
+
+
+
+@@ -47,4 +47,4 @@ The basis of steering is the standard signs of aerospace convention
+
+
+
+-
+\ No newline at end of file
++
+diff --git a/conf/messages.xml b/conf/messages.xml
+index 4ee38a0..179784a 100644
+--- a/conf/messages.xml
++++ b/conf/messages.xml
+@@ -856,7 +856,6 @@
+
+
+
+-
+
+
+
+@@ -2160,7 +2159,12 @@
+
+
+
+-
++
++ bits 31-21 course in decideg : bits 20-10 ground speed in cm/s : bits 9-0 climb speed in cm/s
++
++
++
++
+
+
+
+@@ -2479,8 +2483,9 @@
+
+
+
+- bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free
+- bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free
++ bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm
++ bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s
++
+
+
+
+diff --git a/conf/modules/swarm_potential.xml b/conf/modules/swarm_potential.xml
+new file mode 100644
+index 0000000..c3f8b22
+--- /dev/null
++++ b/conf/modules/swarm_potential.xml
+@@ -0,0 +1,17 @@
++
++
++
++
++
++ Potential fields collision avoidance.
++
++
++
++
++
++
++
++
++
+diff --git a/conf/telemetry/default_rotorcraft_slow.xml b/conf/telemetry/default_rotorcraft_slow.xml
+index 88a3995..efcf5a0 100644
+--- a/conf/telemetry/default_rotorcraft_slow.xml
++++ b/conf/telemetry/default_rotorcraft_slow.xml
+@@ -27,7 +27,8 @@
+
+
+
+-
++
++
+
+
+
+@@ -142,6 +143,15 @@
+ -->
+
+
++
++
++
++
++
++
++
++
++
+
+
+
+diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c
+index 39199eb..e99c70f 100644
+--- a/sw/airborne/arch/sim/sim_gps.c
++++ b/sw/airborne/arch/sim/sim_gps.c
+@@ -39,7 +39,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
+ //TODO set alt above ellipsoid, use hmsl for now
+ lla_f.alt = Double_val(a);
+ LLA_BFP_OF_REAL(gps.lla_pos, lla_f);
+- SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
++ SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
+
+ gps.utm_pos.east = Int_val(x);
+ gps.utm_pos.north = Int_val(y);
+diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
+index 248f62e..64be8ce 100644
+--- a/sw/airborne/firmwares/fixedwing/datalink.c
++++ b/sw/airborne/firmwares/fixedwing/datalink.c
+@@ -118,15 +118,15 @@ void dl_parse_msg(void)
+ #ifdef TRAFFIC_INFO
+ case DL_ACINFO: {
+ if (DL_ACINFO_ac_id(dl_buffer) == AC_ID) { break; }
+- uint8_t id = DL_ACINFO_ac_id(dl_buffer);
+- float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
+- float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
+- float a = MOfCm(DL_ACINFO_alt(dl_buffer));
+- float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer)) / 10.);
+- float s = MOfCm(DL_ACINFO_speed(dl_buffer));
+- float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
+- uint32_t t = DL_ACINFO_itow(dl_buffer);
+- SetAcInfo(id, ux, uy, c, a, s, cl, t);
++ uint8_t id = DL_ACINFO_ac_id(dl_buffer);
++ float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
++ float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
++ float a = MOfCm(DL_ACINFO_alt(dl_buffer));
++ float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer)) / 10.);
++ float s = MOfCm(DL_ACINFO_speed(dl_buffer));
++ float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
++ uint32_t t = DL_ACINFO_itow(dl_buffer);
++ SetAcInfo(id, ux, uy, c, a, s, cl, t);
+ }
+ break;
+ #endif
+diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
+index a9e505b..5d9eeee 100644
+--- a/sw/airborne/firmwares/rotorcraft/datalink.c
++++ b/sw/airborne/firmwares/rotorcraft/datalink.c
+@@ -49,6 +49,10 @@
+ #include "subsystems/gps/gps_datalink.h"
+ #endif
+
++#ifdef TRAFFIC_INFO
++#include "subsystems/navigation/traffic_info.h"
++#endif // TRAFFIC_INFO
++
+ #include "firmwares/rotorcraft/navigation.h"
+ #include "firmwares/rotorcraft/autopilot.h"
+
+@@ -56,7 +60,11 @@
+ #include "state.h"
+ #include "led.h"
+
++#define MOfCm(_x) (((float)(_x))/100.)
++#define MOfMM(_x) (((float)(_x))/1000.)
++
+ #define IdOfMsg(x) (x[1])
++#define SenderIdOfMsg(x) (x[0])
+
+ #if USE_NPS
+ bool_t datalink_enabled = TRUE;
+@@ -65,9 +73,62 @@ bool_t datalink_enabled = TRUE;
+ void dl_parse_msg(void)
+ {
+
++ uint8_t sender_id = SenderIdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(dl_buffer);
+- switch (msg_id) {
+
++ /* parse telemetry messages coming from other AC */
++ if (sender_id != 0) {
++ switch (msg_id) {
++#ifdef TRAFFIC_INFO
++ case DL_GPS_SMALL: {
++ uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
++
++ // Position in ENU coordinates
++ int16_t course = (int32_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
++ if (course & 0x400) {
++ course |= 0xFFFFF800; // fix for twos complements
++ }
++ int16_t gspeed = (int32_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
++ if (gspeed & 0x400) {
++ gspeed |= 0xFFFFF800; // fix for twos complements
++ }
++ int16_t climb = (int16_t)(multiplex_speed >> 2 & 0x7FF); // bits 9-0 z climb speed in cm/s
++ if (climb & 0x400) {
++ climb |= 0xFFFFF800; // fix for twos complements
++ }
++
++ SetAcInfo(sender_id,
++ MOfCm(DL_GPS_SMALL_utm_east(dl_buffer)), /*m*/
++ MOfCm(DL_GPS_SMALL_utm_north(dl_buffer)), /*m*/
++ RadOfDeg(((float)course) / 10.), /*rad(CW)*/
++ MOfCm(DL_GPS_SMALL_alt(dl_buffer)), /*m*/
++ MOfCm(gspeed), /*m/s*/
++ MOfCm(climb), /*m/s*/
++ gps_tow_from_sys_ticks(sys_time.nb_tick));
++ break;
++ }
++
++ case DL_GPS: {
++ SetAcInfo(sender_id,
++ MOfCm(DL_GPS_utm_east(dl_buffer)), /*m*/
++ MOfCm(DL_GPS_utm_north(dl_buffer)), /*m*/
++ RadOfDeg(((float)DL_GPS_course(dl_buffer)) / 10.), /*rad(CW)*/
++ MOfCm(DL_GPS_alt(dl_buffer)), /*m*/
++ MOfCm(DL_GPS_speed(dl_buffer)), /*m/s*/
++ MOfCm(DL_GPS_climb(dl_buffer)), /*m/s*/
++ (uint32_t)DL_GPS_itow(dl_buffer));
++ break;
++ }
++#endif
++ default: {
++ break;
++ }
++ }
++ return; // msg was telemetry not datalink so return
++ }
++
++ /* parse telemetry messages coming from ground station */
++ switch (msg_id) {
+ case DL_PING: {
+ DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
+ }
+@@ -90,7 +151,7 @@ void dl_parse_msg(void)
+ }
+ break;
+
+-#if defined USE_NAVIGATION
++#ifdef USE_NAVIGATION
+ case DL_BLOCK : {
+ if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
+ nav_goto_block(DL_BLOCK_block_id(dl_buffer));
+@@ -138,21 +199,21 @@ void dl_parse_msg(void)
+ }
+ break;
+ #endif // RADIO_CONTROL_TYPE_DATALINK
+-#if defined GPS_DATALINK
+-#ifdef GPS_USE_DATALINK_SMALL
+- case DL_REMOTE_GPS_SMALL :
++#if USE_GPS
++#ifdef GPS_DATALINK
++ case DL_REMOTE_GPS_SMALL : {
+ // Check if the GPS is for this AC
+- if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
+- break;
+- }
++ if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
+
+ parse_gps_datalink_small(
+ DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
+ DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
+- DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
+- break;
+-#endif
+- case DL_REMOTE_GPS :
++ DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
++ DL_REMOTE_GPS_SMALL_heading(dl_buffer));
++ }
++ break;
++
++ case DL_REMOTE_GPS : {
+ // Check if the GPS is for this AC
+ if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
+
+@@ -171,10 +232,11 @@ void dl_parse_msg(void)
+ DL_REMOTE_GPS_ecef_zd(dl_buffer),
+ DL_REMOTE_GPS_tow(dl_buffer),
+ DL_REMOTE_GPS_course(dl_buffer));
+- break;
+-#endif
+-#if USE_GPS
+- case DL_GPS_INJECT :
++ }
++ break;
++#endif // GPS_DATALINK
++
++ case DL_GPS_INJECT : {
+ // Check if the GPS is for this AC
+ if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
+
+@@ -183,9 +245,10 @@ void dl_parse_msg(void)
+ DL_GPS_INJECT_packet_id(dl_buffer),
+ DL_GPS_INJECT_data_length(dl_buffer),
+ DL_GPS_INJECT_data(dl_buffer)
+- );
+- break;
+-#endif
++ );
++ }
++ break;
++#endif // USE_GPS
+
+ case DL_GUIDED_SETPOINT_NED:
+ if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
+@@ -212,7 +275,22 @@ void dl_parse_msg(void)
+ /* others not handled yet */
+ break;
+ }
+- break;
++
++#ifdef TRAFFIC_INFO
++ case DL_ACINFO: {
++ if (DL_ACINFO_ac_id(dl_buffer) == AC_ID) { break; }
++ uint8_t id = DL_ACINFO_ac_id(dl_buffer);
++ float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
++ float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
++ float a = MOfCm(DL_ACINFO_alt(dl_buffer));
++ float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer)) / 10.);
++ float s = MOfCm(DL_ACINFO_speed(dl_buffer));
++ float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
++ uint32_t t = DL_ACINFO_itow(dl_buffer);
++ SetAcInfo(id, ux, uy, c, a, s, cl, t);
++ }
++ break;
++#endif
+ default:
+ break;
+ }
+diff --git a/sw/airborne/modules/multi/swarm_potential.c b/sw/airborne/modules/multi/swarm_potential.c
+new file mode 100644
+index 0000000..5e25a0b
+--- /dev/null
++++ b/sw/airborne/modules/multi/swarm_potential.c
+@@ -0,0 +1,204 @@
++/*
++ * Copyright (C) Kirk Scheper
++ *
++ * 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
++ * .
++ */
++/**
++ * @file "modules/multi/swarm_potential.c"
++ * @author Kirk Scheper
++ * This module is generates a command to avoid other vehicles based on their relative gps location
++ */
++
++#include "modules/multi/swarm_potential.h"
++#include "subsystems/gps.h"
++#include "subsystems/gps/gps_datalink.h"
++#include "subsystems/datalink/downlink.h"
++#include "guidance/guidance_h.h"
++#include "guidance/guidance_v.h"
++#include "state.h"
++#include "navigation.h"
++
++#include "subsystems/navigation/traffic_info.h"
++
++#include "generated/airframe.h" // AC_ID
++
++#include "subsystems/abi.h" // rssi
++
++int8_t rssi[NB_ACS_ID];
++int8_t tx_strength[NB_ACS_ID];
++
++struct force_ potential_force;
++
++float force_pos_gain;
++float force_speed_gain;
++float force_climb_gain;
++
++#ifndef FORCE_POS_GAIN
++#define FORCE_POS_GAIN 1.
++#endif
++
++#ifndef FORCE_SPEED_GAIN
++#define FORCE_SPEED_GAIN 1.
++#endif
++
++#ifndef FORCE_CLIMB_GAIN
++#define FORCE_CLIMB_GAIN 1.
++#endif
++
++#ifndef FORCE_MAX_DIST
++#define FORCE_MAX_DIST 100.
++#endif
++
++abi_event ev;
++
++void rssi_cb(uint8_t sender_id __attribute__((unused)), uint8_t _ac_id, int8_t _tx_strength, int8_t _rssi);
++void rssi_cb(uint8_t sender_id __attribute__((unused)), uint8_t _ac_id, int8_t _tx_strength, int8_t _rssi) {
++ tx_strength[the_acs_id[_ac_id]] = _tx_strength;
++ rssi[the_acs_id[_ac_id]] = _rssi;
++}
++
++#if PERIODIC_TELEMETRY
++#include "subsystems/datalink/telemetry.h"
++
++static void send_periodic(struct transport_tx *trans, struct link_device *dev) {
++ pprz_msg_send_POTENTIAL(trans, dev, AC_ID, &potential_force.east, &potential_force.north,
++ &potential_force.alt, &potential_force.speed_x, &potential_force.speed_z);
++}
++
++#endif
++
++void swarm_potential_init(void)
++{
++ potential_force.east = 0.;
++ potential_force.north = 0.;
++ potential_force.alt = 0.;
++
++ potential_force.speed_x = 0.;
++ potential_force.speed_y = 0.;
++ potential_force.speed_z = 0.;
++
++ force_pos_gain = FORCE_POS_GAIN;
++ force_speed_gain = FORCE_SPEED_GAIN;
++ force_climb_gain = FORCE_CLIMB_GAIN;
++
++ AbiBindMsgBLUEGIGA_RSSI(ABI_BROADCAST, &ev, rssi_cb);
++
++#if PERIODIC_TELEMETRY
++ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_POTENTIAL, send_periodic);
++#endif
++}
++
++void swarm_potential_periodic(void) {
++ uint16_t i;
++ for (i = 0; i < NB_ACS; ++i) {
++ if (the_acs[i].ac_id == 0 || the_acs[i].ac_id == AC_ID) { continue; }
++ struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
++ potential_force.east = ac->east;
++ potential_force.north = ac->north;
++ potential_force.alt = ac->alt;
++ }
++
++ /* The GPS messages are most likely too large to be send over either the datalink
++ * The local position is an int32 and the 11 LSBs of the x and y axis are compressed into
++ * a single integer. The z axis is considered unsigned and only the latter 10 LSBs are
++ * used.
++ */
++ uint32_t multiplex_speed = (((uint32_t)(gps.course*10.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
++ multiplex_speed |= (((uint32_t)(gps.gspeed*100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
++ multiplex_speed |= (((uint32_t)(-gps.ned_vel.z*100.0)) & 0x3FF); // bits 9-0 z position in cm
++
++ if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
++ int16_t alt = (int16_t)gps.utm_pos.alt;
++ DOWNLINK_SEND_GPS_SMALL(DefaultChannel, DefaultDevice, &multiplex_speed, &gps.utm_pos.east,
++ &gps.utm_pos.north, &alt);
++ } else if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
++ int16_t alt = (int16_t)gps.ecef_pos.z;
++ DOWNLINK_SEND_GPS_SMALL(DefaultChannel, DefaultDevice, &multiplex_speed, &gps.ecef_pos.x,
++ &gps.ecef_pos.y, &alt);
++ } else { // debug
++ int32_t nil = 0;
++ uint32_t nil1 = 0;
++ int16_t nil2 = 0;
++ DOWNLINK_SEND_GPS_SMALL(DefaultChannel, DefaultDevice, &nil1, &nil, &nil, &nil2);
++ }
++}
++
++int swarm_potential_task(void)
++{
++ uint8_t i;
++
++ float ch = cosf(stateGetHorizontalSpeedDir_f());
++ float sh = sinf(stateGetHorizontalSpeedDir_f());
++ potential_force.east = 0.;
++ potential_force.north = 0.;
++ potential_force.alt = 0.;
++
++ // compute control forces
++ int8_t nb = 0;
++ for (i = 0; i < NB_ACS; ++i) {
++ if (the_acs[i].ac_id == 0 || the_acs[i].ac_id == AC_ID) { continue; }
++ struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
++ float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
++ // if AC not responding for too long, continue, else compute force
++ if (delta_t > CARROT) { continue; }
++ else {
++ float sha = sinf(ac->course);
++ float cha = cosf(ac->course);
++ float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x;
++ float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y;
++ float da = ac->alt + ac->climb * delta_t - stateGetPositionEnu_f()->z;
++ float dist = sqrtf(de * de + dn * dn + da * da);
++ if (dist == 0.) { continue; }
++ float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
++ float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
++ float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
++ float scal = dve * de + dvn * dn + dva * da;
++ float d3 = dist * dist * dist;
++ potential_force.east += scal * de / d3;
++ potential_force.north += scal * dn / d3;
++ potential_force.alt += scal * da / d3;
++ ++nb;
++ }
++ }
++ if (nb == 0) { return 1; }
++
++ // set commands
++ //NavVerticalAutoThrottleMode(0.);
++
++ // carrot
++ potential_force.speed_x = -force_pos_gain * potential_force.east;
++ potential_force.speed_y = -force_pos_gain * potential_force.north;
++ potential_force.speed_z = -force_climb_gain * potential_force.alt;
++
++#if 0
++ VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i());
++ guidance_h.sp.speed.x = potential_force.speed_x;
++ guidance_h.sp.speed.y = potential_force.speed_y;
++
++ GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
++ guidance_v_zd_sp = potential_force.speed_z;
++#else
++ //guidance_h.sp.pos.x += potential_force.speed_x*;
++ //guidance_h.sp.pos.x += potential_force.speed_y*;
++
++ //guidance_v_z_ref += potential_force.speed_z*PERIOD;
++#endif
++
++// DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north,
++// &potential_force.alt, &potential_force.speed, &potential_force.climb);
++ return 1;
++}
+diff --git a/sw/airborne/modules/multi/swarm_potential.h b/sw/airborne/modules/multi/swarm_potential.h
+new file mode 100644
+index 0000000..4e9da9a
+--- /dev/null
++++ b/sw/airborne/modules/multi/swarm_potential.h
+@@ -0,0 +1,48 @@
++/*
++ * Copyright (C) Kirk Scheper
++ *
++ * 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
++ * .
++ */
++/**
++ * @file "modules/multi/swarm_potential.h"
++ * @author Kirk Scheper
++ * This module is generates a command to avoid other vehicles based on their relative gps location
++ */
++
++#ifndef SWARM_POTENTIAL_H
++#define SWARM_POTENTIAL_H
++
++struct force_ {
++ float east;
++ float north;
++ float alt;
++ float speed_x;
++ float speed_y;
++ float speed_z;
++};
++
++extern struct force_ potential_force;
++
++extern float force_pos_gain;
++extern float force_speed_gain;
++extern float force_climb_gain;
++
++extern void swarm_potential_init(void);
++extern void swarm_potential_periodic(void);
++extern int swarm_potential_task(void);
++
++#endif // SWARM_POTENTIAL_H
+diff --git a/sw/airborne/subsystems/datalink/bluegiga.c b/sw/airborne/subsystems/datalink/bluegiga.c
+index 8985c5b..15c09fd 100644
+--- a/sw/airborne/subsystems/datalink/bluegiga.c
++++ b/sw/airborne/subsystems/datalink/bluegiga.c
+@@ -30,9 +30,21 @@
+ #include "mcu_periph/gpio.h"
+ #include "mcu_periph/spi.h"
+
++#ifdef MODEM_LED
++#include "led.h"
++#endif
++
++#include "subsystems/abi.h"
++
++#define SENDER_ID 1
++
+ // for memset
+ #include
+
++//#include "subsystems/datalink/telemetry_common.h"
++//#include "subsystems/datalink/telemetry.h"
++#include "generated/periodic_telemetry.h"
++
+ #ifndef BLUEGIGA_SPI_DEV
+ #error "bluegiga: must define a BLUEGIGA_SPI_DEV"
+ #endif
+@@ -46,14 +58,18 @@
+ #define BLUEGIGA_DRDY_GPIO_PIN SUPERBITRF_DRDY_PIN
+ #endif
+
++#define TxStrengthOfSender(x) (x[1])
++#define RssiOfSender(x) (x[2])
++#define Pprz_StxOfMsg(x) (x[3])
++#define SenderIdOfMsg(x) (x[5])
++
+ enum BlueGigaStatus coms_status;
+ struct bluegiga_periph bluegiga_p;
+ struct spi_transaction bluegiga_spi;
+
+-signed char bluegiga_rssi[256]; // values initialized with 127
+-unsigned char telemetry_copy[20];
++uint8_t broadcast_msg[20];
+
+-void bluegiga_load_tx(struct bluegiga_periph *p);
++void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans);
+ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data);
+ void bluegiga_receive(struct spi_transaction *trans);
+
+@@ -80,6 +96,8 @@ static int dev_char_available(struct bluegiga_periph *p)
+ {
+ return bluegiga_ch_available(p);
+ }
++
++// note, need to run dev_char_available first
+ static uint8_t dev_get_byte(struct bluegiga_periph *p)
+ {
+ uint8_t ret = p->rx_buf[p->rx_extract_idx];
+@@ -115,7 +133,7 @@ static void send_bluegiga(struct transport_tx *trans, struct link_device *dev)
+
+ if (now_ts > last_ts) {
+ uint32_t rate = 1000 * bluegiga_p.bytes_recvd_since_last / (now_ts - last_ts);
+- pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate, 20, telemetry_copy);
++ pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate);
+
+ bluegiga_p.bytes_recvd_since_last = 0;
+ last_ts = now_ts;
+@@ -140,7 +158,7 @@ void bluegiga_init(struct bluegiga_periph *p)
+ bluegiga_spi.cpha = SPICphaEdge2;
+ bluegiga_spi.dss = SPIDss8bit;
+ bluegiga_spi.bitorder = SPIMSBFirst;
+- bluegiga_spi.cdiv = SPIDiv64;
++ bluegiga_spi.cdiv = SPIDiv256;
+ bluegiga_spi.after_cb = (SPICallback) trans_cb;
+
+ // Configure generic link device
+@@ -157,18 +175,14 @@ void bluegiga_init(struct bluegiga_periph *p)
+ p->tx_insert_idx = 0;
+ p->tx_extract_idx = 0;
+
+- for (int i = 0; i < bluegiga_spi.input_length; i++) {
+- p->work_rx[i] = 0;
+- }
+- for (int i = 0; i < bluegiga_spi.output_length; i++) {
+- p->work_tx[i] = 0;
+- }
+- for (int i = 0; i < 255; i++) {
+- bluegiga_rssi[i] = 127;
+- }
++ memset(p->work_rx, 0, bluegiga_spi.input_length);
++ memset(p->work_tx, 0, bluegiga_spi.output_length);
++
++ memset(broadcast_msg, 0, 19);
+
+ p->bytes_recvd_since_last = 0;
+ p->end_of_msg = p->tx_insert_idx;
++ p->connected = 0;
+
+ // set DRDY interrupt pin for spi master triggered on falling edge
+ gpio_setup_output(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN);
+@@ -194,10 +208,11 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data)
+ }
+
+ /* Load waiting data into tx peripheral buffer */
+-void bluegiga_load_tx(struct bluegiga_periph *p)
++void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans)
+ {
++ uint8_t packet_len;
+ // check data available in buffer to send
+- uint8_t packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE);
++ packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE);
+ if (packet_len > 19) {
+ packet_len = 19;
+ }
+@@ -214,7 +229,7 @@ void bluegiga_load_tx(struct bluegiga_periph *p)
+ bluegiga_increment_buf(&p->tx_extract_idx, packet_len);
+
+ // clear unused bytes
+- for (i = packet_len + 1; i < bluegiga_spi.output_length; i++) {
++ for (i = packet_len + 1; i < trans->output_length; i++) {
+ p->work_tx[i] = 0;
+ }
+
+@@ -235,97 +250,104 @@ void bluegiga_receive(struct spi_transaction *trans)
+ for (uint8_t i = 0; i < trans->output_length; i++) {
+ trans->output_buf[i] = 0;
+ }
++ } else if (coms_status == BLUEGIGA_SENDING_BROADCAST) {
++ // sending second half of broadcast message
++ for (uint8_t i = 0; i < broadcast_msg[0]; i++) {
++ trans->output_buf[i] = broadcast_msg[i];
++ }
++ coms_status = BLUEGIGA_SENDING;
++ return;
+ }
+
+ /*
+- * 0xff communication lost with ground station
+- * 0xfe RSSI value from broadcaster
+- * 0xfd Change in broadcast mode
+- * 0xfc Receive all recorded RSSI
+- * <=20 Data package from ground station
++ * >235 data package from broadcast mode
++ * 0x50 communication lost with ground station
++ * 0x51 interrupt handled
++ * <20 data package from connection
+ */
+
+ uint8_t packet_len = 0;
+ uint8_t read_offset = 0;
+ switch (trans->input_buf[0]) {
+- case 0xff: // communication lost with ground station
+-#ifdef MODEM_LED
+- LED_OFF(MODEM_LED);
+-#endif
+- coms_status = BLUEGIGA_UNINIT;
+- gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
+- break;
+- case 0xfe: // RSSI value from broadcaster
+- bluegiga_rssi[trans->input_buf[1]] = trans->input_buf[2];
+- packet_len = trans->input_buf[3];
+- read_offset = 4;
+- break;
+- case 0xfd: // Change in broadcast mode
+- gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
+-
+- // fetch scan status
+- if (trans->input_buf[1] == 1) {
+- coms_status = BLUEGIGA_BROADCASTING;
++ case 0x50: // communication status changed
++ bluegiga_p.connected = trans->input_buf[1];
++ if (bluegiga_p.connected) {
++ //telemetry_mode_Main = TELEMETRY_PROCESS_Main;
+ } else {
+- coms_status = BLUEGIGA_UNINIT;
++ //telemetry_mode_Main = NB_TELEMETRY_MODES; // send no periodic telemetry
+ }
++ coms_status = BLUEGIGA_IDLE;
+ break;
+- case 0xfc: // Receive all recorded RSSI
+- for (uint8_t i = 0; i < trans->input_buf[1]; i++) {
+- bluegiga_rssi[trans->input_buf[2] + i] = trans->input_buf[3 + i];
+- }
++ case 0x51: // Interrupt handled
++ gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
+ break;
+ default:
+- packet_len = trans->input_buf[0]; // length of transmitted message
+- read_offset = 1;
++ coms_status = BLUEGIGA_IDLE;
++ // compute length of transmitted message
++ if (trans->input_buf[0] < trans->input_length) { // normal connection mode
++ packet_len = trans->input_buf[0];
++ read_offset = 1;
++ } else if (trans->input_buf[0] > 0xff - trans->input_length) { // broadcast mode
++ packet_len = 0xff - trans->input_buf[0];
++
++ int8_t tx_strength = TxStrengthOfSender(trans->input_buf);
++ int8_t rssi = RssiOfSender(trans->input_buf);
++ uint8_t ac_id = SenderIdOfMsg(trans->input_buf);
++
++ if (Pprz_StxOfMsg(trans->input_buf) == STX) {
++ AbiSendMsgBLUEGIGA_RSSI(SENDER_ID, ac_id, tx_strength, rssi);
++ }
++
++ read_offset = 3;
++ }
+ }
+
+ // handle incoming datalink message
+- if (packet_len > 0 && packet_len <= trans->input_length) {
++ if (packet_len > 0 && packet_len <= trans->input_length - read_offset) {
++#ifdef MODEM_LED
++ LED_TOGGLE(MODEM_LED);
++#endif
+ // Handle received message
+ for (uint8_t i = 0; i < packet_len; i++) {
+ bluegiga_p.rx_buf[(bluegiga_p.rx_insert_idx + i) % BLUEGIGA_BUFFER_SIZE] = trans->input_buf[i + read_offset];
+ }
+ bluegiga_increment_buf(&bluegiga_p.rx_insert_idx, packet_len);
+ bluegiga_p.bytes_recvd_since_last += packet_len;
+- coms_status = BLUEGIGA_IDLE;
+-
+- for (uint8_t i = 0; i < trans->input_length; i++) {
+- telemetry_copy[i] = trans->input_buf[i];
+- }
+- } else {
+- coms_status = BLUEGIGA_IDLE;
+ }
+
+ // load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register
+- bluegiga_load_tx(&bluegiga_p);
++ bluegiga_load_tx(&bluegiga_p, trans);
+
+ // register spi slave read for next transaction
+- spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi);
++ spi_slave_register(&(BLUEGIGA_SPI_DEV), trans);
+ }
+ }
+
+-/* command bluetooth to switch to active scan mode to get rssi values from neighbouring drones */
+-void bluegiga_scan(struct bluegiga_periph *p)
++/* Send data for broadcast message to the bluegiga module
++ * maximum size of message is 22 bytes
++ */
++void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len)
+ {
++ if (msg_len == 0 || msg_len > 22) {
++ return;
++ }
+
+- memset(p->work_tx, 0, 20);
+- p->work_tx[0] = 0xfd; // change broadcast mode header
+-
+- coms_status = BLUEGIGA_SENDING;
+-
+- // trigger bluegiga to read direct command
+- gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt
+-}
+-
+-/* Request list of all recorded RSSI */
+-void bluegiga_request_all_rssi(struct bluegiga_periph *p)
+-{
++ uint8_t max_length = 20;
++ p->work_tx[0] = msg_len;
+
+- memset(p->work_tx, 0, 20);
+- p->work_tx[0] = 0xfc;
++ if (msg_len < max_length) {
++ for (uint8_t i = 0; i < msg_len; i++) {
++ p->work_tx[i + 1] = msg[i];
++ }
++ coms_status = BLUEGIGA_SENDING;
++ } else {
++ for (uint8_t i = 0; i < max_length - 1; i++) {
++ p->work_tx[i + 1] = msg[i];
++ }
+
+- coms_status = BLUEGIGA_SENDING;
++ memcpy(broadcast_msg, msg + max_length - 1, msg_len - (max_length - 1));
++ coms_status = BLUEGIGA_SENDING_BROADCAST;
++ }
+
+ // trigger bluegiga to read direct command
+ gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt
+diff --git a/sw/airborne/subsystems/datalink/bluegiga.h b/sw/airborne/subsystems/datalink/bluegiga.h
+index f7770ab..59988c2 100644
+--- a/sw/airborne/subsystems/datalink/bluegiga.h
++++ b/sw/airborne/subsystems/datalink/bluegiga.h
+@@ -34,7 +34,7 @@ enum BlueGigaStatus {
+ BLUEGIGA_UNINIT, /**< The com isn't initialized */
+ BLUEGIGA_IDLE, /**< The com is in idle */
+ BLUEGIGA_SENDING, /**< The com is sending */
+- BLUEGIGA_BROADCASTING /**< The com is switched from data link to rssi scanning */
++ BLUEGIGA_SENDING_BROADCAST /**< The com is switched from data link to rssi scanning */
+ };
+
+ #ifndef BLUEGIGA_BUFFER_SIZE
+@@ -63,6 +63,7 @@ struct bluegiga_periph {
+ /* some administrative variables */
+ uint32_t bytes_recvd_since_last;
+ uint8_t end_of_msg;
++ uint8_t connected;
+
+ };
+
+@@ -75,38 +76,6 @@ void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len);
+
+ void bluegiga_init(struct bluegiga_periph *p);
+ void bluegiga_scan(struct bluegiga_periph *p);
+-void bluegiga_request_all_rssi(struct bluegiga_periph *p);
+-
+-// BLUEGIGA is using pprz_transport
+-// FIXME it should not appear here, this will be fixed with the rx improvements some day...
+-// BLUEGIGA needs a specific read_buffer function
+-#include "subsystems/datalink/pprz_transport.h"
+-#include "led.h"
+-static inline void bluegiga_read_buffer(struct bluegiga_periph *p, struct pprz_transport *t)
+-{
+- do {
+- uint8_t c = 0;
+- do {
+- parse_pprz(t, p->rx_buf[(p->rx_extract_idx + c++) % BLUEGIGA_BUFFER_SIZE]);
+- } while (((p->rx_extract_idx + c) % BLUEGIGA_BUFFER_SIZE != p->rx_insert_idx)
+- && !(t->trans_rx.msg_received));
+- // reached end of circular read buffer or message received
+- // if received, decode and advance
+- if (t->trans_rx.msg_received) {
+-#ifdef MODEM_LED
+- LED_TOGGLE(MODEM_LED);
+-#endif
+- pprz_parse_payload(t);
+- t->trans_rx.msg_received = FALSE;
+- }
+- bluegiga_increment_buf(&p->rx_extract_idx, c);
+- } while (bluegiga_ch_available(p)); // continue till all messages read
+-}
+-
+-// transmit previous data in buffer and parse data received
+-#define BlueGigaCheckAndParse(_dev,_trans) { \
+- if (bluegiga_ch_available(&(_dev))) \
+- bluegiga_read_buffer(&(_dev), &(_trans)); \
+- }
++void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len);
+
+ #endif /* BLUEGIGA_DATA_LINK_H */
+diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
+index 23bbbb9..1cb8ccc 100644
+--- a/sw/airborne/subsystems/datalink/datalink.h
++++ b/sw/airborne/subsystems/datalink/datalink.h
+@@ -117,7 +117,7 @@ static inline void DlCheckAndParse(void)
+ #elif defined DATALINK && DATALINK == BLUEGIGA
+
+ #define DatalinkEvent() { \
+- BlueGigaCheckAndParse(DOWNLINK_DEVICE, pprz_tp); \
++ PprzCheckAndParse(DOWNLINK_DEVICE, pprz_tp);\
+ DlCheckAndParse(); \
+ }
+
+diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c
+index a1b113b..21389c5 100644
+--- a/sw/airborne/subsystems/datalink/telemetry.c
++++ b/sw/airborne/subsystems/datalink/telemetry.c
+@@ -48,7 +48,7 @@ int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id,
+ uint8_t i, j;
+ // return if NULL is passed as periodic_telemetry
+ if (_pt == NULL) { return -1; }
+- // check if message with id _msgn has a periodic entery in telemetry file
++ // check if message with id _msgn has a periodic entry in telemetry file
+ for (i = 0; i < _pt->nb; i++) {
+ if (_pt->cbs[i].id == _id) {
+ // msg found, register another callback if not all TELEMETRY_NB_CBS slots taken
+diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
+index 1b6f4c3..500821d 100644
+--- a/sw/airborne/subsystems/gps/gps_datalink.c
++++ b/sw/airborne/subsystems/gps/gps_datalink.c
+@@ -27,35 +27,19 @@
+ * GPS structure to the values received.
+ */
+
++#include "messages.h"
++#include "generated/airframe.h" // AC_ID
++#include "generated/flight_plan.h" // reference lla NAV_XXX0
++#include "subsystems/datalink/downlink.h"
++
+ #include "subsystems/gps.h"
+ #include "subsystems/abi.h"
+
+-// #include
+-
+-#if GPS_USE_DATALINK_SMALL
+-#ifndef GPS_LOCAL_ECEF_ORIGIN_X
+-#error Local x coordinate in ECEF of the remote GPS required
+-#endif
+-
+-#ifndef GPS_LOCAL_ECEF_ORIGIN_Y
+-#error Local y coordinate in ECEF of the remote GPS required
+-#endif
+-
+-#ifndef GPS_LOCAL_ECEF_ORIGIN_Z
+-#error Local z coordinate in ECEF of the remote GPS required
+-#endif
+-
+-struct EcefCoor_i tracking_ecef;
+-
+-struct LtpDef_i tracking_ltp;
++// #include
+
++struct LtpDef_i ltp_def;
+ struct EnuCoor_i enu_pos, enu_speed;
+
+-struct EcefCoor_i ecef_pos, ecef_vel;
+-
+-struct LlaCoor_i lla_pos;
+-#endif
+-
+ bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed
+
+ /** GPS initialization */
+@@ -66,70 +50,68 @@ void gps_impl_init(void)
+ gps.gspeed = 700; // To enable course setting
+ gps.cacc = 0; // To enable course setting
+
+-#if GPS_USE_DATALINK_SMALL
+- tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X;
+- tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y;
+- tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z;
++ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
++ llh_nav0.lat = NAV_LAT0;
++ llh_nav0.lon = NAV_LON0;
++ /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
++ llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
+
+- ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef);
+-#endif
++ struct EcefCoor_i ecef_nav0;
++ ecef_of_lla_i(&ecef_nav0, &llh_nav0);
++ ltp_def_from_ecef_i(<p_def, &ecef_nav0);
+ }
+
+-#ifdef GPS_USE_DATALINK_SMALL
+ // Parse the REMOTE_GPS_SMALL datalink packet
+-void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy)
++void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
+ {
+-
+ // Position in ENU coordinates
+- enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm
+- if (enu_pos.x & 0x200) {
+- enu_pos.x |= 0xFFFFFC00; // fix for twos complements
++ enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
++ if (enu_pos.x & 0x400) {
++ enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
+ }
+- enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm
+- if (enu_pos.y & 0x200) {
+- enu_pos.y |= 0xFFFFFC00; // fix for twos complements
++ enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
++ if (enu_pos.y & 0x400) {
++ enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
+ }
+- enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm
+- // bits 1 and 0 are free
+-
+- // printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z);
++ enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
+
+ // Convert the ENU coordinates to ECEF
+- ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos);
+- gps.ecef_pos = ecef_pos;
++ ecef_of_enu_point_i(&gps.ecef_pos, <p_def, &enu_pos);
++ SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
+
+- lla_of_ecef_i(&lla_pos, &ecef_pos);
+- gps.lla_pos = lla_pos;
++ lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos);
+ SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
+
+- enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s
+- if (enu_speed.x & 0x200) {
+- enu_speed.x |= 0xFFFFFC00; // fix for twos complements
++ enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
++ if (enu_speed.x & 0x400) {
++ enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
+ }
+- enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s
+- if (enu_speed.y & 0x200) {
+- enu_speed.y |= 0xFFFFFC00; // fix for twos complements
++ enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
++ if (enu_speed.y & 0x400) {
++ enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
++ }
++ enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
++ if (enu_speed.z & 0x200) {
++ enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
+ }
+- enu_speed.z = 0;
+-
+- // printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z);
+
+- ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed);
++ ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed);
+ SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
+
+- gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy
++ // todo check correct
++ gps.ned_vel.x = enu_speed.x;
++ gps.ned_vel.y = enu_speed.y;
++ gps.ned_vel.z = -enu_speed.z;
++ SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
++
++ gps.hmsl = ltp_def.hmsl + enu_pos.z * 10;
+ SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
+
+- gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2
+- if (gps.course & 0x200) {
+- gps.course |= 0xFFFFFC00; // fix for twos complements
+- }
+- // printf("Heading: %d\n", gps.course);
+- gps.course *= 1e5;
++ gps.course = ((int32_t)heading)*1e3;
+ SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
+
+ gps.num_sv = num_sv;
+- gps.tow = 0; // set time-of-weak to 0
++ gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // todo check
+ gps.fix = GPS_FIX_3D; // set 3D fix to true
+ gps_available = TRUE; // set GPS available to true
+
+@@ -143,7 +125,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
+ }
+ AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
+ }
+-#endif
+
+ /** Parse the REMOTE_GPS datalink packet */
+ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
+@@ -168,10 +149,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
+ gps.ecef_vel.z = ecef_zd;
+ SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
+
++ struct LtpDef_i ref_ltp;
++ ltp_def_from_ecef_i(&ref_ltp, &gps.ecef_pos);
++ ned_of_ecef_vect_i(&gps.ned_vel, &ref_ltp, &gps.ecef_vel);
++ SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
++
+ gps.course = course;
+ SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
++
+ gps.num_sv = numsv;
+- gps.tow = tow;
++ if (tow == 0) {
++ gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
++ } else {
++ gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
++ }
+ gps.fix = GPS_FIX_3D;
+ gps_available = TRUE;
+
+diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h
+index 504ba49..2b9905e 100644
+--- a/sw/airborne/subsystems/gps/gps_datalink.h
++++ b/sw/airborne/subsystems/gps/gps_datalink.h
+@@ -32,12 +32,19 @@
+
+ #include "std.h"
+ #include "generated/airframe.h"
++#include "subsystems/gps.h"
+ #define GPS_NB_CHANNELS 0
+
+ extern bool_t gps_available;
+-#ifdef GPS_USE_DATALINK_SMALL
+-void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy);
+-#endif
++
++extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading);
++
++struct GpsState;
++extern void parse_remote_gps_datalink_small(struct GpsState *remote_gps, uint8_t num_sv, uint32_t pos_xyz,
++ uint32_t speed_xyh, int8_t speed_z);
++
++extern void send_remote_gps_datalink_small(void);
++
+ extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
+ int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
+ int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
+diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
+index 7081e20..abbd2f4 100644
+--- a/sw/airborne/subsystems/ins/ins_int.c
++++ b/sw/airborne/subsystems/ins/ins_int.c
+@@ -98,6 +98,10 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
+ #define INS_VFF_R_GPS 2.0
+ #endif
+
++#ifndef INS_VFF_VZ_R_GPS
++#define INS_VFF_VZ_R_GPS 2.0
++#endif
++
+ /** maximum number of propagation steps without any updates in between */
+ #ifndef INS_MAX_PROPAGATION_STEPS
+ #define INS_MAX_PROPAGATION_STEPS 200
+@@ -374,6 +378,7 @@ void ins_int_update_gps(struct GpsState *gps_s)
+
+ #if INS_USE_GPS_ALT
+ vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
++ vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
+ #endif
+
+ #if USE_HFF
+diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c
+index eeff6eb..8f56a43 100644
+--- a/sw/airborne/subsystems/navigation/traffic_info.c
++++ b/sw/airborne/subsystems/navigation/traffic_info.c
+@@ -28,8 +28,11 @@
+
+ #include
+ #include "subsystems/navigation/traffic_info.h"
++//#include "subsystems/navigation/common_nav.h"
+ #include "generated/airframe.h"
+
++#include "subsystems/gps.h"
++
+ uint8_t acs_idx;
+ uint8_t the_acs_id[NB_ACS_ID];
+ struct ac_info_ the_acs[NB_ACS];
+@@ -46,3 +49,45 @@ struct ac_info_ *get_ac_info(uint8_t id)
+ {
+ return &the_acs[the_acs_id[id]];
+ }
++
++// 0 is reserved for ground station (id=0)
++// 1 is reserved for this AC (id=AC_ID)
++void SetAcInfo(uint8_t _id, float _utm_x /*m*/, float _utm_y /*m*/, float _course/*rad(CW)*/, float _alt/*m*/, float _gspeed/*m/s*/, float _climb, uint32_t _itow) {
++ if (acs_idx < NB_ACS) {
++ if (_id > 0 && the_acs_id[_id] == 0) {
++ the_acs_id[_id] = acs_idx++;
++ the_acs[the_acs_id[_id]].ac_id = _id;
++ }
++ the_acs[the_acs_id[_id]].east = _utm_x;// - nav_utm_east0;
++ the_acs[the_acs_id[_id]].north = _utm_y;// - nav_utm_north0;
++ the_acs[the_acs_id[_id]].course = _course;
++ the_acs[the_acs_id[_id]].alt = _alt;
++ the_acs[the_acs_id[_id]].gspeed = _gspeed;
++ the_acs[the_acs_id[_id]].climb = _climb;
++ the_acs[the_acs_id[_id]].itow = _itow;
++ }
++}
++
++// 0 is reserved for ground station (id=0)
++// 1 is reserved for this AC (id=AC_ID)
++void SetAcInfoLLA(uint8_t id, uint32_t lat, uint32_t lon, uint32_t alt, uint32_t course, uint16_t gspeed, uint16_t climb,
++ uint32_t tow)
++{
++ if (acs_idx < NB_ACS) {
++ if (id > 0 && the_acs_id[id] == 0) {
++ the_acs_id[id] = acs_idx++;
++ the_acs[the_acs_id[id]].ac_id = id;
++ }
++
++ struct EcefCoor_i ecef_pos;
++ struct LlaCoor_i lla = {.lat = lat, .lon = lon, .alt = alt};
++ ecef_of_lla_i(&ecef_pos, &lla);
++ the_acs[the_acs_id[id]].east = (float)ecef_pos.x * 100;
++ the_acs[the_acs_id[id]].north = (float)ecef_pos.y * 100;
++ the_acs[the_acs_id[id]].alt = (float)ecef_pos.z * 100;
++ the_acs[the_acs_id[id]].course = (float)course;
++ the_acs[the_acs_id[id]].gspeed = (float)gspeed*100;
++ the_acs[the_acs_id[id]].climb = (float)climb*100;
++ the_acs[the_acs_id[id]].itow = tow;
++ }
++}
+diff --git a/sw/airborne/subsystems/navigation/traffic_info.h b/sw/airborne/subsystems/navigation/traffic_info.h
+index eef8b75..55278b9 100644
+--- a/sw/airborne/subsystems/navigation/traffic_info.h
++++ b/sw/airborne/subsystems/navigation/traffic_info.h
+@@ -47,26 +47,12 @@ extern uint8_t acs_idx;
+ extern uint8_t the_acs_id[NB_ACS_ID];
+ extern struct ac_info_ the_acs[NB_ACS];
+
+-// 0 is reserved for ground station (id=0)
+-// 1 is reserved for this AC (id=AC_ID)
+-#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/,_climb, _itow) { \
+- if (acs_idx < NB_ACS) { \
+- if (_id > 0 && the_acs_id[_id] == 0) { \
+- the_acs_id[_id] = acs_idx++; \
+- the_acs[the_acs_id[_id]].ac_id = (uint8_t)_id; \
+- } \
+- the_acs[the_acs_id[_id]].east = _utm_x - nav_utm_east0; \
+- the_acs[the_acs_id[_id]].north = _utm_y - nav_utm_north0; \
+- the_acs[the_acs_id[_id]].course = _course; \
+- the_acs[the_acs_id[_id]].alt = _alt; \
+- the_acs[the_acs_id[_id]].gspeed = _gspeed; \
+- the_acs[the_acs_id[_id]].climb = _climb; \
+- the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \
+- } \
+- }
+-
+ extern void traffic_info_init(void);
+-
+ struct ac_info_ *get_ac_info(uint8_t id);
+
++void SetAcInfo(uint8_t _id, float _utm_x /*m*/, float _utm_y /*m*/, float _course/*rad(CW)*/, float _alt/*m*/, float _gspeed/*m/s*/, float _climb, uint32_t _itow);
++// todo define units
++void SetAcInfoLLA(uint8_t id, uint32_t lat, uint32_t lon, uint32_t alt, uint32_t course, uint16_t gspeed, uint16_t climb,
++ uint32_t tow);
++
+ #endif
+diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c
+index 9ad0a21..bab8915 100644
+--- a/sw/ground_segment/misc/natnet2ivy.c
++++ b/sw/ground_segment/misc/natnet2ivy.c
+@@ -19,14 +19,14 @@
+ * Boston, MA 02111-1307, USA.
+ */
+
+- /** \file natnet2ivy.c
+- * \brief NatNet (GPS) to ivy forwarder
+- *
+- * This receives aircraft position information through the Optitrack system
+- * NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps
+- * subsystem "datalink" is then able to parse the GPS position and use it to
+- * navigate inside the Optitrack system.
+- */
++/** \file natnet2ivy.c
++* \brief NatNet (GPS) to ivy forwarder
++*
++* This receives aircraft position information through the Optitrack system
++* NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps
++* subsystem "datalink" is then able to parse the GPS position and use it to
++* navigate inside the Optitrack system.
++*/
+
+ #include
+ #include
+@@ -38,6 +38,7 @@
+ #include
+ #include
+ #include
++#include
+
+ #include "std.h"
+ #include "arch/linux/udp_socket.h"
+@@ -57,6 +58,9 @@ uint16_t natnet_data_port = 1511;
+ uint8_t natnet_major = 2;
+ uint8_t natnet_minor = 7;
+
++FILE *fp;
++bool_t log_exists = 0;
++
+ /** Ivy Bus default */
+ #ifdef __APPLE__
+ char *ivy_bus = "224.255.255.255";
+@@ -126,8 +130,9 @@ double tracking_offset_angle; ///< The offset from the tracking system to
+ float natnet_latency;
+
+ /** Parse the packet from NatNet */
+-void natnet_parse(unsigned char *in) {
+- int i,j,k;
++void natnet_parse(unsigned char *in)
++{
++ int i, j, k;
+
+ // Create a pointer to go trough the packet
+ char *ptr = (char *)in;
+@@ -143,8 +148,7 @@ void natnet_parse(unsigned char *in) {
+ memcpy(&nBytes, ptr, 2); ptr += 2;
+ printf_natnet("Byte count : %d\n", nBytes);
+
+- if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet
+- {
++ if (MessageID == NAT_FRAMEOFDATA) { // FRAME OF MOCAP DATA packet
+ // Frame number
+ int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4;
+ printf_natnet("Frame # : %d\n", frameNumber);
+@@ -154,8 +158,7 @@ void natnet_parse(unsigned char *in) {
+ int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4;
+ printf_natnet("Marker Set Count : %d\n", nMarkerSets);
+
+- for (i=0; i < nMarkerSets; i++)
+- {
++ for (i = 0; i < nMarkerSets; i++) {
+ // Markerset name
+ char szName[256];
+ strcpy(szName, ptr);
+@@ -167,24 +170,22 @@ void natnet_parse(unsigned char *in) {
+ int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4;
+ printf_natnet("Marker Count : %d\n", nMarkers);
+
+- for(j=0; j < nMarkers; j++)
+- {
++ for (j = 0; j < nMarkers; j++) {
+ float x = 0; memcpy(&x, ptr, 4); ptr += 4;
+ float y = 0; memcpy(&y, ptr, 4); ptr += 4;
+ float z = 0; memcpy(&z, ptr, 4); ptr += 4;
+- printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n",j,x,y,z);
++ printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n", j, x, y, z);
+ }
+ }
+
+ // Unidentified markers
+ int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4;
+ printf_natnet("Unidentified Marker Count : %d\n", nOtherMarkers);
+- for(j=0; j < nOtherMarkers; j++)
+- {
++ for (j = 0; j < nOtherMarkers; j++) {
+ float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
+ float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
+ float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4;
+- printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n",j,x,y,z);
++ printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n", j, x, y, z);
+ }
+
+ // ========== RIGID BODIES ==========
+@@ -194,13 +195,14 @@ void natnet_parse(unsigned char *in) {
+ printf_natnet("Rigid Body Count : %d\n", nRigidBodies);
+
+ // Check if there ie enough space for the rigid bodies
+- if(nRigidBodies > MAX_RIGIDBODIES) {
+- fprintf(stderr, "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", MAX_RIGIDBODIES);
++ if (nRigidBodies > MAX_RIGIDBODIES) {
++ fprintf(stderr,
++ "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n",
++ MAX_RIGIDBODIES);
+ exit(EXIT_FAILURE);
+ }
+
+- for (j=0; j < nRigidBodies; j++)
+- {
++ for (j = 0; j < nRigidBodies; j++) {
+ // rigid body pos/ori
+ struct RigidBody old_rigid;
+ memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody));
+@@ -214,15 +216,17 @@ void natnet_parse(unsigned char *in) {
+ memcpy(&rigidBodies[j].qy, ptr, 4); ptr += 4; //qz --> QY
+ memcpy(&rigidBodies[j].qw, ptr, 4); ptr += 4; //qw --> QW
+ printf_natnet("ID (%d) : %d\n", j, rigidBodies[j].id);
+- printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x,rigidBodies[j].y,rigidBodies[j].z);
+- printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx,rigidBodies[j].qy,rigidBodies[j].qz,rigidBodies[j].qw);
++ printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x, rigidBodies[j].y, rigidBodies[j].z);
++ printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx, rigidBodies[j].qy, rigidBodies[j].qz,
++ rigidBodies[j].qw);
+
+ // Differentiate the position to get the speed (TODO: crossreference with labeled markers for occlussion)
+ rigidBodies[j].totalVelocitySamples++;
+- if(old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z
+- || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz || old_rigid.qw != rigidBodies[j].qw) {
++ if (old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z
++ || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz
++ || old_rigid.qw != rigidBodies[j].qw) {
+
+- if(old_rigid.posSampled) {
++ if (old_rigid.posSampled) {
+ rigidBodies[j].vel_x += (rigidBodies[j].x - old_rigid.x);
+ rigidBodies[j].vel_y += (rigidBodies[j].y - old_rigid.y);
+ rigidBodies[j].vel_z += (rigidBodies[j].z - old_rigid.z);
+@@ -231,12 +235,12 @@ void natnet_parse(unsigned char *in) {
+
+ rigidBodies[j].nSamples++;
+ rigidBodies[j].posSampled = TRUE;
+- }
+- else
++ } else {
+ rigidBodies[j].posSampled = FALSE;
++ }
+
+ // When marker id changed, reset the velocity
+- if(old_rigid.id != rigidBodies[j].id) {
++ if (old_rigid.id != rigidBodies[j].id) {
+ rigidBodies[j].vel_x = 0;
+ rigidBodies[j].vel_y = 0;
+ rigidBodies[j].vel_z = 0;
+@@ -249,71 +253,67 @@ void natnet_parse(unsigned char *in) {
+ // Associated marker positions
+ memcpy(&rigidBodies[j].nMarkers, ptr, 4); ptr += 4;
+ printf_natnet("Marker Count: %d\n", rigidBodies[j].nMarkers);
+- int nBytes = rigidBodies[j].nMarkers*3*sizeof(float);
+- float* markerData = (float*)malloc(nBytes);
++ int nBytes = rigidBodies[j].nMarkers * 3 * sizeof(float);
++ float *markerData = (float *)malloc(nBytes);
+ memcpy(markerData, ptr, nBytes);
+ ptr += nBytes;
+
+- if(natnet_major >= 2)
+- {
++ if (natnet_major >= 2) {
+ // Associated marker IDs
+- nBytes = rigidBodies[j].nMarkers*sizeof(int);
+- int* markerIDs = (int*)malloc(nBytes);
++ nBytes = rigidBodies[j].nMarkers * sizeof(int);
++ int *markerIDs = (int *)malloc(nBytes);
+ memcpy(markerIDs, ptr, nBytes);
+ ptr += nBytes;
+
+ // Associated marker sizes
+- nBytes = rigidBodies[j].nMarkers*sizeof(float);
+- float* markerSizes = (float*)malloc(nBytes);
++ nBytes = rigidBodies[j].nMarkers * sizeof(float);
++ float *markerSizes = (float *)malloc(nBytes);
+ memcpy(markerSizes, ptr, nBytes);
+ ptr += nBytes;
+
+- for(k=0; k < rigidBodies[j].nMarkers; k++)
+- {
+- printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
++ for (k = 0; k < rigidBodies[j].nMarkers; k++) {
++ printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k],
++ markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]);
+ }
+
+- if(markerIDs)
++ if (markerIDs) {
+ free(markerIDs);
+- if(markerSizes)
++ }
++ if (markerSizes) {
+ free(markerSizes);
++ }
+
+- }
+- else
+- {
+- for(k=0; k < rigidBodies[j].nMarkers; k++)
+- {
+- printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
++ } else {
++ for (k = 0; k < rigidBodies[j].nMarkers; k++) {
++ printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k * 3], markerData[k * 3 + 1],
++ markerData[k * 3 + 2]);
+ }
+ }
+- if(markerData)
++ if (markerData) {
+ free(markerData);
++ }
+
+- if(natnet_major >= 2)
+- {
++ if (natnet_major >= 2) {
+ // Mean marker error
+ memcpy(&rigidBodies[j].error, ptr, 4); ptr += 4;
+ printf_natnet("Mean marker error: %3.8f\n", rigidBodies[j].error);
+ }
+
+ // 2.6 and later
+- if( ((natnet_major == 2)&&(natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0) )
+- {
+- // params
+- short params = 0; memcpy(¶ms, ptr, 2); ptr += 2;
++ if (((natnet_major == 2) && (natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0)) {
++ // params
++ short params = 0; memcpy(¶ms, ptr, 2); ptr += 2;
+ // bool bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame
+ }
+ } // next rigid body
+
+ // ========== SKELETONS ==========
+ // Skeletons (version 2.1 and later)
+- if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2))
+- {
++ if (((natnet_major == 2) && (natnet_minor > 0)) || (natnet_major > 2)) {
+ int nSkeletons = 0;
+ memcpy(&nSkeletons, ptr, 4); ptr += 4;
+ printf_natnet("Skeleton Count : %d\n", nSkeletons);
+- for (j=0; j < nSkeletons; j++)
+- {
++ for (j = 0; j < nSkeletons; j++) {
+ // Skeleton id
+ int skeletonID = 0;
+ memcpy(&skeletonID, ptr, 4); ptr += 4;
+@@ -321,8 +321,7 @@ void natnet_parse(unsigned char *in) {
+ int nRigidBodies = 0;
+ memcpy(&nRigidBodies, ptr, 4); ptr += 4;
+ printf_natnet("Rigid Body Count : %d\n", nRigidBodies);
+- for (j=0; j < nRigidBodies; j++)
+- {
++ for (j = 0; j < nRigidBodies; j++) {
+ // Rigid body pos/ori
+ int ID = 0; memcpy(&ID, ptr, 4); ptr += 4;
+ float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
+@@ -333,32 +332,32 @@ void natnet_parse(unsigned char *in) {
+ float qz = 0; memcpy(&qz, ptr, 4); ptr += 4;
+ float qw = 0; memcpy(&qw, ptr, 4); ptr += 4;
+ printf_natnet("ID : %d\n", ID);
+- printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x,y,z);
+- printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx,qy,qz,qw);
++ printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x, y, z);
++ printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx, qy, qz, qw);
+
+ // Sssociated marker positions
+ int nRigidMarkers = 0; memcpy(&nRigidMarkers, ptr, 4); ptr += 4;
+ printf_natnet("Marker Count: %d\n", nRigidMarkers);
+- int nBytes = nRigidMarkers*3*sizeof(float);
+- float* markerData = (float*)malloc(nBytes);
++ int nBytes = nRigidMarkers * 3 * sizeof(float);
++ float *markerData = (float *)malloc(nBytes);
+ memcpy(markerData, ptr, nBytes);
+ ptr += nBytes;
+
+ // Associated marker IDs
+- nBytes = nRigidMarkers*sizeof(int);
+- int* markerIDs = (int*)malloc(nBytes);
++ nBytes = nRigidMarkers * sizeof(int);
++ int *markerIDs = (int *)malloc(nBytes);
+ memcpy(markerIDs, ptr, nBytes);
+ ptr += nBytes;
+
+ // Associated marker sizes
+- nBytes = nRigidMarkers*sizeof(float);
+- float* markerSizes = (float*)malloc(nBytes);
++ nBytes = nRigidMarkers * sizeof(float);
++ float *markerSizes = (float *)malloc(nBytes);
+ memcpy(markerSizes, ptr, nBytes);
+ ptr += nBytes;
+
+- for(k=0; k < nRigidMarkers; k++)
+- {
+- printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
++ for (k = 0; k < nRigidMarkers; k++) {
++ printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k],
++ markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]);
+ }
+
+ // Mean marker error
+@@ -366,25 +365,26 @@ void natnet_parse(unsigned char *in) {
+ printf_natnet("Mean marker error: %3.2f\n", fError);
+
+ // Release resources
+- if(markerIDs)
++ if (markerIDs) {
+ free(markerIDs);
+- if(markerSizes)
++ }
++ if (markerSizes) {
+ free(markerSizes);
+- if(markerData)
++ }
++ if (markerData) {
+ free(markerData);
++ }
+ } // next rigid body
+ } // next skeleton
+ }
+
+ // ========== LABELED MARKERS ==========
+ // Labeled markers (version 2.3 and later)
+- if( ((natnet_major == 2)&&(natnet_minor>=3)) || (natnet_major>2))
+- {
++ if (((natnet_major == 2) && (natnet_minor >= 3)) || (natnet_major > 2)) {
+ int nLabeledMarkers = 0;
+ memcpy(&nLabeledMarkers, ptr, 4); ptr += 4;
+ printf_natnet("Labeled Marker Count : %d\n", nLabeledMarkers);
+- for (j=0; j < nLabeledMarkers; j++)
+- {
++ for (j = 0; j < nLabeledMarkers; j++) {
+ int ID = 0; memcpy(&ID, ptr, 4); ptr += 4;
+ float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
+ float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
+@@ -392,7 +392,7 @@ void natnet_parse(unsigned char *in) {
+ float size = 0.0f; memcpy(&size, ptr, 4); ptr += 4;
+
+ printf_natnet("ID : %d\n", ID);
+- printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x,y,z);
++ printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x, y, z);
+ printf_natnet("size: [%3.2f]\n", size);
+ }
+ }
+@@ -409,45 +409,47 @@ void natnet_parse(unsigned char *in) {
+ // End of data tag
+ int eod = 0; memcpy(&eod, ptr, 4); ptr += 4;
+ printf_natnet("End Packet\n-------------\n");
+- }
+- else
+- {
++ } else {
+ printf("Error: Unrecognized packet type from Optitrack NatNet.\n");
+ }
+ }
+
+ /** The transmitter periodic function */
+-gboolean timeout_transmit_callback(gpointer data) {
++gboolean timeout_transmit_callback(gpointer data)
++{
+ int i;
+
+ // Loop trough all the available rigidbodies (TODO: optimize)
+- for(i = 0; i < MAX_RIGIDBODIES; i++) {
++ for (i = 0; i < MAX_RIGIDBODIES; i++) {
+ // Check if ID's are correct
+- if(rigidBodies[i].id >= MAX_RIGIDBODIES) {
+- fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1);
++ if (rigidBodies[i].id >= MAX_RIGIDBODIES) {
++ fprintf(stderr,
++ "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n",
++ rigidBodies[i].id, MAX_RIGIDBODIES - 1);
+ exit(EXIT_FAILURE);
+ }
+
+ // Check if we want to transmit (follow) this rigid
+- if(aircrafts[rigidBodies[i].id].ac_id == 0)
++ if (aircrafts[rigidBodies[i].id].ac_id == 0) {
+ continue;
++ }
+
+ // When we don track anymore and timeout or start tracking
+- if(rigidBodies[i].nSamples < 1
+- && aircrafts[rigidBodies[i].id].connected
+- && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
++ if (rigidBodies[i].nSamples < 1
++ && aircrafts[rigidBodies[i].id].connected
++ && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) {
+ aircrafts[rigidBodies[i].id].connected = FALSE;
+ fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
+- rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
+- }
+- else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
++ rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
++ } else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
+ fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
+- rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
++ rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
+ }
+
+ // Check if we still track the rigid
+- if(rigidBodies[i].nSamples < 1)
++ if (rigidBodies[i].nSamples < 1) {
+ continue;
++ }
+
+ // Update the last tracked
+ aircrafts[rigidBodies[i].id].connected = TRUE;
+@@ -466,15 +468,15 @@ gboolean timeout_transmit_callback(gpointer data) {
+ pos.z = rigidBodies[i].z;
+
+ // Convert the position to ecef and lla based on the Optitrack LTP
+- ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos);
++ ecef_of_enu_point_d(&ecef_pos , &tracking_ltp , &pos);
+ lla_of_ecef_d(&lla_pos, &ecef_pos);
+
+ // Check if we have enough samples to estimate the velocity
+ rigidBodies[i].nVelocityTransmit++;
+- if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
++ if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
+ // Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples)
+ double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) /
+- ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
++ ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit);
+ rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time;
+ rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time;
+ rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
+@@ -485,7 +487,7 @@ gboolean timeout_transmit_callback(gpointer data) {
+ speed.z = rigidBodies[i].vel_z;
+
+ // Conver the speed to ecef based on the Optitrack LTP
+- ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed);
++ ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel , &tracking_ltp , &speed);
+ }
+
+ // Copy the quaternions and convert to euler angles for the heading
+@@ -496,62 +498,128 @@ gboolean timeout_transmit_callback(gpointer data) {
+ double_eulers_of_quat(&orient_eulers, &orient);
+
+ // Calculate the heading by adding the Natnet offset angle and normalizing it
+- double heading = -orient_eulers.psi+90.0/57.6 - tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
++ double heading = -orient_eulers.psi + 90.0 / 57.6 -
++ tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
+ NormRadAngle(heading);
+
+- printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id
+- , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
++ printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id,
++ aircrafts[rigidBodies[i].id].ac_id
++ , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
+ printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
+- rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
+- rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
++ rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
++ rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z);
+
+ // Transmit the REMOTE_GPS packet on the ivy bus (either small or big)
+- if(small_packets) {
+- /* The GPS messages are most likely too large to be send over either the datalink
+- * The local position is an int32 and the 10 LSBs of the x and y axis are compressed into
++ if (small_packets) {
++ /* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into
+ * a single integer. The z axis is considered unsigned and only the latter 10 LSBs are
+ * used.
+ */
+- uint32_t pos_xyz = (((uint32_t)(pos.x*100.0)) & 0x3FF) << 22; // bits 31-22 x position in cm
+- pos_xyz |= (((uint32_t)(pos.y*100.0)) & 0x3FF) << 12; // bits 21-12 y position in cm
+- pos_xyz |= (((uint32_t)(pos.z*100.0)) & 0x3FF) << 2; // bits 11-2 z position in cm
+- // bits 1 and 0 are free
+
++ uint32_t pos_xyz;
++ // check if position within limits
++ if (fabs(pos.x * 100) < pow(2, 10)) {
++ pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
++ } else {
++ fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x);
++ pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm
++ }
++
++ if (fabs(pos.y * 100) < pow(2, 10)) {
++ pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
++ } else {
++ fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y);
++ pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm
++ }
++
++ if (pos.z * 100 < pow(2, 10)) {
++ pos_xyz |= (((uint32_t)(pos.z * 100.0)) & 0x3FF); // bits 9-0 z position in cm
++ } else {
++ fprintf(stderr,"Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z);
++ pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm
++ }
+ // printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z);
+
+- uint32_t speed_xy = (((uint32_t)(speed.x*100.0)) & 0x3FF) << 22; // bits 31-22 speed x in cm/s
+- speed_xy |= (((uint32_t)(speed.y*100.0)) & 0x3FF) << 12; // bits 21-12 speed y in cm/s
+- speed_xy |= (((uint32_t)(heading*100.0)) & 0x3FF) << 2; // bits 11-2 heading in rad*1e2 (The heading is already subsampled)
+- // bits 1 and 0 are free
++ if (log_exists == 0) {
++ fprintf(stderr,"Open natnet_log.dat for writing.\n");
++ fp = fopen("natnet_log.dat", "a");
++ log_exists = 1;
++ }
++ if (fp == NULL) {
++ fprintf(stderr, "I couldn't open natnet_log.dat for writing.\n");
++ exit(0);
++ }
++
++ /* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into
++ * a single integer.
++ */
++ uint32_t speed_xyz;
++ // check if speed within limits
++ if (fabs(speed.x * 100) < pow(2, 10)) {
++ speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
++ } else {
++ fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x);
++ speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
++ }
++
++ if (fabs(speed.y * 100) < pow(2, 10)) {
++ speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
++ } else {
++ fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y);
++ speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
++ }
++
++ if (fabs(pos.z * 100) < pow(2, 9)) {
++ speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s
++ } else {
++ fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z);
++ speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s
++ }
+
+ // printf("ENU Vel: %u (%.2f, %.2f, 0.0)\n", speed_xy, speed.x, speed.y);
+
+ // printf("Heading: %.2f\n", heading);
+
+- IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte)
+- (uint8_t)rigidBodies[i].nMarkers, // status (1 byte)
+- pos_xyz, //uint32 ENU X, Y and Z in CM (4 bytes)
+- speed_xy); //uint32 ENU velocity X, Y in cm/s and heading in rad*1e2 (4 bytes)
+- }
+- else {
++ IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte)
++ (uint8_t)rigidBodies[i].nMarkers, // uint8 Number of markers (sv_num) (1 byte)
++ pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes)
++ speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes)
++ (int16_t)(heading * 10000)); // int6_t heading in rad*1e4 (2 bytes)
++
++ struct timeval cur_time;
++ gettimeofday(&cur_time, NULL);
++ fprintf(fp, "%d %d %d %d %d %d %d %d %d %d %d %d %d\n", aircrafts[rigidBodies[i].id].ac_id,
++ rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
++ (int)(pos.x * 100.0), //int32 X in CM
++ (int)(pos.y * 100.0), //int32 Y in CM
++ (int)(pos.z * 100.0), //int32 Z in CM
++ (int)(speed.x * 100.0), //int32 ECEF velocity X in cm/s
++ (int)(speed.y * 100.0), //int32 ECEF velocity Y in cm/s
++ (int)(speed.z * 100.0), //int32 ECEF velocity Z in cm/s
++ (int)(orient_eulers.phi * 10000000.0), //int32 Course in rad*1e7
++ (int)(orient_eulers.theta * 10000000.0), //int32 Course in rad*1e7
++ (int)((-orient_eulers.psi + 90.0 / 57.6) * 10000000.0), //int32 Course in rad*1e7
++ (int)cur_time.tv_sec,
++ (int)cur_time.tv_usec);
++ } else {
+ IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
+- rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
+- (int)(ecef_pos.x*100.0), //int32 ECEF X in CM
+- (int)(ecef_pos.y*100.0), //int32 ECEF Y in CM
+- (int)(ecef_pos.z*100.0), //int32 ECEF Z in CM
+- (int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7
+- (int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7
+- (int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid
+- (int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm
+- (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s
+- (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s
+- (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s
+- 0,
+- (int)(heading*10000000.0)); //int32 Course in rad*1e7
++ rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
++ (int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
++ (int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM
++ (int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM
++ (int)(lla_pos.lat * 10000000.0), //int32 LLA latitude in rad*1e7
++ (int)(lla_pos.lon * 10000000.0), //int32 LLA longitude in rad*1e7
++ (int)(rigidBodies[i].z * 1000.0), //int32 LLA altitude in mm above elipsoid
++ (int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm
++ (int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in m/s
++ (int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in m/s
++ (int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in m/s
++ 0,
++ (int)(heading * 10000000.0)); //int32 Course in rad*1e7
+ }
+
+ // Reset the velocity differentiator if we calculated the velocity
+- if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
++ if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
+ rigidBodies[i].vel_x = 0;
+ rigidBodies[i].vel_y = 0;
+ rigidBodies[i].vel_z = 0;
+@@ -567,7 +635,8 @@ gboolean timeout_transmit_callback(gpointer data) {
+ }
+
+ /** The NatNet sampler periodic function */
+-static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
++static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
++{
+ static unsigned char buffer_data[MAX_PACKETSIZE];
+ static int bytes_data = 0;
+
+@@ -575,7 +644,7 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
+ bytes_data += udp_socket_recv(&natnet_data, buffer_data, MAX_PACKETSIZE);
+
+ // Parse NatNet data
+- if(bytes_data >= 2 && bytes_data >= buffer_data[1]) {
++ if (bytes_data >= 2 && bytes_data >= buffer_data[1]) {
+ natnet_parse(buffer_data);
+ bytes_data = 0;
+ }
+@@ -585,7 +654,8 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
+
+
+ /** Print the program help */
+-void print_help(char* filename) {
++void print_help(char *filename)
++{
+ static const char *usage =
+ "Usage: %s [options]\n"
+ " Options :\n"
+@@ -613,8 +683,9 @@ void print_help(char* filename) {
+ }
+
+ /** Check the amount of arguments */
+-void check_argcount(int argc, char** argv, int i, int expected) {
+- if(i+expected >= argc) {
++void check_argcount(int argc, char **argv, int i, int expected)
++{
++ if (i + expected >= argc) {
+ fprintf(stderr, "Option %s expected %d arguments\r\n\r\n", argv[i], expected);
+ print_help(argv[0]);
+ exit(0);
+@@ -622,30 +693,31 @@ void check_argcount(int argc, char** argv, int i, int expected) {
+ }
+
+ /** Parse the options from the commandline */
+-static void parse_options(int argc, char** argv) {
++static void parse_options(int argc, char **argv)
++{
+ int i, count_ac = 0;
+- for(i = 1; i < argc; ++i) {
++ for (i = 1; i < argc; ++i) {
+
+ // Print help
+- if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
++ if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) {
+ print_help(argv[0]);
+ exit(0);
+ }
+ // Set the verbosity level
+- if(strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) {
++ if (strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ verbose = atoi(argv[++i]);
+ }
+
+ // Set an rigid body to ivy ac_id
+- else if(strcmp(argv[i], "-ac") == 0) {
++ else if (strcmp(argv[i], "-ac") == 0) {
+ check_argcount(argc, argv, i, 2);
+
+ int rigid_id = atoi(argv[++i]);
+ uint8_t ac_id = atoi(argv[++i]);
+
+- if(rigid_id >= MAX_RIGIDBODIES) {
++ if (rigid_id >= MAX_RIGIDBODIES) {
+ fprintf(stderr, "Rigid body ID must be less then %d (MAX_RIGIDBODIES)\n\n", MAX_RIGIDBODIES);
+ print_help(argv[0]);
+ exit(EXIT_FAILURE);
+@@ -655,19 +727,19 @@ static void parse_options(int argc, char** argv) {
+ }
+
+ // Set the NatNet multicast address
+- else if(strcmp(argv[i], "-multicast_addr") == 0) {
++ else if (strcmp(argv[i], "-multicast_addr") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ natnet_multicast_addr = argv[++i];
+ }
+ // Set the NatNet server ip address
+- else if(strcmp(argv[i], "-server") == 0) {
++ else if (strcmp(argv[i], "-server") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ natnet_addr = argv[++i];
+ }
+ // Set the NatNet server version
+- else if(strcmp(argv[i], "-version") == 0) {
++ else if (strcmp(argv[i], "-version") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ float version = atof(argv[++i]);
+@@ -675,13 +747,13 @@ static void parse_options(int argc, char** argv) {
+ natnet_minor = (uint8_t)(version * 10.0) % 10;
+ }
+ // Set the NatNet server data port
+- else if(strcmp(argv[i], "-data_port") == 0) {
++ else if (strcmp(argv[i], "-data_port") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ natnet_data_port = atoi(argv[++i]);
+ }
+ // Set the NatNet server command port
+- else if(strcmp(argv[i], "-cmd_port") == 0) {
++ else if (strcmp(argv[i], "-cmd_port") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ natnet_cmd_port = atoi(argv[++i]);
+@@ -710,31 +782,31 @@ static void parse_options(int argc, char** argv) {
+ ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
+ }
+ // Set the tracking system offset angle in degrees
+- else if(strcmp(argv[i], "-offset_angle") == 0) {
++ else if (strcmp(argv[i], "-offset_angle") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ tracking_offset_angle = atof(argv[++i]);
+ }
+
+ // Set the transmit frequency
+- else if(strcmp(argv[i], "-tf") == 0) {
++ else if (strcmp(argv[i], "-tf") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ freq_transmit = atoi(argv[++i]);
+ }
+ // Set the minimum amount of velocity samples for the differentiator
+- else if(strcmp(argv[i], "-vel_samples") == 0) {
++ else if (strcmp(argv[i], "-vel_samples") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ min_velocity_samples = atoi(argv[++i]);
+ }
+ // Set to use small packets
+- else if(strcmp(argv[i], "-small") == 0) {
++ else if (strcmp(argv[i], "-small") == 0) {
+ small_packets = TRUE;
+ }
+
+ // Set the ivy bus
+- else if(strcmp(argv[i], "-ivy_bus") == 0) {
++ else if (strcmp(argv[i], "-ivy_bus") == 0) {
+ check_argcount(argc, argv, i, 1);
+
+ ivy_bus = argv[++i];
+@@ -749,14 +821,14 @@ static void parse_options(int argc, char** argv) {
+ }
+
+ // Check if at least one aircraft is set
+- if(count_ac < 1) {
++ if (count_ac < 1) {
+ fprintf(stderr, "You must specify at least one aircraft (-ac )\n\n");
+ print_help(argv[0]);
+ exit(EXIT_FAILURE);
+ }
+ }
+
+-int main(int argc, char** argv)
++int main(int argc, char **argv)
+ {
+ // Set the default tracking system position and angle
+ struct EcefCoor_d tracking_ecef;
+@@ -768,10 +840,12 @@ int main(int argc, char** argv)
+
+ // Parse the options from cmdline
+ parse_options(argc, argv);
+- printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
++ printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat),
++ DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
+
+ // Create the network connections
+- printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
++ printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n",
++ natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
+ udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving
+ udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);
+ udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB
+@@ -787,8 +861,8 @@ int main(int argc, char** argv)
+
+ // Create the main timers
+ printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n",
+- freq_transmit, min_velocity_samples);
+- g_timeout_add(1000/freq_transmit, timeout_transmit_callback, NULL);
++ freq_transmit, min_velocity_samples);
++ g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL);
+
+ GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd);
+ g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP,
+diff --git a/sw/tools/bluegiga_usb_dongle/main.c b/sw/tools/bluegiga_usb_dongle/main.c
+index e098263..458ac0f 100644
+--- a/sw/tools/bluegiga_usb_dongle/main.c
++++ b/sw/tools/bluegiga_usb_dongle/main.c
+@@ -134,6 +134,7 @@ FUNCTION ANALYSIS:
+
+ // uncomment the following line to show outgoing/incoming BGAPI packet data
+ //#define DEBUG
++#define DEBUG_BROADCAST
+
+ // timeout for serial port read operations
+ #define UART_TIMEOUT 1000
+@@ -166,7 +167,7 @@ int count[8] = {0, 0, 0, 0, 0, 0, 0, 0}, send_count[8] = {0, 0, 0, 0, 0, 0, 0, 0
+ int rssi_count = 0;
+ int last0 = 0, last1 = 0;
+
+-uint8_t connection_interval = 10; // connection interval in ms
++float connection_interval = 10.; // connection interval in ms
+
+ int ac_id[8] = { -1, -1, -1, -1, -1, -1, -1, -1};
+
+@@ -182,6 +183,7 @@ unsigned int send_extract_idx[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ int connected_devices = 0;
+ //bd_addr found_devices[MAX_DEVICES];
+ int connected[] = {0, 0, 0, 0, 0, 0, 0, 0};
++bd_addr connected_addr[MAX_DEVICES];
+
+ int connect_all = 0;
+ uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00};
+@@ -190,6 +192,8 @@ uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00};
+
+ // {0x00,0x00,0x2d,0x80,0x07,0x00}; // beginning of all modules adresses
+
++FILE* rssi_fp = NULL;
++
+ // list all possible pending actions
+ enum actions {
+ action_none,
+@@ -197,7 +201,8 @@ enum actions {
+ action_connect,
+ action_info,
+ action_connect_all,
+- action_broadcast
++ action_broadcast,
++ action_broadcast_connect
+ };
+ enum actions action = action_none;
+
+@@ -313,6 +318,19 @@ void print_bdaddr(bd_addr bdaddr)
+ }
+
+ /**
++ * Copy Bluetooth MAC address in hexadecimal notation
++ *
++ * @param dst Bluetooth MAC address, destination
++ * @param src Bluetooth MAC address, source
++ */
++void cpy_bdaddr(uint8_t* dst, const uint8_t* src)
++{
++ uint8_t i = 0;
++ for(i = 0; i < 6; i++)
++ dst[i] = src[i];
++}
++
++/**
+ * Display raw BGAPI packet in hexadecimal notation
+ *
+ * @param data1 Fixed-length portion of BGAPI packet (should always be bytes long)
+@@ -344,7 +362,7 @@ void send_api_packet(uint8 len1, uint8 *data1, uint16 len2, uint8 *data2)
+ {
+ #ifdef DEBUG
+ // display outgoing BGAPI packet
+- print_raw_packet((struct ble_header *)data1, data2, 1);
++// print_raw_packet((struct ble_header *)data1, data2);
+ #endif
+ if (uart_tx(len1, data1) || uart_tx(len2, data2)) {
+ // uart_tx returns non-zero on failure
+@@ -386,7 +404,7 @@ int read_api_packet(int timeout_ms)
+
+ #ifdef DEBUG
+ // display incoming BGAPI packet
+- print_raw_packet(&hdr, data, 0);
++ print_raw_packet(&hdr, data);
+ #endif
+
+ if (!msg) {
+@@ -446,26 +464,41 @@ void ble_rsp_system_get_info(const struct ble_msg_system_get_info_rsp_t *msg)
+ */
+ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg)
+ {
+- if (action == action_broadcast) {
+- fprintf(stderr, "advertisement from: ");
+- print_bdaddr(msg->sender);
+- fprintf(stderr, " data: ");
+- int i;
+- for (i = 0; i < msg->data.len; i++) {
++
++#ifdef DEBUG_BROADCAST
++ if(cmp_addr(msg->sender.addr, MAC_ADDR) >= 3 )// && msg->sender.addr[0] == 0xdf)
++ {
++ gettimeofday(&tm, NULL); //Time zone struct is obsolete, hence NULL
++ mytime = (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0;
++ fprintf(stderr, "%f %x %d, ", mytime, msg->sender.addr[0], msg->rssi);
++ uint8_t i = 0;
++ for(i = 0; i < msg->data.len; i++)
+ fprintf(stderr, "%02x ", msg->data.data[i]);
+- }
+ fprintf(stderr, "\n");
++ }
++#endif
++
++ if(rssi_fp)
++ {
++ fprintf(rssi_fp, "%f %d %d\n", mytime, msg->sender.addr[0], msg->rssi);
++ fflush(rssi_fp);
++ }
++
++ if (action == action_broadcast) {
+ if (sock[0])
+- sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT,
++ sendto(sock[0], msg->data.data+13, msg->data.len-13, MSG_DONTWAIT,
+ (struct sockaddr *)&send_addr[0], sizeof(struct sockaddr));
+- } else {
++ //printf("first: %02x, last: %02x\n", msg->data.data[13], msg->data.data[msg->data.len]);
++ }
++
++ if (action != action_broadcast) {
+ uint8_t i, j;
+ char *name = NULL;
+
+ // Check if this device already found, if not add to the list
+
+ if (!connect_all) {
+- fprintf(stderr, "New device found: ");
++ //fprintf(stderr, "New device found: ");
+
+ // Parse data
+ for (i = 0; i < msg->data.len;) {
+@@ -535,7 +568,15 @@ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg
+ }
+
+ // automatically connect if responding device has appropriate mac address header
+- if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) {
++ // check if bluegiga drone and connectable
++ if (connect_all && msg->packet_type == 0 && cmp_addr(msg->sender.addr, MAC_ADDR) >= 3) {
++ uint8 i = 0;
++ while(i++ < MAX_DEVICES)
++ {
++ if (!cmp_addr(msg->sender.addr, connected_addr[i].addr))
++ return;
++ }
++
+ fprintf(stderr, "Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr, "\n");
+ //change_state(state_connecting);
+ // connection interval unit 1.25ms
+@@ -575,6 +616,7 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg
+ // Connection request completed
+ else if (msg->flags & connection_completed) {
+ if (msg->connection + 1 > connected_devices) { connected_devices++; }
++ cpy_bdaddr(connected_addr[msg->connection].addr, msg->address.addr);
+ //change_state(state_connected);
+ connection_interval = msg->conn_interval * 1.25;
+ fprintf(stderr, "Connected, nr: %d, connection interval: %d = %fms\n", msg->connection, msg->conn_interval,
+@@ -612,13 +654,16 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg
+ change_state(state_listening_measurements);
+ enable_indications(msg->connection, drone_handle_configuration);
+ if (connect_all) {
+- ble_cmd_gap_discover(gap_discover_generic);
++ ble_cmd_gap_discover(gap_discover_generic);
+ }
+ }
+ // Find primary services
+ else {
+ change_state(state_finding_services);
+ ble_cmd_attclient_read_by_group_type(msg->connection, FIRST_HANDLE, LAST_HANDLE, 2, primary_service_uuid);
++ if (connect_all) {
++ ble_cmd_gap_discover(gap_discover_generic);
++ }
+ }
+ }
+ }
+@@ -677,9 +722,6 @@ void ble_evt_attclient_procedure_completed(const struct ble_msg_attclient_proced
+ else {
+ change_state(state_listening_measurements);
+ enable_indications(msg->connection, drone_handle_configuration);
+- if (connect_all) {
+- ble_cmd_gap_discover(gap_discover_generic);
+- }
+ }
+ }
+
+@@ -708,6 +750,13 @@ void ble_evt_attclient_find_information_found(const struct ble_msg_attclient_fin
+ drone_handle_measurement = msg->chrhandle;
+ } else if (uuid == DRONE_BROADCAST_UUID) {
+ drone_handle_broadcast = msg->chrhandle;
++
++ // Handle for Drone Data configuration already known
++ if (action == action_broadcast_connect) {
++ unsigned char var[] = {1};
++ printf("Request sent: %d\n", var[0]);
++ ble_cmd_attclient_attribute_write(msg->connection, drone_handle_broadcast, 1, &var);
++ }
+ }
+ }
+ }
+@@ -749,6 +798,8 @@ void ble_evt_attclient_attribute_value(const struct ble_msg_attclient_attribute_
+ if (sock[msg->connection])
+ sendto(sock[msg->connection], msg->value.data, msg->value.len, MSG_DONTWAIT,
+ (struct sockaddr *)&send_addr[msg->connection], sizeof(struct sockaddr));
++ //printf("%02x %02x %02x %02x\n", msg->value.data[0], msg->value.data[1], msg->value.data[2], msg->value.data[3]);
++ //printf("%d\n", msg->value.len);
+
+ }
+
+@@ -824,12 +875,19 @@ void *send_msg()
+ // fprintf(stderr,"Long msg: %d, buff size: %d\n", bt_msg_len, diff);
+ //ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]);
+
+- ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, &data_buf[device][extract_idx[device]]);
+- extract_idx[device] = (extract_idx[device] + bt_msg_len) % BUF_SIZE;
++ uint16_t i = 0;
++ unsigned char buf[19];
++ for (i = 0; i < bt_msg_len; i++)
++ {
++ buf[i] = data_buf[device][extract_idx[device]];
++ extract_idx[device] = (extract_idx[device] + 1) % BUF_SIZE;
++ }
++
++ ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, buf);
+ }
+ device++;
+ } // next device
+- usleep(connection_interval * 1000); // send messages at max intervals of the connection interval
++ usleep(connection_interval * 1000*2); // send messages at max intervals of the connection interval, 2 safety factor
+ } // repeat
+ }
+ pthread_exit(NULL);
+@@ -858,18 +916,21 @@ void *recv_paparazzi_comms()
+ if (connected[device] && sock[device]) {
+ bytes_recv = recvfrom(sock[device], recv_data, BUF_SIZE, MSG_DONTWAIT, (struct sockaddr *)&rec_addr[device],
+ (socklen_t *)&sin_size);
+- if (bytes_recv > 0) { // TODO: can overtake extract!
++ // ensure we don't overtake reading
++ if (bytes_recv > 0 && bytes_recv - 1 <= (extract_idx[device] - insert_idx[device] - 1 + BUF_SIZE) % BUF_SIZE ) {
++ uint16_t i = 0;
++ for (i = 0; i < bytes_recv; i++){
++ data_buf[device][insert_idx[device]] = recv_data[i];
++ insert_idx[device] = (insert_idx[device] + 1) % BUF_SIZE;
++ }
+ send_count[device] += bytes_recv;
+- memcpy(&data_buf[device][insert_idx[device]], recv_data, bytes_recv);
+-
+- insert_idx[device] = (insert_idx[device] + bytes_recv) % BUF_SIZE;
+ }
+ }
+ device++;
+ }
+ }
+ }
+- usleep(20000); // assuming connection interval 10ms, give a bit of overhead
++ usleep(connection_interval * 1000 * 2); // assuming connection interval 10ms, give a bit of overhead
+ }
+ pthread_exit(NULL);
+ }
+@@ -918,7 +979,7 @@ int main(int argc, char *argv[])
+ action = action_scan;
+ } else if (strcmp(argv[CLARG_ACTION], "info") == 0) {
+ action = action_info;
+- } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0) {
++ } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0 || strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) {
+ action = action_broadcast;
+ if (argc > CLARG_ACTION + 2) {
+ send_port = atoi(argv[CLARG_ACTION + 1]);
+@@ -927,6 +988,10 @@ int main(int argc, char *argv[])
+ usage(argv[0]);
+ return 1;
+ }
++ if (strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) {
++ connect_all = 1;
++ action = action_broadcast_connect;
++ }
+ } else if (strcmp(argv[CLARG_ACTION], "all") == 0) {
+ connect_all = 1;
+ action = action_scan;
+@@ -968,6 +1033,23 @@ int main(int argc, char *argv[])
+ return 1;
+ }
+
++ size_t i = 0;
++ for (i = 0; i < argc; i++)
++ {
++ if(strcmp(argv[i],"log") == 0){
++ time_t timev;
++ time(&timev);
++ char timedate[256];
++ strftime(timedate, 256, "var/logs/%Y%m%d_%H%M%S.rssilog", localtime(&timev));
++ rssi_fp = fopen(timedate, "w");
++ if (!rssi_fp)
++ {
++ fprintf(stderr,"Unable to open file for logging: %s\n Make sure to run from paparazzi home\n", timedate);
++ return -1;
++ }
++ }
++ }
++
+ // set BGLib output function pointer to "send_api_packet" function
+ bglib_output = send_api_packet;
+
+@@ -1003,8 +1085,12 @@ int main(int argc, char *argv[])
+ // get the mac address of the dongle
+ ble_cmd_system_address_get();
+
++ // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1)
++ if (action == action_broadcast)
++ ble_cmd_gap_set_adv_parameters(0x20, 0x28, 0x07);
++
+ // Execute action
+- if (action == action_scan) {
++ if (action == action_scan || action == action_broadcast || action == action_broadcast_connect) {
+ ble_cmd_gap_discover(gap_discover_generic);
+ } else if (action == action_info) {
+ ble_cmd_system_get_info();
+@@ -1012,9 +1098,6 @@ int main(int argc, char *argv[])
+ fprintf(stderr, "Trying to connect\n");
+ change_state(state_connecting);
+ ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 6, 16, 100, 0);
+- } else if (action == action_broadcast) {
+- ble_cmd_gap_set_adv_parameters(0x200, 0x200,
+- 0x07); // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1)
+ }
+
+ pthread_create(&threads[0], NULL, send_msg, NULL);
+@@ -1032,4 +1115,7 @@ int main(int argc, char *argv[])
+ uart_close();
+
+ pthread_exit(NULL);
++
++ if (rssi_fp)
++ fclose(rssi_fp);
+ }
+diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml
+index 16a918c..e95218e 100644
+--- a/sw/tools/generators/gen_messages.ml
++++ b/sw/tools/generators/gen_messages.ml
+@@ -288,8 +288,8 @@ module Gen_onboard = struct
+ (** Converts bytes into the required type *)
+ let typed = fun o pprz_type -> (* o for offset *)
+ let size = pprz_type.Pprz.size in
+- if check_alignment && o mod (min size 4) <> 0 then
+- failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name);
++ (**if check_alignment && o mod (min size 4) <> 0 then
++ failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); *)
+
+ match size with
+ 1 -> sprintf "(%s)(*((uint8_t*)_payload+%d))" pprz_type.Pprz.inttype o
+@@ -327,8 +327,8 @@ module Gen_onboard = struct
+
+ (** The macro to access to the array itself *)
+ let pprz_type = Syntax.assoc_types t in
+- if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then
+- failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name);
++ (**if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then
++ failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name);*)
+
+ fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset;
+ offset := -1 (** Mark for no more fields *)
+@@ -337,8 +337,8 @@ module Gen_onboard = struct
+ fprintf h "#define DL_%s_%s_length(_payload) (%d)\n" msg_name field_name len;
+ (** The macro to access to the array itself *)
+ let pprz_type = Syntax.assoc_types t in
+- if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then
+- failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name);
++ (**if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then
++ failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name);*)
+
+ fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset;
+ offset := !offset + (pprz_type.Pprz.size*len)
+@@ -387,7 +387,7 @@ let () =
+ end;
+
+ (** Macros for airborne datalink (receiving) *)
+- if class_name = "datalink" || class_name = "intermcu" then
++ if class_name = "datalink" || class_name = "intermcu" || class_name = "telemetry" then
+ List.iter (Gen_onboard.print_get_macros h true) messages;
+
+ Printf.fprintf h "#endif // _VAR_MESSAGES_%s_H_\n" class_name
+diff --git a/sw/tools/generators/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml
+index b45e12d..d61520d 100644
+--- a/sw/tools/generators/gen_periodic.ml
++++ b/sw/tools/generators/gen_periodic.ml
+@@ -182,6 +182,8 @@ let print_process_send = fun out_h xml freq modules ->
+ let modes = Xml.children process in
+
+ fprintf out_h "\n/* Periodic telemetry (type %s): %s process */\n" telem_type process_name;
++ let nb_modes = List.length modes in
++ fprintf out_h "#define NB_TELEMETRY_MODES %s\n" (string_of_int nb_modes);
+ let p_id = ref 0 in
+ Xml2h.define (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id);
+ incr p_id;
diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
index a0d7b83727..415d302999 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.c
+++ b/sw/airborne/subsystems/gps/gps_datalink.c
@@ -98,7 +98,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
- // todo check correct
gps.ned_vel.x = enu_speed.x;
gps.ned_vel.y = enu_speed.y;
gps.ned_vel.z = -enu_speed.z;
@@ -111,7 +110,7 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = num_sv;
- gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // todo check
+ gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick);
gps.fix = GPS_FIX_3D; // set 3D fix to true
gps_available = TRUE; // set GPS available to true