diff --git a/diff.diff b/diff.diff
deleted file mode 100644
index 5de33f7296..0000000000
--- a/diff.diff
+++ /dev/null
@@ -1,2699 +0,0 @@
-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;