diff --git a/.gitignore b/.gitignore index 172b21d315..d2b0ab9013 100644 --- a/.gitignore +++ b/.gitignore @@ -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 diff --git a/.gitmodules b/.gitmodules index c98d5fa341..76616d37ce 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/Makefile b/Makefile index 84618f8b94..a509f520ef 100644 --- a/Makefile +++ b/Makefile @@ -172,6 +172,8 @@ ext: # subdirs: $(SUBDIRS) +$(MISC): ext + $(SUBDIRS): $(MAKE) -C $@ diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile index 5b0cfd2c95..8b33a40e5b 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile @@ -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\" diff --git a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile index d3c2617dc8..1cdbe1d32a 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile @@ -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\" diff --git a/conf/messages.xml b/conf/messages.xml index 0f047dfa58..81558fcfbb 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1348,7 +1348,7 @@ - + @@ -1989,7 +1989,7 @@ - + @@ -2534,6 +2534,12 @@ + + + + + + @@ -2611,7 +2617,7 @@ - + diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.c b/sw/airborne/arch/linux/mcu_periph/uart_arch.c index 1b4787745e..d1b1247fe7 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.c @@ -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 */ diff --git a/sw/airborne/boards/ardrone2.h b/sw/airborne/boards/ardrone2.h index cbba10fa89..db17a0ac11 100644 --- a/sw/airborne/boards/ardrone2.h +++ b/sw/airborne/boards/ardrone2.h @@ -4,7 +4,7 @@ #define BOARD_ARDRONE2 #ifndef UART1_DEV -#define UART1_DEV "/dev/ttyUSB0" +#define UART1_DEV /dev/ttyUSB0 #endif /* Default actuators driver */ diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h index 45b72faf7b..04158f6468 100644 --- a/sw/airborne/boards/bebop.h +++ b/sw/airborne/boards/bebop.h @@ -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" diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index a83a9d07b0..e6d7400f7e 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -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; diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index e2c69d2719..daa59920d0 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -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) diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c index 0d20eab8c6..17e13cf35a 100644 --- a/sw/airborne/modules/geo_mag/geo_mag.c +++ b/sw/airborne/modules/geo_mag/geo_mag.c @@ -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; } } diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 1c9429783b..f819dbd7cc 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -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); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index df83e800d2..cd13dd3b00 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -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 diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 5b42fc2755..53dd3a2c4f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -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.; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 94ba268aa0..09f4c01e16 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -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)) { diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 2ef2126b58..cf69cd8a87 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -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))){ + +} diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 06e50c5de8..62525b789f 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -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; diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index 4e69204112..6b5e8a1c99 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.c +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2014 Gautier Hattenberger + * 2015 Freek van Tienen * * This file is part of paparazzi. * @@ -29,8 +30,7 @@ * https://github.com/swift-nav/sbp_tutorial */ -#include -#include +#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 +#include +#include +#include +#include +#include + +/* + * 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); +} diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h index bdff9b6f05..a9024d4965 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.h +++ b/sw/airborne/subsystems/gps/gps_piksi.h @@ -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 diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 87a6e45a6b..2b8be8e368 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -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 diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 96da9ef59d..c82dcb3af0 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -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) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 070f00c5ec..7540ce9a7a 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -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; } diff --git a/sw/ext/Makefile b/sw/ext/Makefile index 79bef4e539..2ef8502f30 100644 --- a/sw/ext/Makefile +++ b/sw/ext/Makefile @@ -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; \ diff --git a/sw/ext/libsbp b/sw/ext/libsbp new file mode 160000 index 0000000000..1c48447386 --- /dev/null +++ b/sw/ext/libsbp @@ -0,0 +1 @@ +Subproject commit 1c48447386e11431a4c1846edb65c06e79e940be diff --git a/sw/ext/libswiftnav/include/common.h b/sw/ext/libswiftnav/include/common.h deleted file mode 100644 index 74d4b2e78a..0000000000 --- a/sw/ext/libswiftnav/include/common.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright (C) 2012 Swift Navigation Inc. - * Contact: Henry Hallam - * Fergus Noble - * - * 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 -#include -#include - -/** \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 */ - diff --git a/sw/ext/libswiftnav/include/edc.h b/sw/ext/libswiftnav/include/edc.h deleted file mode 100644 index 838029ef7a..0000000000 --- a/sw/ext/libswiftnav/include/edc.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * Copyright (C) 2010 Swift Navigation Inc. - * Contact: Fergus Noble - * - * 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 */ diff --git a/sw/ext/libswiftnav/include/sbp.h b/sw/ext/libswiftnav/include/sbp.h deleted file mode 100644 index f94fff09e4..0000000000 --- a/sw/ext/libswiftnav/include/sbp.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright (C) 2011-2014 Swift Navigation Inc. - * Contact: Fergus Noble - * - * 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 */ - diff --git a/sw/ext/libswiftnav/include/sbp_messages.h b/sw/ext/libswiftnav/include/sbp_messages.h deleted file mode 100644 index 9591fce391..0000000000 --- a/sw/ext/libswiftnav/include/sbp_messages.h +++ /dev/null @@ -1,182 +0,0 @@ -/* - * Copyright (C) 2013 Swift Navigation Inc. - * Contact: Fergus Noble - * - * 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 */ - diff --git a/sw/ext/libswiftnav/src/edc.c b/sw/ext/libswiftnav/src/edc.c deleted file mode 100644 index 9e607d195d..0000000000 --- a/sw/ext/libswiftnav/src/edc.c +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (C) 2010 Swift Navigation Inc. - * Contact: Fergus Noble - * - * 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; -} - -/** \} */ - -/** \} */ - diff --git a/sw/ext/libswiftnav/src/sbp.c b/sw/ext/libswiftnav/src/sbp.c deleted file mode 100644 index 1519a7782c..0000000000 --- a/sw/ext/libswiftnav/src/sbp.c +++ /dev/null @@ -1,473 +0,0 @@ -/* - * Copyright (C) 2011-2014 Swift Navigation Inc. - * Contact: Fergus Noble - * - * 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; imsg_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; -} - -/** \} */ -/** \} */ - diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 8d3b2de4be..3d33033219 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -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; diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index 56f51e85ec..c217b237ea 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -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) $< diff --git a/sw/ground_segment/misc/sbp2ivy.c b/sw/ground_segment/misc/sbp2ivy.c new file mode 100644 index 0000000000..dfa56e82e5 --- /dev/null +++ b/sw/ground_segment/misc/sbp2ivy.c @@ -0,0 +1,226 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 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; +} diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 5c3f09dc0f..5a1a792661 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -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 -> diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index a758104df1..ebd33c6958 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -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"|]