mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
add new module for formation flight and clean old files
This commit is contained in:
Gautier Hattenberger
@@ -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
|
||||
ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
|
||||
ap.srcs += traffic_info.c formation.c snav.c
|
||||
# traffic
|
||||
ap.CFLAGS += -DTRAFFIC_INFO
|
||||
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.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
|
||||
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 snav.c tcas.c
|
||||
|
||||
</makefile>
|
||||
</airframe>
|
||||
|
||||
@@ -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
|
||||
ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
|
||||
ap.srcs += traffic_info.c formation.c snav.c
|
||||
#traffic
|
||||
ap.CFLAGS += -DTRAFFIC_INFO
|
||||
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.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
|
||||
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 snav.c tcas.c
|
||||
|
||||
</makefile>
|
||||
</airframe>
|
||||
|
||||
@@ -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
|
||||
ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
|
||||
ap.srcs += traffic_info.c formation.c snav.c
|
||||
# traffic
|
||||
ap.CFLAGS += -DTRAFFIC_INFO
|
||||
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.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
|
||||
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 snav.c tcas.c
|
||||
|
||||
</makefile>
|
||||
</airframe>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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 */
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
Reference in New Issue
Block a user