add new module for formation flight and clean old files

conf/airframes/mmlaas_N1.xml

@@ -2,6 +2,10 @@
 
 <airframe name="MiniMag Laas N1 Tiny 0.99">
 
+  <modules>
+    <load name="formation_flight.xml"/>
+  </modules>
+
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE"   no="0" min="1200" neutral="1200" max="2000"/>
@@ -79,7 +83,7 @@
   </section>
 
   <section name="BAT">
-    <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8600"/>
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
   </section>
  
@@ -189,7 +193,7 @@
   </section>
 
   <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+    <define name="YAW_RESPONSE_FACTOR" value="1."/>
   </section>
 
  <makefile>
@@ -231,9 +235,9 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
 # ap.CFLAGS += -DUSE_GPIO
 # ap.srcs += $(SRC_ARCH)/gpio.c
 
-# formation
+# traffic
-ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
+ap.CFLAGS += -DTRAFFIC_INFO
-ap.srcs += traffic_info.c formation.c snav.c
+ap.srcs += traffic_info.c snav.c
 
 # external module
 ap.srcs += external.c
@@ -246,11 +250,14 @@ ap.srcs += tcas.c
 
 #ap.CFLAGS += -DHITL
 
+ap.CFLAGS += -DUSE_MODULES
+sim.CFLAGS += -DUSE_MODULES
+
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
+sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO -DTCAS
-sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
+sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c snav.c tcas.c
 
   </makefile>
 </airframe>

conf/airframes/mmlaas_N2.xml

@@ -2,6 +2,10 @@
 
 <airframe name="MiniMag Laas N2 Tiny 1.1">
 
+  <modules>
+    <load name="formation_flight.xml"/>
+  </modules>
+
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE"   no="0" min="1200" neutral="1200" max="2000"/>
@@ -80,7 +84,7 @@
   </section>
 
   <section name="BAT">
-    <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8600"/>
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
   </section>
  
@@ -190,15 +194,18 @@
   </section>
 
   <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+    <define name="YAW_RESPONSE_FACTOR" value="1."/>
   </section>
 
  <makefile>
+
+CONFIG=\"tiny_1_1.h\"
+
 include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
 
 FLASH_MODE=IAP
 
-ap.CFLAGS +=  -DFBW -DAP -DBOARD_CONFIG=\"tiny_1_1.h\" -DLED -DTIME_LED=1
+ap.CFLAGS +=  -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED -DTIME_LED=1
 ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
 
 ap.srcs += commands.c
@@ -228,9 +235,9 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
 
-#formation
+#traffic
-ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
+ap.CFLAGS += -DTRAFFIC_INFO
-ap.srcs += traffic_info.c formation.c snav.c
+ap.srcs += traffic_info.c snav.c
 
 # external module
 ap.srcs += external.c
@@ -244,11 +251,13 @@ ap.srcs += tcas.c
 
 #ap.CFLAGS += -DHITL
 
+ap.CFLAGS += -DUSE_MODULES
+sim.CFLAGS += -DUSE_MODULES
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
+sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO -DTCAS
-sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
+sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c snav.c tcas.c
 
   </makefile>
 </airframe>

conf/airframes/mmlaas_N3.xml

@@ -2,6 +2,10 @@
 
 <airframe name="MiniMag Laas N3 Tiny 1.1">
 
+  <modules>
+    <load name="formation_flight.xml"/>
+  </modules>
+
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE"   no="0" min="1200" neutral="1200" max="2000"/>
@@ -80,7 +84,7 @@
   </section>
 
   <section name="BAT">
-    <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8600"/>
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
   </section>
  
@@ -190,7 +194,7 @@
   </section>
 
   <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+    <define name="YAW_RESPONSE_FACTOR" value="1."/>
   </section>
 
  <makefile>
@@ -228,9 +232,9 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
 
