From b1246def84c1cd6f5588317b6b591c4a346bb63e Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sat, 12 Sep 2015 22:52:49 +0200 Subject: [PATCH] [gps] update piksi driver --- .../subsystems/fixedwing/gps_piksi.makefile | 2 +- .../subsystems/rotorcraft/gps_piksi.makefile | 1 - sw/airborne/subsystems/gps/gps_piksi.c | 374 ++++++++---------- sw/airborne/subsystems/gps/gps_piksi.h | 2 + 4 files changed, 164 insertions(+), 215 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile index 75622a6361e..8b33a40e5b8 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile @@ -20,7 +20,7 @@ ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c # libsbp -ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/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 diff --git a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile index 747ca5e8943..1cdbe1d32a5 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile @@ -21,7 +21,6 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c # libsbp ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include -ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/libsbp ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c nps.CFLAGS += -DUSE_GPS diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index 368abbc3bf2..6b5e8a1c995 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,9 +30,6 @@ * https://github.com/swift-nav/sbp_tutorial */ -#include -#include -#include #include "subsystems/gps/gps_piksi.h" #include "subsystems/gps.h" #include "subsystems/abi.h" @@ -43,17 +41,35 @@ #include "generated/flight_plan.h" #endif -#ifndef USE_PIKSI_BASELINE_ECEF -#define USE_PIKSI_BASELINE_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 -/* Force piksi module to use internal patch antenna - * Caution: default value might by "Auto" or "External" depending on the firmware version +/* + * Set the UART config depending on which UART is connected */ -#define USE_PIKSI_PATCH_ANTENNA 1 -#if USE_PIKSI_PATCH_ANTENNA -#include -static const char ANTENNA_SETTING[] = "frontend antenna_selection Patch"; +#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 /* @@ -72,24 +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; +sbp_msg_callbacks_node_t tracking_state_node; +sbp_msg_callbacks_node_t tracking_state_dep_a_node; -#if USE_PIKSI_BASELINE_ECEF -sbp_msg_callbacks_node_t baseline_ecef_node; - -struct gps_baseline { - struct EcefCoor_i ecef_pos; ///< position in ECEF in cm - bool_t ecef_valid; -}; - -static struct gps_baseline baseline; - -#endif - -//#if USE_PIKSI_BASELINE_NED -//sbp_msg_callbacks_node_t baseline_ned_node; -//#endif 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. @@ -101,42 +106,29 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)), uint8_t msg[], void *context __attribute__((unused))) { + static uint8_t last_flags = 0; msg_pos_ecef_t pos_ecef = *(msg_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); FIXME not implemented yet by libswiftnav - // instead give some arbitrary values telling the fix mode - uint8_t fix_mode = (pos_ecef.flags & 0x3); - if (fix_mode == 2) { - gps.pacc = 2; - } else if (fix_mode == 1) { - gps.pacc = 1; - } else { - gps.pacc = 99; - } - gps.num_sv = pos_ecef.n_sats; - gps.fix = GPS_FIX_3D; - gps.tow = pos_ecef.tow; - // gps_piksi_publish(); // Done after receiving vel_ned -} -#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))) -{ - msg_baseline_ecef_t baseline_ecef = *(msg_baseline_ecef_t *)msg; - baseline.ecef_pos.x = (int32_t)(baseline_ecef.x / 10); - baseline.ecef_pos.y = (int32_t)(baseline_ecef.y / 10); - baseline.ecef_pos.z = (int32_t)(baseline_ecef.z / 10); - if (baseline.ecef_valid == FALSE) { - gps_piksi_set_base_pos(); + // 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; + + 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; } - baseline.ecef_valid = TRUE; + 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)), @@ -150,7 +142,7 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)), 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)), @@ -158,51 +150,47 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), uint8_t msg[], void *context __attribute__((unused))) { + static uint8_t last_flags = 0; msg_pos_llh_t pos_llh = *(msg_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.); + + // 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; - } + /* 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; + // 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; } -//#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))) -//{ -// msg_baseline_ned_t baseline_ned = *(sbp_baseline_ned_t *)msg; -//} -//#endif - static void sbp_vel_ned_callback(uint16_t sender_id __attribute__((unused)), uint8_t len __attribute__((unused)), uint8_t msg[], @@ -237,28 +225,46 @@ static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)), gps.tow = gps_time.tow; } -/* - * FIFO to hold received UART bytes before - * libswiftnav SBP submodule parses them. - */ -#define FIFO_LEN 512 -char sbp_msg_fifo[FIFO_LEN]; +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; -/* 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); -uint32_t write_callback(uint8_t *buff, uint32_t n, void *context); + 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) { - baseline.ecef_valid = FALSE; - /* 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_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); @@ -266,33 +272,37 @@ void gps_impl_init(void) 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); -#if USE_PIKSI_BASELINE_ECEF - sbp_register_callback(&sbp_state, SBP_MSG_BASELINE_ECEF, &sbp_baseline_ecef_callback, NULL, &baseline_ecef_node); -#endif -//#if USE_PIKSI_BASELINE_NED -// sbp_register_callback(&sbp_state, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback, NULL, &baseline_ned_node); -//#endif - -#if USE_PIKSI_PATCH_ANTENNA - sbp_send_message(&sbp_state, SBP_MSG_SETTINGS, 0, sizeof(ANTENNA_SETTING) ,(u8*)(&ANTENNA_SETTING), write_callback); -#endif + 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; } - /* - * 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 @@ -300,108 +310,46 @@ 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; } AbiSendMsgGPS(GPS_PIKSI_ID, now_ts, &gps); } -void gps_piksi_set_base_pos(void) -{ - // compute base pos from current rover position and baseleg - // TODO would probably need some averaging on the last values - struct EcefCoor_i base_pos; - VECT3_DIFF(base_pos, gps.ecef_pos, baseline.ecef_pos); - struct LlaCoor_i base_lla; - lla_of_ecef_i(&base_lla, &base_pos); - // fill sbp message - msg_base_pos_t msg_base_pos = { - .lat = (double)(DEG_OF_EM7DEG(base_lla.lat)), - .lon = (double)(DEG_OF_EM7DEG(base_lla.lon)), - .height = (double)(M_OF_MM(base_lla.alt)) - }; - // send message to piksi module - sbp_send_message(&sbp_state, SBP_MSG_BASE_POS, 0, sizeof(msg_base_pos_t), (u8*)(&msg_base_pos), write_callback); -} - -/* - * FIFO implementation - */ -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. + * Read bytes from the Piksi UART connection + * This is a wrapper functions used in the libsbp library */ -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; - } - } - return i; -} -/* Return 1 if true, 0 otherwise. */ -uint8_t fifo_full(void) -{ - if (((tail + 1) % FIFO_LEN) == head) { - return 1; + buff[i] = uart_getch(&(GPS_LINK)); } - return 0; + return i; } -/* Write some bytes on a uart link */ -uint32_t write_callback(uint8_t *buff, uint32_t n, void *context __attribute__((unused))) +/* + * 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))) { uint32_t i = 0; for (i = 0; i < n; i++) { - uart_transmit(&(GPS_LINK), buff[i]); + uart_put_byte(&(GPS_LINK), buff[i]); } 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 4757f47e91b..a9024d49659 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.h +++ b/sw/airborne/subsystems/gps/gps_piksi.h @@ -32,6 +32,8 @@ #ifndef GPS_PIKSI_H #define GPS_PIKSI_H +#define GPS_NB_CHANNELS 10 + extern void gps_piksi_event(void); /*