Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge pull request #1349 from paparazzi/piksi

Browse Source 
Update Piksi driver
- better handling of the the baseline messages
- use the libsbp as a submodule
- compute the base "position" from the global position and the local baseline
- send the `BASE_POS` message to module so he can produce pseudo-global DGPS position
- send the antenna frontend mode via sbp message
- add DGPS and RTK fix modes to GPS
- fix uart write on linux
- add GPS_INJECT message
- add sbp2ivy
This commit is contained in:
Felix Ruess
2015-09-13 10:43:35 +02:00
parent 0a0b119c8f 648c00ddfa
commit f5f30aef40
36 changed files with 550 additions and 1210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -146,6 +146,7 @@ paparazzi.sublime-workspace
/sw/ground_segment/misc/davis2ivy
/sw/ground_segment/misc/kestrel2ivy
/sw/ground_segment/misc/natnet2ivy
/sw/ground_segment/misc/sbp2ivy
/sw/ground_segment/misc/video_synchronizer
# /sw/airborne/arch/lpc21/test/bootloader
.gitmodules
+3
View File
@@ -13,3 +13,6 @@
[submodule "sw/ext/libzbar"]
path = sw/ext/libzbar
url = https://github.com/paparazzi/libzbar
[submodule "sw/ext/libsbp"]
path = sw/ext/libsbp
url = https://github.com/paparazzi/libsbp.git
Makefile
+2
View File
@@ -172,6 +172,8 @@ ext:
#
subdirs: $(SUBDIRS)
$(MISC): ext
$(SUBDIRS):
$(MAKE) -C $@
conf/firmwares/subsystems/fixedwing/gps_piksi.makefile
+3 -3
View File
@@ -19,9 +19,9 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c
# libswiftnav
ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libswiftnav/include
ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libswiftnav/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libswiftnav/src/edc.c
# libsbp
ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include
ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile
+3 -3
View File
@@ -19,9 +19,9 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c
# libswiftnav
ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libswiftnav/include
ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libswiftnav/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libswiftnav/src/edc.c
# libsbp
ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include
ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
conf/messages.xml
+9 -3
View File
@@ -1348,7 +1348,7 @@
<field name="tow" type="uint32"/>
<field name="pdop" type="uint16"/>
<field name="numsv" type="uint8"/>
<field name="fix" type="uint8" values="NONE|UKN1|UKN2|3D"/>
<field name="fix" type="uint8" values="NONE|NA|2D|3D|DGPS|RTK"/>
</message>
<message name="AHRS_EULER_INT" id="156">
@@ -1989,7 +1989,7 @@
<field name="motor_nb_err" type="uint8"/>
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<field name="frame_rate" type="uint8" unit="Hz"/>
<field name="gps_status" type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
<field name="gps_status" type="uint8" values="NO_FIX|NA|2D|3D|DGPS|RTK"/>
<field name="ap_mode" type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE"/>
<field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
<field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
@@ -2534,6 +2534,12 @@
<field name="pan" type="int8"/>
</message>
<message name="GPS_INJECT" id="153" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="packet_id" type="uint8"/>
<field name="data" type="uint8[]"/>
</message>
</msg_class>
@@ -2611,7 +2617,7 @@
<field name="lat_mode" type="string" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
<field name="horiz_mode" type="string" values="WAYPOINT|ROUTE|CIRCLE"/>
<field name="gaz_mode" type="string" values="MANUAL|THROTTLE|CLIMB|ALT"/>
<field name="gps_mode" type="string" values="NOFIX|DRO|2D|3D|GPSDRO"/>
<field name="gps_mode" type="string" values="NOFIX|NA|2D|3D|DGPS|RTK"/>
<field name="kill_mode" type="string" values="OFF|ON"/>
<field name="flight_time" type="uint32" />
<field name="state_filter_mode" type="string" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
sw/airborne/arch/linux/mcu_periph/uart_arch.c
+15 -11
View File
@@ -48,8 +48,8 @@ static void uart_receive_handler(struct uart_periph *periph);
static void *uart_thread(void *data __attribute__((unused)));
static pthread_mutex_t uart_mutex = PTHREAD_MUTEX_INITIALIZER;
//#define TRACE(fmt,args...) fprintf(stderr, fmt, args)
#define TRACE(fmt,args...)
#define TRACE(fmt,args...) fprintf(stderr, fmt, args)
//#define TRACE(fmt,args...)
void uart_arch_init(void)
{
@@ -245,7 +245,11 @@ void uart_put_byte(struct uart_periph *periph, uint8_t data)
/* write single byte to serial port */
struct SerialPort *port = (struct SerialPort *)(periph->reg_addr);
int ret = write((int)(port->fd), &data, 1);
int ret = 0;
do{
ret = write((int)(port->fd), &data, 1);
} while(ret < 1 && errno == EAGAIN); //FIXME: max retry
if (ret < 1) {
TRACE("uart_put_byte: write %d failed [%d: %s]\n", data, ret, strerror(errno));
@@ -273,7 +277,7 @@ static void uart_receive_handler(struct uart_periph *periph)
periph->rx_insert_idx = temp; // update insert index
}
else {
TRACE("uart_receive_handler: rx_buf full! discarding received byte: %x %c", c, c);
TRACE("uart_receive_handler: rx_buf full! discarding received byte: %x %c\n", c, c);
}
}
pthread_mutex_unlock(&uart_mutex);
@@ -303,7 +307,7 @@ uint16_t uart_char_available(struct uart_periph *p)
void uart0_init(void)
{
uart_periph_init(&uart0);
strncpy(uart0.dev, UART0_DEV, UART_DEV_NAME_SIZE);
strncpy(uart0.dev, STRINGIFY(UART0_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart0, UART0_BAUD);
}
#endif /* USE_UART0 */
@@ -312,7 +316,7 @@ void uart0_init(void)
void uart1_init(void)
{
uart_periph_init(&uart1);
strncpy(uart1.dev, UART1_DEV, UART_DEV_NAME_SIZE);
strncpy(uart1.dev, STRINGIFY(UART1_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart1, UART1_BAUD);
}
#endif /* USE_UART1 */
@@ -321,7 +325,7 @@ void uart1_init(void)
void uart2_init(void)
{
uart_periph_init(&uart2);
strncpy(uart2.dev, UART2_DEV, UART_DEV_NAME_SIZE);
strncpy(uart2.dev, STRINGIFY(UART2_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart2, UART2_BAUD);
}
#endif /* USE_UART2 */
@@ -330,7 +334,7 @@ void uart2_init(void)
void uart3_init(void)
{
uart_periph_init(&uart3);
strncpy(uart3.dev, UART3_DEV, UART_DEV_NAME_SIZE);
strncpy(uart3.dev, STRINGIFY(UART3_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart3, UART3_BAUD);
}
#endif /* USE_UART3 */
@@ -339,7 +343,7 @@ void uart3_init(void)
void uart4_init(void)
{
uart_periph_init(&uart4);
strncpy(uart4.dev, UART4_DEV, UART_DEV_NAME_SIZE);
strncpy(uart4.dev, STRINGIFY(UART4_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart4, UART4_BAUD);
}
#endif /* USE_UART4 */
@@ -348,7 +352,7 @@ void uart4_init(void)
void uart5_init(void)
{
uart_periph_init(&uart5);
strncpy(uart5.dev, UART5_DEV, UART_DEV_NAME_SIZE);
strncpy(uart5.dev, STRINGIFY(UART5_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart5, UART5_BAUD);
}
#endif /* USE_UART5 */
@@ -357,7 +361,7 @@ void uart5_init(void)
void uart6_init(void)
{
uart_periph_init(&uart6);
strncpy(uart6.dev, UART6_DEV, UART_DEV_NAME_SIZE);
strncpy(uart6.dev, STRINGIFY(UART6_DEV), UART_DEV_NAME_SIZE);
uart_periph_set_baudrate(&uart6, UART6_BAUD);
}
#endif /* USE_UART6 */
sw/airborne/boards/ardrone2.h
+1 -1
View File
@@ -4,7 +4,7 @@
#define BOARD_ARDRONE2
#ifndef UART1_DEV
#define UART1_DEV "/dev/ttyUSB0"
#define UART1_DEV /dev/ttyUSB0
#endif
/* Default actuators driver */
sw/airborne/boards/bebop.h
+1 -1
View File
@@ -26,7 +26,7 @@
#define BOARD_BEBOP
/** uart connected to GPS internally */
#define UART1_DEV "/dev/ttyPA1"
#define UART1_DEV /dev/ttyPA1
/* Default actuators driver */
#define DEFAULT_ACTUATORS "boards/bebop/actuators.h"
sw/airborne/firmwares/rotorcraft/datalink.c
+13
View File
@@ -164,6 +164,19 @@ void dl_parse_msg(void)
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
break;
#endif
#if USE_GPS
case DL_GPS_INJECT :
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
break;
#endif
default:
break;
sw/airborne/math/pprz_geodetic_int.h
+2
View File
@@ -130,6 +130,8 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st
#define M_OF_MM(_mm) ((_mm)/1e3)
#define EM7RAD_OF_RAD(_r) ((_r)*1e7)
#define RAD_OF_EM7RAD(_r) ((_r)/1e7)
#define EM7DEG_OF_DEG(_r) ((_r)*1e7)
#define DEG_OF_EM7DEG(_r) ((_r)/1e7)
#define EM7DEG_OF_RAD(_r) (DegOfRad(_r)*1e7)
#define RAD_OF_EM7DEG(_r) (RadOfDeg(_r)/1e7)
sw/airborne/modules/geo_mag/geo_mag.c
+1 -1
View File
@@ -51,7 +51,7 @@ void geo_mag_init(void)
void geo_mag_periodic(void)
{
//FIXME: kill_throttle has no place in a geomag module
if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle) {
if (!geo_mag.ready && GpsFixValid() && kill_throttle) {
geo_mag.calc_once = TRUE;
}
}
sw/airborne/modules/ins/ahrs_chimu_spi.c
+1 -1
View File
@@ -147,7 +147,7 @@ void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d)
float gps_speed = 0;
if (gps_fix == GPS_FIX_3D) {
if (GpsFixValid()) {
gps_speed = gps_speed_3d / 100.;
}
gps_speed = FloatSwap(gps_speed);
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+2 -2
View File
@@ -401,7 +401,7 @@ void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag)
void ahrs_fc_update_gps(struct GpsState *gps_s)
{
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
if (gps_s->fix == GPS_FIX_3D) {
if (gps_s->fix >= GPS_FIX_3D) {
ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.;
ahrs_fc.ltp_vel_norm_valid = TRUE;
} else {
@@ -411,7 +411,7 @@ void ahrs_fc_update_gps(struct GpsState *gps_s)
#if AHRS_USE_GPS_HEADING && USE_GPS
//got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
if (gps_s->fix == GPS_FIX_3D && gps_s->gspeed >= 500 &&
if (gps_s->fix >= GPS_FIX_3D && gps_s->gspeed >= 500 &&
gps_s->cacc <= RadOfDeg(10 * 1e7)) {
// gps_s->course is in rad * 1e7, we need it in rad
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+1 -1
View File
@@ -177,7 +177,7 @@ void ahrs_dcm_update_gps(struct GpsState *gps_s)
static float last_gps_speed_3d = 0;
#if USE_GPS
if (gps_s->fix == GPS_FIX_3D) {
if (gps_s->fix >= GPS_FIX_3D) {
ahrs_dcm.gps_age = 0;
ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+2 -2
View File
@@ -514,7 +514,7 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
void ahrs_icq_update_gps(struct GpsState *gps_s __attribute__((unused)))
{
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
if (gps_s->fix == GPS_FIX_3D) {
if (gps_s->fix >= GPS_FIX_3D) {
ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps_s->speed_3d / 100.);
ahrs_icq.ltp_vel_norm_valid = TRUE;
} else {
@@ -525,7 +525,7 @@ void ahrs_icq_update_gps(struct GpsState *gps_s __attribute__((unused)))
#if AHRS_USE_GPS_HEADING && USE_GPS
// got a 3d fix, ground speed > AHRS_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
// and course accuracy is better than 10deg
if (gps_s->fix == GPS_FIX_3D &&
if (gps_s->fix >= GPS_FIX_3D &&
gps_s->gspeed >= (AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100) &&
gps_s->cacc <= RadOfDeg(10 * 1e7)) {
sw/airborne/subsystems/gps.c
+8 -1
View File
@@ -76,7 +76,7 @@ static inline void send_svinfo_available(struct transport_tx *trans, struct link
if (i >= gps.nb_channels) { i = 0; }
// send SVINFO for all satellites while no GPS fix,
// after 3D fix, send avialable sats if they were updated
if (gps.fix != GPS_FIX_3D) {
if (gps.fix < GPS_FIX_3D) {
send_svinfo_id(trans, dev, i);
} else if (gps.svinfos[i].cno != last_cnos[i]) {
send_svinfo_id(trans, dev, i);
@@ -191,3 +191,10 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
return itow_now;
}
/**
* Default parser for GPS injected data
*/
void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){
}
sw/airborne/subsystems/gps.h
+10 -13
View File
@@ -38,11 +38,14 @@
#include GPS_TYPE_H
#endif
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GPS_FIX_NONE 0x00 ///< No GPS fix
#define GPS_FIX_2D 0x02 ///< 2D GPS fix
#define GPS_FIX_3D 0x03 ///< 3D GPS fix
#define GPS_FIX_DGPS 0x04 ///< DGPS fix
#define GPS_FIX_RTK 0x05 ///< RTK GPS fix
#define GpsFixValid() (gps.fix == GPS_FIX_3D)
#define GpsFixValid() (gps.fix >= GPS_FIX_3D)
#define GpsIsLost() !GpsFixValid()
#ifndef GPS_NB_CHANNELS
@@ -102,23 +105,17 @@ extern struct GpsState gps;
/** initialize the global GPS state */
extern void gps_init(void);
/* GPS model specific init implementation */
/** GPS model specific init implementation */
extern void gps_impl_init(void);
/** GPS packet injection (default empty) */
extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data);
/** GPS timeout in seconds */
#ifndef GPS_TIMEOUT
#define GPS_TIMEOUT 2
#endif
static inline bool_t GpsIsLost(void)
{
if (gps.fix == GPS_FIX_3D) {
return FALSE;
}
return TRUE;
}
static inline bool_t gps_has_been_good(void)
{
static bool_t gps_had_valid_fix = FALSE;
sw/airborne/subsystems/gps/gps_piksi.c
+187 -176
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
@@ -29,8 +30,7 @@
* https://github.com/swift-nav/sbp_tutorial
*/
#include <sbp.h>
#include <sbp_messages.h>
#include "subsystems/gps/gps_piksi.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
#include "mcu_periph/uart.h"
@@ -41,8 +41,35 @@
#include "generated/flight_plan.h"
#endif
#ifndef USE_PIKSI_ECEF
#define USE_PIKSI_ECEF 1
#include <libsbp/sbp.h>
#include <libsbp/navigation.h>
#include <libsbp/observation.h>
#include <libsbp/tracking.h>
#include <libsbp/settings.h>
#include <libsbp/piksi.h>
/*
* Set the Piksi GPS antenna (default is Patch, internal)
*/
#if USE_PIKSI_EXT_ANTENNA
static const char SBP_ANT_SET[] = "frontend""\x00""antenna_selection""\x00""External";
#elif USE_PIKSI_AUTO_ANTENNA
static const char SBP_ANT_SET[] = "frontend""\x00""antenna_selection""\x00""Auto";
#else
static const char SBP_ANT_SET[] = "frontend""\x00""antenna_selection""\x00""Patch";
#endif
/*
* Set the UART config depending on which UART is connected
*/
#if USE_PIKSI_UARTA
static const char SBP_UART_SET1[] = "uart_uarta""\x00""mode""\x00""SBP";
static const char SBP_UART_SET2[] = "uart_uarta""\x00""sbp_message_mask""\x00""784"; //0x310 which masks all navigation and tracking messages
static const char SBP_UART_SET3[] = "uart_uarta""\x00""configure_telemetry_radio_on_boot""\x00""False";
#else
static const char SBP_UART_SET1[] = "uart_uartb""\x00""mode""\x00""SBP";
static const char SBP_UART_SET2[] = "uart_uartb""\x00""sbp_message_mask""\x00""784"; //0x310 which masks all navigation and tracking messages
static const char SBP_UART_SET3[] = "uart_uartb""\x00""configure_telemetry_radio_on_boot""\x00""False";
#endif
/*
@@ -61,14 +88,13 @@ sbp_msg_callbacks_node_t pos_llh_node;
sbp_msg_callbacks_node_t vel_ned_node;
sbp_msg_callbacks_node_t dops_node;
sbp_msg_callbacks_node_t gps_time_node;
#if USE_PIKSI_BASELINE_ECEF
sbp_msg_callbacks_node_t baseline_ecef_node;
#endif
//#if USE_PIKSI_BASELINE_NED
//sbp_msg_callbacks_node_t baseline_ned_node;
//#endif
sbp_msg_callbacks_node_t tracking_state_node;
sbp_msg_callbacks_node_t tracking_state_dep_a_node;
static void gps_piksi_publish(void);
uint32_t gps_piksi_read(uint8_t *buff, uint32_t n, void *context __attribute__((unused)));
uint32_t gps_piksi_write(uint8_t *buff, uint32_t n, void *context __attribute__((unused)));
/*
* Callback functions to interpret SBP messages.
@@ -80,48 +106,43 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_pos_ecef_t pos_ecef = *(sbp_pos_ecef_t *)msg;
gps.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0);
gps.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0);
gps.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0);
gps.pacc = (uint32_t)(pos_ecef.accuracy);
gps.num_sv = pos_ecef.n_sats;
gps.fix = GPS_FIX_3D;
gps.tow = pos_ecef.tow;
}
static uint8_t last_flags = 0;
msg_pos_ecef_t pos_ecef = *(msg_pos_ecef_t *)msg;
#if USE_PIKSI_BASELINE_ECEF
static void sbp_baseline_ecef_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_baseline_ecef_t baseline_ecef = *(sbp_baseline_ecef_t *)msg;
gps.ecef_pos.x = (int32_t)(baseline_ecef.x / 10);
gps.ecef_pos.y = (int32_t)(baseline_ecef.y / 10);
gps.ecef_pos.z = (int32_t)(baseline_ecef.z / 10);
gps.pacc = (uint32_t)(baseline_ecef.accuracy);
gps.num_sv = baseline_ecef.n_sats;
gps.tow = baseline_ecef.tow;
// Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this)
if(pos_ecef.flags > 0 ){//|| last_flags == 0) {
gps.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0);
gps.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0);
gps.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0);
gps.pacc = (uint32_t)(pos_ecef.accuracy);// FIXME not implemented yet by libswiftnav
gps.num_sv = pos_ecef.n_sats;
gps.tow = pos_ecef.tow;
// High precision solution available
gps_piksi_publish();
if(pos_ecef.flags == 1)
gps.fix = GPS_FIX_RTK;
else if(pos_ecef.flags == 2)
gps.fix = GPS_FIX_DGPS;
else
gps.fix = GPS_FIX_3D;
}
last_flags = pos_ecef.flags;
if(pos_ecef.flags > 0) gps_piksi_publish(); // Only if RTK position
}
#endif
static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_vel_ecef_t vel_ecef = *(sbp_vel_ecef_t *)msg;
msg_vel_ecef_t vel_ecef = *(msg_vel_ecef_t *)msg;
gps.ecef_vel.x = (int32_t)(vel_ecef.x / 10);
gps.ecef_vel.y = (int32_t)(vel_ecef.y / 10);
gps.ecef_vel.z = (int32_t)(vel_ecef.z / 10);
gps.sacc = (uint32_t)(vel_ecef.accuracy);
// Solution available (VEL_ECEF is the last message to be send)
gps_piksi_publish();
gps_piksi_publish(); // TODO: filter out if got RTK position
}
static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)),
@@ -129,57 +150,53 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_pos_llh_t pos_llh = *(sbp_pos_llh_t *)msg;
gps.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7);
gps.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7);
int32_t alt = (int32_t)(pos_llh.height * 1000.);
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
// height is above ellipsoid or MSL according to bit flag (but not both are available)
// 0: above ellipsoid
// 1: above MSL
// we have to get the HMSL from the flight plan for now
if (bit_is_set(pos_llh.flags, 3)) {
gps.hmsl = alt;
gps.lla_pos.alt = alt + NAV_MSL0;
} else {
gps.lla_pos.alt = alt;
gps.hmsl = alt - NAV_MSL0;
}
#else
// but here we fill the two alt with the same value since we don't know HMSL
gps.lla_pos.alt = alt;
gps.hmsl = alt;
#endif
}
static uint8_t last_flags = 0;
msg_pos_llh_t pos_llh = *(msg_pos_llh_t *)msg;
//#if USE_PIKSI_BASELINE_NED
//static void sbp_baseline_ned_callback(uint16_t sender_id __attribute__((unused)),
// uint8_t len __attribute__((unused)),
// uint8_t msg[],
// void *context __attribute__((unused)))
//{
// sbp_baseline_ned_t baseline_ned = *(sbp_baseline_ned_t *)msg;
//}
//#endif
// Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this)
if(pos_llh.flags > 0 || last_flags == 0) {
gps.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7);
gps.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7);
int32_t alt = (int32_t)(pos_llh.height * 1000.);
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
// height is above ellipsoid or MSL according to bit flag (but not both are available)
// 0: above ellipsoid
// 1: above MSL
// we have to get the HMSL from the flight plan for now
if (bit_is_set(pos_llh.flags, 3)) {
gps.hmsl = alt;
gps.lla_pos.alt = alt + NAV_MSL0;
} else {
gps.lla_pos.alt = alt;
gps.hmsl = alt - NAV_MSL0;
}
#else
// but here we fill the two alt with the same value since we don't know HMSL
gps.lla_pos.alt = alt;
gps.hmsl = alt;
#endif
}
last_flags = pos_llh.flags;
}
static void sbp_vel_ned_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_vel_ned_t vel_ned = *(sbp_vel_ned_t *)msg;
msg_vel_ned_t vel_ned = *(msg_vel_ned_t *)msg;
gps.ned_vel.x = (int32_t)(vel_ned.n / 10);
gps.ned_vel.y = (int32_t)(vel_ned.e / 10);
gps.ned_vel.z = (int32_t)(vel_ned.d / 10);
@@ -194,7 +211,7 @@ static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_dops_t dops = *(sbp_dops_t *)msg;
msg_dops_t dops = *(msg_dops_t *)msg;
gps.pdop = dops.pdop;
}
@@ -203,60 +220,89 @@ static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)),
uint8_t msg[],
void *context __attribute__((unused)))
{
sbp_gps_time_t gps_time = *(sbp_gps_time_t *)msg;
msg_gps_time_t gps_time = *(msg_gps_time_t *)msg;
gps.week = gps_time.wn;
gps.tow = gps_time.tow;
}
static void sbp_tracking_state_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len,
uint8_t msg[],
void *context __attribute__((unused)))
{
uint8_t channels_cnt = len/sizeof(tracking_channel_state_t);
msg_tracking_state_t *tracking_state = (msg_tracking_state_t *)msg;
for(uint8_t i = 0; i < channels_cnt; i++) {
if(tracking_state->states[i].state == 1) {
gps.svinfos[i].svid = tracking_state->states[i].sid + 1;
gps.svinfos[i].cno = tracking_state->states[i].cn0;
}
}
}
static void sbp_tracking_state_dep_a_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len,
uint8_t msg[],
void *context __attribute__((unused)))
{
uint8_t channels_cnt = len/sizeof(tracking_channel_state_dep_a_t);
msg_tracking_state_dep_a_t *tracking_state = (msg_tracking_state_dep_a_t *)msg;
for(uint8_t i = 0; i < channels_cnt; i++) {
if(tracking_state->states[i].state == 1) {
gps.svinfos[i].svid = tracking_state->states[i].prn + 1;
gps.svinfos[i].cno = tracking_state->states[i].cn0;
}
}
}
/*
* Initialize the Piksi GPS and write the settings
*/
void gps_impl_init(void)
{
/* Setup SBP nodes */
sbp_state_init(&sbp_state);
/* Register a node and callback, and associate them with a specific message ID. */
sbp_register_callback(&sbp_state, SBP_POS_ECEF, &sbp_pos_ecef_callback, NULL, &pos_ecef_node);
sbp_register_callback(&sbp_state, SBP_VEL_ECEF, &sbp_vel_ecef_callback, NULL, &vel_ecef_node);
sbp_register_callback(&sbp_state, SBP_POS_LLH, &sbp_pos_llh_callback, NULL, &pos_llh_node);
sbp_register_callback(&sbp_state, SBP_VEL_NED, &sbp_vel_ned_callback, NULL, &vel_ned_node);
sbp_register_callback(&sbp_state, SBP_DOPS, &sbp_dops_callback, NULL, &dops_node);
sbp_register_callback(&sbp_state, SBP_GPS_TIME, &sbp_gps_time_callback, NULL, &gps_time_node);
#if USE_PIKSI_BASELINE_ECEF
sbp_register_callback(&sbp_state, SBP_BASELINE_ECEF, &sbp_baseline_ecef_callback, NULL, &baseline_ecef_node);
#endif
//#if USE_PIKSI_BASELINE_NED
// sbp_register_callback(&sbp_state, SBP_BASELINE_NED, &sbp_baseline_ned_callback, NULL, &baseline_ned_node);
//#endif
sbp_register_callback(&sbp_state, SBP_MSG_POS_ECEF, &sbp_pos_ecef_callback, NULL, &pos_ecef_node);
sbp_register_callback(&sbp_state, SBP_MSG_VEL_ECEF, &sbp_vel_ecef_callback, NULL, &vel_ecef_node);
sbp_register_callback(&sbp_state, SBP_MSG_POS_LLH, &sbp_pos_llh_callback, NULL, &pos_llh_node);
sbp_register_callback(&sbp_state, SBP_MSG_VEL_NED, &sbp_vel_ned_callback, NULL, &vel_ned_node);
sbp_register_callback(&sbp_state, SBP_MSG_DOPS, &sbp_dops_callback, NULL, &dops_node);
sbp_register_callback(&sbp_state, SBP_MSG_GPS_TIME, &sbp_gps_time_callback, NULL, &gps_time_node);
sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE, &sbp_tracking_state_callback, NULL, &tracking_state_node);
sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE_DEP_A, &sbp_tracking_state_dep_a_callback, NULL, &tracking_state_dep_a_node);
/* Write settings */
sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_ANT_SET), (u8*)(&SBP_ANT_SET), gps_piksi_write);
sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_UART_SET1), (u8*)(&SBP_UART_SET1), gps_piksi_write);
sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_UART_SET2), (u8*)(&SBP_UART_SET2), gps_piksi_write);
sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_UART_SET3), (u8*)(&SBP_UART_SET3), gps_piksi_write);
sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_SAVE, SBP_SENDER_ID, 0, NULL, gps_piksi_write);
msg_base_pos_t base_pos;
base_pos.lat = 51.991152;
base_pos.lon = 4.378052;
base_pos.height = 50.;
sbp_send_message(&sbp_state, SBP_MSG_BASE_POS, SBP_SENDER_ID, sizeof(msg_base_pos_t), (u8*)(&base_pos), gps_piksi_write);
gps.nb_channels = GPS_NB_CHANNELS;
}
/*
* FIFO to hold received UART bytes before
* libswiftnav SBP submodule parses them.
*/
#define FIFO_LEN 512
char sbp_msg_fifo[FIFO_LEN];
/* FIFO functions */
uint8_t fifo_empty(void);
uint8_t fifo_full(void);
uint8_t fifo_write(char c);
uint8_t fifo_read_char(char *c);
uint32_t fifo_read(uint8_t *buff, uint32_t n, void *context);
/*
* Event function
* Event handler for reading the GPS UART bytes
*/
void gps_piksi_event(void)
{
// fill fifo with new uart bytes
while (uart_char_available(&(GPS_LINK))) {
uint8_t c = uart_getch(&(GPS_LINK));
fifo_write(c);
}
// call sbp event function
sbp_process(&sbp_state, &fifo_read);
if (uart_char_available(&(GPS_LINK)))
sbp_process(&sbp_state, &gps_piksi_read);
}
/*
* Publish the GPS data from the Piksi on the ABI bus
*/
static void gps_piksi_publish(void)
{
// current timestamp
@@ -264,7 +310,7 @@ static void gps_piksi_publish(void)
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
if (gps.fix >= GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
@@ -272,73 +318,38 @@ static void gps_piksi_publish(void)
}
/*
* FIFO implementation
* Read bytes from the Piksi UART connection
* This is a wrapper functions used in the libsbp library
*/
uint16_t head = 0;
uint16_t tail = 0;
/* Return 1 if true, 0 otherwise. */
uint8_t fifo_empty(void)
{
if (head == tail) {
return 1;
}
return 0;
}
/*
* Append a character to our SBP message fifo.
* Returns 1 if char successfully appended to fifo.
* Returns 0 if fifo is full.
*/
uint8_t fifo_write(char c)
{
if (fifo_full()) {
return 0;
}
sbp_msg_fifo[tail] = c;
tail = (tail + 1) % FIFO_LEN;
return 1;
}
/*
* Read 1 char from fifo.
* Returns 0 if fifo is empty, otherwise 1.
*/
uint8_t fifo_read_char(char *c)
{
if (fifo_empty()) {
return 0;
}
*c = sbp_msg_fifo[head];
head = (head + 1) % FIFO_LEN;
return 1;
}
/*
* Read arbitrary number of chars from FIFO. Must conform to
* function definition that is passed to the function
* sbp_process().
* Returns the number of characters successfully read.
*/
uint32_t fifo_read(uint8_t *buff, uint32_t n, void *context __attribute__((unused)))
uint32_t gps_piksi_read(uint8_t *buff, uint32_t n, void *context __attribute__((unused)))
{
uint32_t i;
for (i = 0; i < n; i++) {
if (!fifo_read_char((char *)(buff + i))) {
if (!uart_char_available(&(GPS_LINK)))
break;
}
buff[i] = uart_getch(&(GPS_LINK));
}
return i;
}
/* Return 1 if true, 0 otherwise. */
uint8_t fifo_full(void)
/*
* Write bytes to the Piksi UART connection
* This is a wrapper functions used in the libsbp library
*/
uint32_t gps_piksi_write(uint8_t *buff, uint32_t n, void *context __attribute__((unused)))
{
if (((tail + 1) % FIFO_LEN) == head) {
return 1;
uint32_t i = 0;
for (i = 0; i < n; i++) {
uart_put_byte(&(GPS_LINK), buff[i]);
}
return 0;
return n;
}
/**
* Override the default GPS packet injector to inject the data trough UART
*/
void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
{
sbp_send_message(&sbp_state, packet_id, SBP_SENDER_ID, length, data, gps_piksi_write);
}
sw/airborne/subsystems/gps/gps_piksi.h
+8 -1
View File
@@ -32,7 +32,14 @@
#ifndef GPS_PIKSI_H
#define GPS_PIKSI_H
void gps_piksi_event(void);
#define GPS_NB_CHANNELS 10
extern void gps_piksi_event(void);
/*
* Reset base station position
*/
extern void gps_piksi_set_base_pos(void);
/*
* The GPS event
sw/airborne/subsystems/ins/ins_float_invariant.c
+2 -2
View File
@@ -431,7 +431,7 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a
void ins_float_invariant_update_gps(struct GpsState *gps_s)
{
if (gps_s->fix == GPS_FIX_3D && ins_float_inv.is_aligned) {
if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
ins_gps_fix_once = TRUE;
#if INS_FINV_USE_UTM
@@ -615,7 +615,7 @@ static inline void error_output(struct InsFloatInv *_ins)
// pos and speed error only if GPS data are valid
// or while waiting first GPS data to prevent diverging
if ((gps.fix == GPS_FIX_3D && ins_float_inv.is_aligned
if ((gps.fix >= GPS_FIX_3D && ins_float_inv.is_aligned
#if INS_FINV_USE_UTM
&& state.utm_initialized_f
#else
sw/airborne/subsystems/ins/ins_gps_passthrough.c
+18 -17
View File
@@ -149,24 +149,25 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
if (gps_s->fix == GPS_FIX_3D) {
if (!ins_gp.ltp_initialized) {
ins_reset_local_origin();
}
/* simply scale and copy pos/speed from gps */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
stateSetPositionNed_i(&ins_gp.ltp_pos);
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
stateSetSpeedNed_i(&ins_gp.ltp_speed);
if (gps_s->fix < GPS_FIX_3D) {
return;
}
if (!ins_gp.ltp_initialized) {
ins_reset_local_origin();
}
/* simply scale and copy pos/speed from gps */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
stateSetPositionNed_i(&ins_gp.ltp_pos);
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
stateSetSpeedNed_i(&ins_gp.ltp_speed);
}
void ins_gps_passthrough_register(void)
sw/airborne/subsystems/ins/ins_int.c
+2 -2
View File
@@ -214,7 +214,7 @@ void ins_int_init(void)
void ins_reset_local_origin(void)
{
#if USE_GPS
if (gps.fix == GPS_FIX_3D) {
if (GpsFixValid()) {
ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos);
ins_int.ltp_def.lla.alt = gps.lla_pos.alt;
ins_int.ltp_def.hmsl = gps.hmsl;
@@ -325,7 +325,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
#if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s)
{
if (gps_s->fix != GPS_FIX_3D) {
if (gps_s->fix < GPS_FIX_3D) {
return;
}
sw/ext/Makefile
+3 -1
View File
@@ -33,7 +33,7 @@ EXT_DIR=$(PAPARAZZI_SRC)/sw/ext
include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain
all: libopencm3 luftboot chibios fatfs
all: libopencm3 luftboot chibios fatfs libsbp
# update (and init if needed) all submodules
update_submodules:
@@ -69,6 +69,8 @@ chibios: chibios.update
fatfs: fatfs.update
libsbp: libsbp.update
clean:
$(Q)if [ -f libopencm3/Makefile ]; then \
$(MAKE) -C $(EXT_DIR)/libopencm3 clean; \
sw/ext/libsbp
Submodule
+1
Submodule sw/ext/libsbp added at 1c48447386
sw/ext/libswiftnav/include/common.h
-57
View File
@@ -1,57 +0,0 @@
/*
* Copyright (C) 2012 Swift Navigation Inc.
* Contact: Henry Hallam <henry@swift-nav.com>
* Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_COMMON_H
#define LIBSWIFTNAV_COMMON_H
/** \defgroup common Common definitions
* Common definitions used throughout the library.
* \{ */
#define MIN(x, y) (((x) < (y)) ? (x) : (y))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
/** \defgroup common_inttypes Integer types
* Specified-width integer type definitions for shorter and nicer code.
*
* These should be used in preference to unspecified width types such as
* `int` which can lead to portability issues between different platforms.
* \{ */
/** Signed 8-bit integer. */
typedef int8_t s8;
/** Signed 16-bit integer. */
typedef int16_t s16;
/** Signed 32-bit integer. */
typedef int32_t s32;
/** Signed 64-bit integer. */
typedef int64_t s64;
/** Unsigned 8-bit integer. */
typedef uint8_t u8;
/** Unsigned 16-bit integer. */
typedef uint16_t u16;
/** Unsigned 32-bit integer. */
typedef uint32_t u32;
/** Unsigned 64-bit integer. */
typedef uint64_t u64;
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_COMMON_H */
sw/ext/libswiftnav/include/edc.h
-21
View File
@@ -1,21 +0,0 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_EDC_H
#define LIBSWIFTNAV_EDC_H
#include "common.h"
u16 crc16_ccitt(const u8 *buf, u32 len, u16 crc);
u32 crc24q(const u8 *buf, u32 len, u32 crc);
#endif /* LIBSWIFTNAV_EDC_H */
sw/ext/libswiftnav/include/sbp.h
-84
View File
@@ -1,84 +0,0 @@
/*
* Copyright (C) 2011-2014 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_SBP_H
#define LIBSWIFTNAV_SBP_H
#include "common.h"
/** \addtogroup sbp
* \{ */
/** Return value indicating success. */
#define SBP_OK 0
/** Return value indicating message decoded and callback executed by sbp_process. */
#define SBP_OK_CALLBACK_EXECUTED 1
/** Return value indicating message decoded with no associated callback in sbp_process. */
#define SBP_OK_CALLBACK_UNDEFINED 2
/** Return value indicating an error with the callback (function defined). */
#define SBP_CALLBACK_ERROR -1
/** Return value indicating a CRC error. */
#define SBP_CRC_ERROR -2
/** Return value indicating an error occured whilst sending an SBP message. */
#define SBP_SEND_ERROR -3
/** Return value indicating an error occured because an argument was NULL. */
#define SBP_NULL_ERROR -4
/** SBP callback function prototype definition. */
typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *context);
/** SBP callback node.
* Forms a linked list of callbacks.
* \note Must be statically allocated for use with sbp_register_callback().
*/
typedef struct sbp_msg_callbacks_node {
u16 msg_type; /**< Message ID associated with callback. */
sbp_msg_callback_t cb; /**< Pointer to callback function. */
void *context; /**< Pointer to a context */
struct sbp_msg_callbacks_node *next; /**< Pointer to next node in list. */
} sbp_msg_callbacks_node_t;
/** State structure for processing SBP messages. */
typedef struct {
enum {
WAITING = 0,
GET_TYPE,
GET_SENDER,
GET_LEN,
GET_MSG,
GET_CRC
} state;
u16 msg_type;
u16 sender_id;
u16 crc;
u8 msg_len;
u8 n_read;
u8 msg_buff[256];
void* io_context;
sbp_msg_callbacks_node_t* sbp_msg_callbacks_head;
} sbp_state_t;
/** \} */
s8 sbp_register_callback(sbp_state_t* s, u16 msg_type, sbp_msg_callback_t cb, void* context,
sbp_msg_callbacks_node_t *node);
void sbp_clear_callbacks(sbp_state_t* s);
sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t* s, u16 msg_type);
void sbp_state_init(sbp_state_t *s);
void sbp_state_set_io_context(sbp_state_t *s, void* context);
s8 sbp_process(sbp_state_t *s, u32 (*read)(u8 *buff, u32 n, void* context));
s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
u32 (*write)(u8 *buff, u32 n, void* context));
#endif /* LIBSWIFTNAV_SBP_H */
sw/ext/libswiftnav/include/sbp_messages.h
-182
View File
@@ -1,182 +0,0 @@
/*
* Copyright (C) 2013 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
/*****************************************************************************
* Automatically generated from sbp.yaml with generate.py, do not hand edit! *
*****************************************************************************/
#ifndef LIBSWIFTNAV_SBP_MESSAGES_H
#define LIBSWIFTNAV_SBP_MESSAGES_H
#include "common.h"
/** System start-up message
* The system start-up message is sent once on system start-up. It is
* intended to be used to notify the host or other attached devices that
* the system has started and is now ready to respond to commands or
* configuration requests.
*/
#define SBP_STARTUP 0xFF00
typedef struct __attribute__((packed)) {
u32 reserved; /**< Reserved */
} sbp_startup_t;
/** System heartbeat message
* The heartbeat message is sent periodically to inform the host or
* other attached devices that the system is running. It is intended to
* be used to monitor for system malfunctions and also contains
* status flags that indicate to the host the status of the system and
* if it is operating correctly.
*
* The system error flag is used to indicate that an error has occurred in
* the system. To determine the source of the error the remaining error
* flags should be inspected.
*/
#define SBP_HEARTBEAT 0xFFFF
typedef struct __attribute__((packed)) {
u32 flags; /**< Status flags */
} sbp_heartbeat_t;
/** GPS Time
* GPS Time.
*/
#define SBP_GPS_TIME 0x0100
typedef struct __attribute__((packed)) {
u16 wn; /**< GPS week number [weeks] */
u32 tow; /**< GPS Time of Week rounded to the nearest ms [ms] */
s32 ns; /**< Nanosecond remainder of rounded tow [ns] */
u8 flags; /**< Status flags (reserved) */
} sbp_gps_time_t;
/** Dilution of Precision
* Dilution of Precision.
*/
#define SBP_DOPS 0x0206
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
u16 gdop; /**< Geometric Dilution of Precision [0.01] */
u16 pdop; /**< Position Dilution of Precision [0.01] */
u16 tdop; /**< Time Dilution of Precision [0.01] */
u16 hdop; /**< Horizontal Dilution of Precision [0.01] */
u16 vdop; /**< Vertical Dilution of Precision [0.01] */
} sbp_dops_t;
/** Position in ECEF
* Position solution in absolute Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_POS_ECEF 0x0200
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
double x; /**< ECEF X coordinate [m] */
double y; /**< ECEF Y coordinate [m] */
double z; /**< ECEF Z coordinate [m] */
u16 accuracy; /**< Position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_pos_ecef_t;
/** Geodetic Position
* Geodetic position solution.
*/
#define SBP_POS_LLH 0x0201
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
double lat; /**< Latitude [deg] */
double lon; /**< Longitude [deg] */
double height; /**< Height [m] */
u16 h_accuracy; /**< Horizontal position accuracy estimate [mm] */
u16 v_accuracy; /**< Vertical position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_pos_llh_t;
/** Baseline in ECEF
* Baseline in Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_BASELINE_ECEF 0x0202
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 x; /**< Baseline ECEF X coordinate [mm] */
s32 y; /**< Baseline ECEF Y coordinate [mm] */
s32 z; /**< Baseline ECEF Z coordinate [mm] */
u16 accuracy; /**< Position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_baseline_ecef_t;
/** Baseline in NED
* Baseline in local North East Down (NED) coordinates.
*/
#define SBP_BASELINE_NED 0x0203
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 n; /**< Baseline North coordinate [mm] */
s32 e; /**< Baseline East coordinate [mm] */
s32 d; /**< Baseline Down coordinate [mm] */
u16 h_accuracy; /**< Horizontal position accuracy estimate [mm] */
u16 v_accuracy; /**< Vertical position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_baseline_ned_t;
/** Velocity in ECEF
* Velocity in Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_VEL_ECEF 0x0204
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 x; /**< Velocity ECEF X coordinate [mm/s] */
s32 y; /**< Velocity ECEF Y coordinate [mm/s] */
s32 z; /**< Velocity ECEF Z coordinate [mm/s] */
u16 accuracy; /**< Velocity accuracy estimate [mm/s] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags (reserved) */
} sbp_vel_ecef_t;
/** Velocity in NED
* Velocity in local North East Down (NED) coordinates.
*/
#define SBP_VEL_NED 0x0205
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 n; /**< Velocity North coordinate [mm/s] */
s32 e; /**< Velocity East coordinate [mm/s] */
s32 d; /**< Velocity Down coordinate [mm/s] */
u16 h_accuracy; /**< Horizontal velocity accuracy estimate [mm/s] */
u16 v_accuracy; /**< Vertical velocity accuracy estimate [mm/s] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags (reserved) */
} sbp_vel_ned_t;
#endif /* LIBSWIFTNAV_SBP_MESSAGES_H */
sw/ext/libswiftnav/src/edc.c
-141
View File
@@ -1,141 +0,0 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
/** \defgroup edc Error Detection and Correction
* Error detection and correction functions.
* \{ */
/** \defgroup crc CRC
* Cyclic redundancy checks.
* \{ */
/* CRC16 implementation acording to CCITT standards */
static const u16 crc16tab[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
/** Calculate CCITT 16-bit Cyclical Redundancy Check (CRC16).
*
* This implementation uses parameters used by XMODEM i.e. polynomial is:
* \f[
* x^{16} + x^{12} + x^5 + 1
* \f]
* Mask 0x11021, not reversed, not XOR'd
* (there are several slight variants on the CCITT CRC-16).
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC16 value
*/
u16 crc16_ccitt(const u8 *buf, u32 len, u16 crc)
{
for (u32 i = 0; i < len; i++)
crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
return crc;
}
static const u32 crc24qtab[256] = {
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
*
* The CRC polynomial used is:
* \f[
* x^{24} + x^{23} + x^{18} + x^{17} + x^{14} + x^{11} + x^{10} +
* x^7 + x^6 + x^5 + x^4 + x^3 + x+1
* \f]
* Mask 0x1864CFB, not reversed, not XOR'd
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC-24Q value
*/
u32 crc24q(const u8 *buf, u32 len, u32 crc)
{
for (u32 i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[(crc >> 16) ^ buf[i]];
return crc;
}
/** \} */
/** \} */
sw/ext/libswiftnav/src/sbp.c
-473
View File
@@ -1,473 +0,0 @@
/*
* Copyright (C) 2011-2014 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
#include "sbp.h"
#define SBP_PREAMBLE 0x55
/** \addtogroup io Input / Output
* \{ */
/** \defgroup sbp SBP
* Send and receive messages using Swift Binary Protocol.
*
* Examples
* ========
*
* Receiving
* ---------
*
* First setup a callback for the message you will be receiving. Our callback
* function must have type #sbp_msg_callback_t, i.e. it must be of the form:
*
* ~~~
* void my_callback(u16 sender_id, u8 len, u8 msg[], void *context)
* {
* // Process msg.
* }
* ~~~
*
* You must also statically allocate a #sbp_msg_callbacks_node_t that will be
* used to keep track of the callback function. You do not need to initialize
* it as this will be done by sbp_register_callback().
*
* ~~~
* static sbp_msg_callbacks_node_t my_callback_node;
* ~~~
*
* Now register your callback function with the SBP library as follows:
*
* ~~~
* sbp_register_callback(&sbp_state, SBP_MY_MSG_TYPE, &my_callback, &context, &my_callback_node);
* ~~~
*
* where `SBP_MY_MSG_TYPE` is the numerical identifier of your message type.
*
* You must now call sbp_process() periodically whenever you have received SBP
* data to be processed, e.g. from the serial port. Remember sbp_process() may
* not use all available data so keep calling sbp_process() until all the
* received serial data has been consumed.
*
* sbp_process() stores its internal state in an #sbp_state_t struct which must
* be initialized by calling sbp_state_init() before its first use.
*
* Here is an example based on reading from a typical UART interface:
*
* ~~~
* u32 my_read(u8 *buff, u32 n, void *context)
* {
* for (u32 i=0; i<n; i++) {
* if (uart_has_data())
* buff[i] = uart_read_char();
* else
* break;
* }
* return i;
* }
*
* int main()
* {
* ...
*
* sbp_state_t s;
* sbp_state_init(&s);
*
* while(uart_has_data()) {
* sbp_process(&s, &my_read);
* }
*
* ...
* }
* ~~~
*
* If you're writing C++ code that wants to reference a pointer to
* an object in the my_read function, you can use the context set
* by calling sbp_state_set_io_context()
*
*
* Sending
* -------
*
* To send an SBP message simply call the sbp_send_message() function,
* providing a `write` function that writes data to your output.
*
* Often the data to be sent will simply be a struct cast to a `u8` buffer. As
* a convenience you may want to define a macro that automatically includes
* your write function and calculates the size of the item to be sent.
*
* ~~~
* // Convenience macro for sending an SBP message.
* #define SBP_MSG(sbp_state, msg_type, item) \
* sbp_send_message(&sbp_state, msg_type, MY_SENDER_ID, \
* sizeof(item), (u8 *)&(item), &my_write)
*
* typedef struct {
* u8 x, y;
* } my_awesome_struct;
*
* u32 my_write(u8 *buff, u32 n, void *context)
* {
* for (u32 i=0; i<n; i++) {
* if (uart_write_char(buff[i]) == ERROR)
* break;
* }
* return i;
* }
*
* int main()
* {
* ...
*
* sbp_state_t s;
* sbp_state_init(&s);
*
* my_awesome_struct payload = { 0x22, 0x33 };
*
* sbp_send_message(&s, SBP_MY_MSG_TYPE, MY_SENDER_ID,
* sizeof(payload), (u8*)&payload, &my_write);
*
* // or
*
* SBP_MSG(s, SBP_MY_MSG_TYPE, payload);
*
* ...
* }
* ~~~
*
*
* \{ */
/** Register a callback for a message type.
* Register a callback that is called when a message
* with type msg_type is received.
*
* \param msg_type Message type associated with callback
* \param cb Pointer to message callback function
* \param context Pointer to context for callback function
* \param node Statically allocated #sbp_msg_callbacks_node_t struct
* \return `SBP_OK` (0) if successful, `SBP_CALLBACK_ERROR` if callback was
* already registered for that message type.
*/
s8 sbp_register_callback(sbp_state_t *s, u16 msg_type, sbp_msg_callback_t cb, void *context,
sbp_msg_callbacks_node_t *node)
{
/* Check our callback function pointer isn't NULL. */
if (cb == 0)
return SBP_NULL_ERROR;
/* Check our callback node pointer isn't NULL. */
if (node == 0)
return SBP_NULL_ERROR;
/* Check if callback was already registered for this type. */
if (sbp_find_callback(s, msg_type) != 0)
return SBP_CALLBACK_ERROR;
/* Fill in our new sbp_msg_callback_node_t. */
node->msg_type = msg_type;
node->cb = cb;
node->context = context;
/* The next pointer is set to NULL, i.e. this
* will be the new end of the linked list.
*/
node->next = 0;
/* If our linked list is empty then just
* add the new node to the start.
*/
if (s->sbp_msg_callbacks_head == 0) {
s->sbp_msg_callbacks_head = node;
return SBP_OK;
}
/* Find the tail of our linked list and
* add our new node to the end.
*/
sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
while (p->next)
p = p->next;
p->next = node;
return SBP_OK;
}
/** Clear all registered callbacks.
* This is probably only useful for testing but who knows!
*/
void sbp_clear_callbacks(sbp_state_t *s)
{
/* Reset the head of the callbacks list to NULL. */
s->sbp_msg_callbacks_head = 0;
}
/** Find the callback function associated with a message type.
* Searches through the list of registered callbacks to find the callback
* associated with the passed message type.
*
* \param msg_type Message type to find callback for
* \return Pointer to callback node (#sbp_msg_callbacks_node_t) or `NULL` if
* callback not found for that message type.
*/
sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t *s, u16 msg_type)
{
/* If our list is empty, return NULL. */
if (!s->sbp_msg_callbacks_head)
return 0;
/* Traverse the linked list and return the callback
* function pointer if we find a node with a matching
* message id.
*/
sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
do
if (p->msg_type == msg_type)
return p;
while ((p = p->next));
/* Didn't find a matching callback, return NULL. */
return 0;
}
/** Initialize an #sbp_state_t struct before use.
* This resets the entire state, including all callbacks.
* Remember to use this function to initialize the state before calling
* sbp_process() for the first time.
*
* \param s State structure
*/
void sbp_state_init(sbp_state_t *s)
{
s->state = WAITING;
/* Set the IO context pointer, passed to read and write functions, to NULL. */
s->io_context = 0;
/* Clear the callbacks, if any, currently in s */
sbp_clear_callbacks(s);
}
/** Set a context to pass to all function pointer calls made by sbp functions
* This helper function sets a void* context pointer in sbp_state.
* Whenever `sbp_process` calls the `read` function pointer, it passes this context.
* Whenever `sbp_send_message` calls the `write` function pointer, it passes this context.
* This allows C++ code to get a pointer to an object inside these functions.
*/
void sbp_state_set_io_context(sbp_state_t *s, void *context)
{
s->io_context = context;
}
/** Read and process SBP messages.
* Reads bytes from an input source using the provided `read` function, decodes
* the SBP framing and performs a CRC check on the message.
*
* When an SBP message is successfully received then the list of callbacks is
* searched for a callback corresponding to the received message type. If a
* callback is found then it is called with the ID of the sender, the message
* length and the message payload data buffer as arguments.
*
* \note sbp_process will always call `read` with n > 0
* (aka it will attempt to always read something)
*
* The supplied `read` function must have the prototype:
*
* ~~~
* u32 read(u8 *buff, u32 n, void* context)
* ~~~
*
* where `n` is the number of bytes requested and `buff` is the buffer into
* which to write the received data, and `context` is the arbitrary pointer
* set by `sbp_state_set_io_context`.
* The function should return the number of
* bytes successfully written into `buff` which may be between 0 and `n`
* inclusive, but must never be greater than `n`.
*
* Note that `sbp_process` may not read all available bytes from the `read`
* function so the caller should loop until all bytes available from the input
* source have been consumed.
*
* \param s State structure
* \param read Function pointer to a function that reads `n` bytes from the
* input source into `buff` and returns the number of bytes
* successfully read.
* \return `SBP_OK` (0) if successful but no complete message yet,
* `SBP_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed,
* `SBP_OK_CALLBACK_UNDEFINED` (2) if message decoded with no associated
* callback, and `SBP_CRC_ERROR` (-2) if a CRC error
* has occurred. Thus can check for >0 to ensure good processing.
*/
s8 sbp_process(sbp_state_t *s, u32 (*read)(u8 *buff, u32 n, void *context))
{
u8 temp;
u16 crc;
switch (s->state) {
case WAITING:
if ((*read)(&temp, 1, s->io_context) == 1)
if (temp == SBP_PREAMBLE) {
s->n_read = 0;
s->state = GET_TYPE;
}
break;
case GET_TYPE:
s->n_read += (*read)((u8*)&(s->msg_type) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
/* Swap bytes to little endian. */
s->n_read = 0;
s->state = GET_SENDER;
}
break;
case GET_SENDER:
s->n_read += (*read)((u8*)&(s->sender_id) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
/* Swap bytes to little endian. */
s->state = GET_LEN;
}
break;
case GET_LEN:
if ((*read)(&(s->msg_len), 1, s->io_context) == 1) {
s->n_read = 0;
s->state = GET_MSG;
}
break;
case GET_MSG:
/* Not received whole message yet, try and read some more. */
s->n_read += (*read)(
&(s->msg_buff[s->n_read]),
s->msg_len - s->n_read,
s->io_context
);
if (s->msg_len - s->n_read <= 0) {
s->n_read = 0;
s->state = GET_CRC;
}
break;
case GET_CRC:
s->n_read += (*read)((u8*)&(s->crc) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
s->state = WAITING;
/* Swap bytes to little endian. */
crc = crc16_ccitt((u8*)&(s->msg_type), 2, 0);
crc = crc16_ccitt((u8*)&(s->sender_id), 2, crc);
crc = crc16_ccitt(&(s->msg_len), 1, crc);
crc = crc16_ccitt(s->msg_buff, s->msg_len, crc);
if (s->crc == crc) {
/* Message complete, process it. */
sbp_msg_callbacks_node_t* node = sbp_find_callback(s, s->msg_type);
if (node) {
(*node->cb)(s->sender_id, s->msg_len, s->msg_buff, node->context);
return SBP_OK_CALLBACK_EXECUTED;
} else {
return SBP_OK_CALLBACK_UNDEFINED;
}
} else
return SBP_CRC_ERROR;
}
break;
default:
s->state = WAITING;
break;
}
return SBP_OK;
}
/** Send SBP messages.
* Takes an SBP message payload, type and sender ID then writes a message to
* the output stream using the supplied `write` function with the correct
* framing and CRC.
*
* The supplied `write` function must have the prototype:
*
* ~~~
* u32 write(u8 *buff, u32 n, void* context)
* ~~~
*
* where `n` is the number of bytes to be written and `buff` is the buffer from
* which to read the data to be written, and `context` is the arbitrary pointer
* set by `sbp_state_set_io_context`. The function should return the number
* of bytes successfully written which may be between 0 and `n`. Currently, if
* the number of bytes written is different from `n` then `sbp_send_message`
* will immediately return with an error.
*
* Note that `sbp_send_message` makes multiple calls to write and therefore if
* a `write` call fails then this may result in a partial message being written
* to the output. This should be caught by the CRC check on the receiving end
* but will result in lost messages.
*
* \param write Function pointer to a function that writes `n` bytes from
* `buff` to the output stream and returns the number of bytes
* successfully written.
* \return `SBP_OK` (0) if successful, `SBP_WRITE_ERROR` if the message could
* not be sent or was only partially sent.
*/
s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
u32 (*write)(u8 *buff, u32 n, void *context))
{
/* Check our payload data pointer isn't NULL unless len = 0. */
if (len != 0 && payload == 0)
return SBP_NULL_ERROR;
/* Check our write function pointer isn't NULL. */
if (write == 0)
return SBP_NULL_ERROR;
u16 crc;
u8 preamble = SBP_PREAMBLE;
if ((*write)(&preamble, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if ((*write)((u8*)&msg_type, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)((u8*)&sender_id, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)(&len, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if (len > 0) {
if ((*write)(payload, len, s->io_context) != len)
return SBP_SEND_ERROR;
}
crc = crc16_ccitt((u8*)&(msg_type), 2, 0);
crc = crc16_ccitt((u8*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
if ((*write)((u8*)&crc, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
return SBP_OK;
}
/** \} */
/** \} */
sw/ground_segment/cockpit/live.ml
+1 -1
View File
@@ -1302,7 +1302,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
then status_filter_mode
else Pprz.string_assoc "gps_mode" vs in
ac.strip#set_label "GPS" gps_mode;
ac.strip#set_color "GPS" (if gps_mode<>"3D" then alert_color else ok_color);
ac.strip#set_color "GPS" (if gps_mode<>"3D" && gps_mode<>"DGPS" && gps_mode<>"RTK" then alert_color else ok_color);
let ft =
sprintf "%02d:%02d:%02d" (flight_time / 3600) ((flight_time / 60) mod 60) (flight_time mod 60) in
ac.strip#set_label "flight_time" ft;
sw/ground_segment/misc/Makefile
+22 -7
View File
@@ -47,27 +47,33 @@ IVY_INC = $(shell pkg-config --cflags-only-I ivy-glib)
IVY_LDFLAGS = $(shell pkg-config --libs-only-L ivy-glib) -livy
endif
# Optitrack specific librarys and includes
NATNET_LIBRARYS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs)
# GLIB libraries
GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs)
# Paparazzi includes
INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ $(IVY_INC)
INCLUDES += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/ -I$(PAPARAZZI_SRC)/sw/airborne/arch/linux/
all: davis2ivy kestrel2ivy natnet2ivy video_synchronizer
all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer
clean:
$(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy video_synchronizer
$(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer
davis2ivy: davis2ivy.o
@echo CC $@
$(Q)$(CC) $(CFLAGS) -o davis2ivy davis2ivy.o $(LIBRARYS) $(IVY_LDFLAGS)
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(IVY_LDFLAGS)
kestrel2ivy: kestrel2ivy.o
@echo CC $@
$(Q)$(CC) $(CFLAGS) -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) $(IVY_LDFLAGS)
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(IVY_LDFLAGS)
natnet2ivy: natnet2ivy.o pprz_geodetic_double.o pprz_algebra_double.o udp_socket.o
@echo CC $@
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(NATNET_LIBRARYS) $(IVY_LDFLAGS)
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
sbp2ivy: sbp2ivy.o serial_port.o sbp.o edc.o
@echo CC $@
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(INCLUDES) $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
video_synchronizer: video_synchronizer.c
@echo CC $@
@@ -82,6 +88,15 @@ pprz_geodetic_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.
udp_socket.o : $(PAPARAZZI_SRC)/sw/airborne/arch/linux/udp_socket.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
serial_port.o : $(PAPARAZZI_SRC)/sw/airborne/arch/linux/serial_port.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
sbp.o : $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c
$(Q)$(CC) $(CFLAGS) -c -std=c99 -O2 -Wall $(INCLUDES) $<
edc.o : $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
$(Q)$(CC) $(CFLAGS) -c -std=c99 -O2 -Wall $(INCLUDES) $<
%.o : %.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
sw/ground_segment/misc/sbp2ivy.c
+226
View File
@@ -0,0 +1,226 @@
/*
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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 sbp2ivy.c
* \brief SBP GPS packets to Ivy for DGPS and RTK
*
* This communicates with an SBP GPS receiver like an
* Swift-Nav Piksi. Tjis then forwards the Observed messages
* over the Ivy bus to inject them for DGPS and RTK positioning.
*/
#include <glib.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <ctype.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include <libsbp/sbp.h>
#include <libsbp/observation.h>
#include "std.h"
#include "serial_port.h"
/** Used variables **/
struct SerialPort *serial_port;
sbp_state_t sbp_state;
sbp_msg_callbacks_node_t sbp_obs_node;
sbp_msg_callbacks_node_t sbp_obs_dep_a_node;
sbp_msg_callbacks_node_t base_pos_node;
/** Default values **/
uint8_t ac_id = 0;
char *serial_device = "/dev/ttyUSB0";
uint32_t serial_baud = B115200;
/** Debugging options */
bool_t verbose = FALSE;
#define printf_debug if(verbose == TRUE) printf
/** Ivy Bus default */
#ifdef __APPLE__
char *ivy_bus = "224.255.255.255";
#else
char *ivy_bus = "127.255.255.255:2010";
#endif
/*
* Read bytes from the Piksi UART connection
* This is a wrapper functions used in the libsbp library
*/
static uint32_t sbp_read(uint8_t *buff, uint32_t n, void *context __attribute__((unused)))
{
int ret = read(serial_port->fd, buff, n);
if(ret > 0)
return ret;
else
return 0;
}
static void ivy_send_message(uint8_t packet_id, uint8_t len, uint8_t msg[]) {
char gps_packet[512], number[5];
uint8_t i;
snprintf(gps_packet, 512, "0 GPS_INJECT %d %d %d", ac_id, packet_id, msg[0]); //AC_ID
for(i = 1; i < len; i++) {
snprintf(number, 5, ",%d", msg[i]);
strcat(gps_packet, number);
}
IvySendMsg(gps_packet);
printf_debug("Ivy send: %s\n", gps_packet);
}
/*
* Callback for the OBS observation message to send it trough GPS_INJECT
*/
static void sbp_obs_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len,
uint8_t msg[],
void *context __attribute__((unused)))
{
if(len > 0) {
ivy_send_message(SBP_MSG_OBS, len, msg);
}
printf_debug("Parsed OBS callback\n");
}
/*
* Callback for the old OBS observation message to send it trough GPS_INJECT
*/
static void sbp_obs_dep_a_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len,
uint8_t msg[],
void *context __attribute__((unused)))
{
if(len > 0) {
ivy_send_message(SBP_MSG_OBS_DEP_A, len, msg);
}
printf_debug("Parsed OBS_DEP_A callback\n");
}
/*
* Callback for the BASE_POS observation message to send it trough GPS_INJECT
*/
static void sbp_base_pos_callback(uint16_t sender_id __attribute__((unused)),
uint8_t len,
uint8_t msg[],
void *context __attribute__((unused)))
{
if(len > 0) {
ivy_send_message(SBP_MSG_BASE_POS, len, msg);
}
printf_debug("Parsed BASE_POS callback\n");
}
/**
* Parse the tty data when bytes are available
*/
static gboolean parse_device_data(GIOChannel *chan, GIOCondition cond, gpointer data)
{
sbp_process(&sbp_state, &sbp_read);
return TRUE;
}
/** Print the program help */
void print_usage(int argc __attribute__((unused)), char ** argv) {
static const char *usage =
"Usage: %s [options]\n"
" Options :\n"
" -h, --help Display this help\n"
" -v, --verbose Verbosity enabled\n\n"
" -d <device> The GPS device(default: /dev/ttyUSB0)\n\n";
fprintf(stderr, usage, argv[0]);
}
int main(int argc, char** argv)
{
// Parse the options from cmdline
char c;
while ((c = getopt (argc, argv, "hvd:b:i:")) != EOF) {
switch (c) {
case 'h':
print_usage(argc, argv);
exit(EXIT_SUCCESS);
break;
case 'v':
verbose = TRUE;
break;
case 'd':
serial_device = optarg;
break;
case 'b':
serial_baud = atoi(optarg);
break;
case 'i':
ac_id = atoi(optarg);
break;
case '?':
if (optopt == 'd' || optopt == 'b' || optopt == 'i')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
print_usage(argc, argv);
exit(EXIT_FAILURE);
default:
abort();
}
}
// Create the Ivy Client
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0);
IvyStart(ivy_bus);
// Start the tty device
printf_debug("Opening tty device %s...\n", serial_device);
serial_port = serial_port_new();
int ret = serial_port_open_raw(serial_port, serial_device, serial_baud);
if (ret != 0) {
fprintf(stderr, "Error opening %s code %d\n", serial_device, ret);
serial_port_free(serial_port);
exit(EXIT_FAILURE);
}
// Setup SBP callbacks
printf_debug("Setup SBP callbacks...\n");
sbp_state_init(&sbp_state);
sbp_register_callback(&sbp_state, SBP_MSG_OBS, &sbp_obs_callback, NULL, &sbp_obs_node);
sbp_register_callback(&sbp_state, SBP_MSG_OBS_DEP_A, &sbp_obs_dep_a_callback, NULL, &sbp_obs_dep_a_node);
sbp_register_callback(&sbp_state, SBP_MSG_BASE_POS, &sbp_base_pos_callback, NULL, &base_pos_node);
// Add IO watch for tty connection
printf_debug("Adding IO watch...\n");
GIOChannel *sk = g_io_channel_unix_new(serial_port->fd);
g_io_add_watch(sk, G_IO_IN, parse_device_data, NULL);
// Run the main loop
printf_debug("Started sbp2ivy for aircraft id %d!\n", ac_id);
g_main_loop_run(ml);
return 0;
}
sw/ground_segment/tmtc/server.ml
+1 -1
View File
@@ -185,7 +185,7 @@ let ac_msg = fun messages_xml logging ac_name ac ->
let cam_max_angle = (Deg>>Rad) 89.
let send_cam_status = fun a ->
if a.gps_mode = gps_mode_3D then
if a.gps_mode >= gps_mode_3D then
match a.nav_ref with
None -> () (* No geo ref for camera target *)
| Some nav_ref ->
sw/ground_segment/tmtc/server_globals.ml
+1 -1
View File
@@ -10,7 +10,7 @@ let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"A
let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
let gps_modes = [|"NOFIX";"NA";"2D";"3D";"DGPS";"RTK"|]
let state_filter_modes = [|"UNKNOWN";"INIT";"ALIGN";"OK";"GPS_LOST";"IMU_LOST";"COV_ERR";"IR_CONTRAST";"ERROR"|]
let _3D = 3
let gps_hybrid_modes = [|"OFF";"ON"|]