-# formation
+# traffic
-ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
+ap.CFLAGS += -DTRAFFIC_INFO
-ap.srcs += traffic_info.c formation.c snav.c
+ap.srcs += traffic_info.c snav.c
 
 # external module
 ap.srcs += external.c
@@ -244,11 +248,14 @@ ap.srcs += tcas.c
 
 #ap.CFLAGS += -DHITL
 
+ap.CFLAGS += -DUSE_MODULES
+sim.CFLAGS += -DUSE_MODULES
+
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
+sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO -DTCAS
-sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
+sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c snav.c tcas.c
 
   </makefile>
 </airframe>

conf/modules/formation_flight.xml

@@ -0,0 +1,14 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="multi">
+  <header>
+    <file name="formation.h"/>
+  </header>
+  <init fun="formation_init()"/>
+  <datalink message="FORMATION_STATUS" fun="ParseFormationStatus()"/>
+  <datalink message="FORMATION_SLOT" fun="ParseFormationSlot()"/>
+  <makefile>
+    <file name="formation.c"/>
+  </makefile>
+</module>
+

sw/airborne/datalink.c

@@ -41,10 +41,6 @@
 #include "traffic_info.h"
 #endif // TRAFFIC_INFO
 
-#ifdef FORMATION
-#include "formation.h"
-#endif // FORMATION
-
 #ifdef USE_JOYSTICK
 #include "joystick.h"
 #endif
@@ -153,24 +149,6 @@ void dl_parse_msg(void) {
     DOWNLINK_SEND_DL_VALUE(&i, &val);
   } else
 #endif /** Else there is no dl_settings section in the flight plan */
-#ifdef FORMATION
-	if (msg_id == DL_FORMATION_SLOT) {
-    uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer);
-    uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer);
-    float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
-    float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
-    float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
-    UpdateSlot(ac_id, slot_east, slot_north, slot_alt);
-    if (ac_id == leader_id) form_mode = mode;
-  } else if (msg_id == DL_FORMATION_STATUS) {
-    uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer);
-    uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
-    uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
-    if (ac_id == AC_ID) leader_id = leader;
-    else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); }
-    else { UpdateFormationStatus(ac_id,UNSET); }
-  } else
-#endif
 #ifdef USE_JOYSTICK
     if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
       JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),

sw/airborne/ins.h

@@ -1,99 +0,0 @@
-/*
- * $Id$
- *  
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * 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, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/** \file ins.h
- *  \brief Device independent INS code
- *
-*/
-
-
-#ifndef INS_H
-#define INS_H
-
-#include "std.h"
-
-#ifndef INS_FORMAT
-#define INS_FORMAT float
-#endif
-
-extern INS_FORMAT ins_x;
-extern INS_FORMAT ins_y;
-extern INS_FORMAT ins_z;
-
-extern INS_FORMAT ins_vx;
-extern INS_FORMAT ins_vy;
-extern INS_FORMAT ins_vz;
-
-extern INS_FORMAT ins_phi;
-extern INS_FORMAT ins_theta;
-extern INS_FORMAT ins_psi;
-
-extern INS_FORMAT ins_p;
-extern INS_FORMAT ins_q;
-extern INS_FORMAT ins_r;
-
-extern INS_FORMAT ins_ax;
-extern INS_FORMAT ins_ay;
-extern INS_FORMAT ins_az;
-
-extern INS_FORMAT ins_mx;
-extern INS_FORMAT ins_my;
-extern INS_FORMAT ins_mz;
-
-extern volatile uint8_t ins_msg_received;
-
-extern void ins_init( void );
-extern void ins_periodic_task( void );
-void parse_ins_msg( void );
-void parse_ins_buffer( uint8_t );
-
-#ifndef SITL
-#include "uart.h"
-
-#define __InsLink(dev, _x) dev##_x
-#define _InsLink(dev, _x)  __InsLink(dev, _x)
-#define InsLink(_x) _InsLink(INS_LINK, _x)
-
-#define InsBuffer() InsLink(ChAvailable())
-#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
-#define InsUartSend1(c) InsLink(Transmit(c))
-#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
-#define InsUartRunning InsLink(TxRunning)
-
-#endif /** !SITL */
-
-#define InsEventCheckAndHandle(handler) { \
-  if (InsBuffer()) {				  \
-    ReadInsBuffer();				  \
-  }						                \
-  if (ins_msg_received) {			\
-    LED_TOGGLE(2); \
-    parse_ins_msg();          \
-    handler();                \
-    ins_msg_received = FALSE; \
-  }						                \
-}
-
-
-#endif /* INS_H */

sw/airborne/ins_xsens.c

@@ -1,346 +0,0 @@
-/*
- * Paparazzi mcu0 $Id$
- *  
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * 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, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/** \file xsens.c
- * \brief Parser for the Xsens protocol
- */
-
-#include "ins.h"
-
-#include <inttypes.h>
-
-#include "airframe.h"
-#include "led.h"
-#include "gps.h"
-
-#include "downlink.h"
-#include "messages.h"
-
-#include "latlong.h"
-
-INS_FORMAT ins_x;
-INS_FORMAT ins_y;
-INS_FORMAT ins_z;
-
-INS_FORMAT ins_vx;
-INS_FORMAT ins_vy;
-INS_FORMAT ins_vz;
-
-INS_FORMAT ins_phi;
-INS_FORMAT ins_theta;
-INS_FORMAT ins_psi;
-
-INS_FORMAT ins_p;
-INS_FORMAT ins_q;
-INS_FORMAT ins_r;
-
-INS_FORMAT ins_ax;
-INS_FORMAT ins_ay;
-INS_FORMAT ins_az;
-
-INS_FORMAT ins_mx;
-INS_FORMAT ins_my;
-INS_FORMAT ins_mz;
-
-volatile uint8_t ins_msg_received;
-
-#define XsensInitCheksum() { send_ck = 0; }
-#define XsensUpdateChecksum(c) { send_ck += c; }
-
-#define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }
-#define XsensSend1ByAddr(x) { XsensSend1(*x); }
-#define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
-#define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
-
-#define XsensHeader(msg_id, len) { \
-  InsUartSend1(XSENS_START); \
-  XsensInitCheksum(); \
-  XsensSend1(XSENS_BID); \
-  XsensSend1(msg_id); \
-  XsensSend1(len); \
-}
-#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
-
-/** Includes macros generated from xsens_MTi-G.xml */ 
-#include "xsens_protocol.h"
-
- 
-#define XSENS_MAX_PAYLOAD 254
-uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
-
-/* output mode : calibrated, orientation, position, velocity, status */
-#ifndef XSENS_OUTPUT_MODE
-#define XSENS_OUTPUT_MODE 0x0836
-#endif
-/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED */
-#ifndef XSENS_OUTPUT_SETTINGS
-#define XSENS_OUTPUT_SETTINGS 0x80000C05
-#endif
-
-#define UNINIT        0
-#define GOT_START     1
-#define GOT_BID       2
-#define GOT_MID       3
-#define GOT_LEN       4
-#define GOT_DATA      5
-#define GOT_CHECKSUM  6
-
-uint8_t xsens_errorcode;
-uint8_t xsens_msg_status;
-uint16_t xsens_time_stamp;
-uint16_t xsens_output_mode;
-uint32_t xsens_output_settings;
-
-static uint8_t xsens_id;
-static uint8_t xsens_status;
-static uint8_t xsens_len;
-static uint8_t xsens_msg_idx;
-static uint8_t ck;
-uint8_t send_ck;
-
-void ins_init( void ) {
-
-  xsens_status = UNINIT;
-
-  xsens_msg_status = 0;
-  xsens_time_stamp = 0;
-  xsens_output_mode = XSENS_OUTPUT_MODE;
-  xsens_output_settings = XSENS_OUTPUT_SETTINGS;
-  /* send mode and settings to MT */
-  XSENS_GoToConfig();
-  XSENS_SetOutputMode(xsens_output_mode);
-  XSENS_SetOutputSettings(xsens_output_settings);
-  //XSENS_GoToMeasurment();
-}
-
-void ins_periodic_task( void ) {
-  RunOnceEvery(100,XSENS_ReqGPSStatus());
-}
-
-void parse_ins_msg( void ) {
-  uint8_t offset = 0;
-  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
-    xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
-  }
-  else if (xsens_id == XSENS_ReqOutputSettings_ID) {
-    xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
-  }
-  else if (xsens_id == XSENS_Error_ID) {
-    xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
-  }
-  else if (xsens_id == XSENS_GPSStatus_ID) {
-    gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
-    uint8_t i;
-    for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) {
-      uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
-      if (ch > GPS_NB_CHANNELS) continue;
-      gps_svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
-      gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
-      gps_svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
-      gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
-    }
-  }
-  else if (xsens_id == XSENS_MTData_ID) {
-    /* test RAW modes else calibrated modes */
-    //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
-      if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
-        ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
-        ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
-        ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
-        offset += XSENS_DATA_RAWInertial_LENGTH;
-      }
-      if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
-        gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset);
-        gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
-        gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
-        /* Set the real UTM zone */
-        gps_utm_zone = (gps_lon/1e7+180) / 6 + 1;
-        latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), gps_utm_zone);
-        /* utm */
-        gps_utm_east = latlong_utm_x * 100;
-        gps_utm_north = latlong_utm_y * 100;
-        ins_x = latlong_utm_x;
-        ins_y = latlong_utm_y;
-        gps_alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 10;
-        ins_z = -(INS_FORMAT)gps_alt / 100.;
-        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
-        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
-        ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
-        gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
-        gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset);
-        gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset);
-        offset += XSENS_DATA_RAWGPS_LENGTH;
-      }
-    //} else {
-      if (XSENS_MASK_Temp(xsens_output_mode)) {
-        offset += XSENS_DATA_Temp_LENGTH;
-      }
-      if (XSENS_MASK_Calibrated(xsens_output_mode)) {
-        uint8_t l = 0;
-        if (!XSENS_MASK_AccOut(xsens_output_settings)) {
-          ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
-          ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
-          ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
-          l++;
-        }
-        if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
-          ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
-          ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
-          ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
-          l++;
-        }
-        if (!XSENS_MASK_MagOut(xsens_output_settings)) {
-          ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
-          ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
-          ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
-          l++;
-        }
-        offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
-      }
-      if (XSENS_MASK_Orientation(xsens_output_mode)) {
-        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
-          float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
-          float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
-          float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
-          float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
-          float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
-          float dcm01 =       2 * (q1*q2 + q0*q3);
-          float dcm02 =       2 * (q1*q3 - q0*q2);
-          float dcm12 =       2 * (q2*q3 + q0*q1);
-          float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
-          ins_phi   = atan2(dcm12, dcm22);
-          ins_theta = -asin(dcm02);
-          ins_psi   = atan2(dcm01, dcm00);
-          gps_course = ins_psi * 1800 / M_PI;
-          offset += XSENS_DATA_Quaternion_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
-          ins_phi   = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
-          ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
-          ins_psi   = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
-          gps_course = (int16_t)(XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * 10);
-          offset += XSENS_DATA_Euler_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
-          offset += XSENS_DATA_Matrix_LENGTH;
-        }
-      }
-      if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
-        uint8_t l = 0;
-        if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
-          l++;
-        }
-        if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
-          l++;
-        }
-        offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
-      }
-      if (XSENS_MASK_Position(xsens_output_mode)) {
-        float lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset);
-        float lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset);
-        gps_lat = (int32_t)(lat * 1e7);
-        gps_lon = (int32_t)(lon * 1e7);
-        gps_utm_zone = (lon+180) / 6 + 1;
-        latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), gps_utm_zone);
-        ins_x = latlong_utm_x;
-        ins_y = latlong_utm_y;
-        gps_utm_east  = ins_x * 100;
-        gps_utm_north = ins_y * 100;
-        ins_z = -XSENS_DATA_Position_alt(xsens_msg_buf,offset);
-#ifndef USE_VFILTER
-        gps_alt = ins_z * 100;
-#endif
-        offset += XSENS_DATA_Position_LENGTH;
-      }
-      if (XSENS_MASK_Velocity(xsens_output_mode)) {
-        ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
-        ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
-        ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
-#ifndef USE_VFILTER
-        gps_climb = (int16_t)(-ins_vz * 100);
-#endif
-        gps_gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
-        offset += XSENS_DATA_Velocity_LENGTH;
-      }
-      if (XSENS_MASK_Status(xsens_output_mode)) {
-        xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
-        if (bit_is_set(xsens_msg_status,2)) gps_mode = 0x03; // gps fix
-        else gps_mode = 0;
-        offset += XSENS_DATA_Status_LENGTH;
-      }
-      if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
-        xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
-        gps_itow = xsens_time_stamp;
-        offset += XSENS_DATA_TimeStamp_LENGTH;
-      }
-    //}
-  }
-
-}
-
-
-void parse_ins_buffer( uint8_t c ) {
-  ck += c;
-  switch (xsens_status) {
-  case UNINIT:
-    if (c != XSENS_START)
-      goto error;
-    xsens_status++;
-    ck = 0;
-    break;
-  case GOT_START:
-    if (c != XSENS_BID)
-      goto error;
-    xsens_status++;
-    break;
-  case GOT_BID:
-    xsens_id = c;
-    xsens_status++;
-    break;
-  case GOT_MID:
-    xsens_len = c;
-    if (xsens_len > XSENS_MAX_PAYLOAD)
-      goto error;
-    xsens_msg_idx = 0;
-    xsens_status++;
-    break;
-  case GOT_LEN:
-    xsens_msg_buf[xsens_msg_idx] = c;
-    xsens_msg_idx++;
-    if (xsens_msg_idx >= xsens_len)
-      xsens_status++;
-    break;
-  case GOT_DATA:
-    if (ck != 0)
-      goto error;
-    ins_msg_received = TRUE;
-    goto restart;
-    break;
-  }
-  return;
- error:  
- restart:
-  xsens_status = UNINIT;
-  return;
-}

sw/airborne/main_ap.c

@@ -115,10 +115,6 @@
 #include "traffic_info.h"
 #endif
 
-#ifdef FORMATION
-#include "formation.h"
-#endif
-
 #ifdef TCAS
 #include "tcas.h"
 #endif
@@ -751,10 +747,6 @@ void init_ap( void ) {
   traffic_info_init();
 #endif
 
-#ifdef FORMATION
-  formation_init();
-#endif
-
 #ifdef TCAS
   tcas_init();
 #endif

sw/airborne/modules/multi/formation.h

@@ -41,6 +41,25 @@ extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
 
 #define UpdateFormationStatus(_id,_status) { formation[the_acs_id[_id]].status = _status; }
 
+#define ParseFormationStatus() { \
+  uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer); \
+  uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer); \
+  uint8_t status = DL_FORMATION_STATUS_status(dl_buffer); \
+  if (ac_id == AC_ID) leader_id = leader; \
+  else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); } \
+  else { UpdateFormationStatus(ac_id,UNSET); } \
+}
+
+#define ParseFormationSlot() { \
+  uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer); \
+  uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer); \
+  float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer); \
+  float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer); \
+  float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer); \
+  UpdateSlot(ac_id, slot_east, slot_north, slot_alt); \
+  if (ac_id == leader_id) form_mode = mode; \
+}
+
 extern int start_formation(void);
 
 extern int stop_formation(void);