diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile new file mode 100644 index 0000000000..4d7cc3f4cb --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile @@ -0,0 +1,36 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Swift-Nav Piksi RTK module + +GPS_LED ?= none + +GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) + +ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=B115200 +ap.CFLAGS += -DGPS_LINK=$(GPS_PORT_LOWER) + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +$(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 + +sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c + diff --git a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile new file mode 100644 index 0000000000..d3c2617dc8 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile @@ -0,0 +1,29 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Swift-Nav Piksi RTK module + +GPS_LED ?= none + +GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) + +ap.CFLAGS += -DUSE_GPS +ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=B115200 +ap.CFLAGS += -DGPS_LINK=$(GPS_PORT_LOWER) + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +$(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 + +nps.CFLAGS += -DUSE_GPS +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c + diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c new file mode 100644 index 0000000000..850df29ea5 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -0,0 +1,331 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * 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 subsystems/gps/gps_piksi.c + * + * Driver for Piksi modules from Swift-Nav + * + * http://docs.swiftnav.com/wiki/Piksi_Integration_Tutorial + * https://github.com/swift-nav/sbp_tutorial + */ + +#include +#include +#include "subsystems/gps.h" +#include "mcu_periph/uart.h" +#include "math/pprz_geodetic_double.h" +#if GPS_USE_LATLONG +#include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/common_nav.h" +#include "generated/flight_plan.h" +#endif + +#ifndef USE_PIKSI_ECEF +#define USE_PIKSI_ECEF 1 +#endif + +/* + * State of the SBP message parser. + * Must be statically allocated. + */ +sbp_state_t sbp_state; + +/* + * SBP callback nodes must be statically allocated. Each message ID / callback + * pair must have a unique sbp_msg_callbacks_node_t associated with it. + */ +sbp_msg_callbacks_node_t pos_ecef_node; +sbp_msg_callbacks_node_t vel_ecef_node; +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 + +/* + * Callback functions to interpret SBP messages. + * Every message ID has a callback associated with it to + * receive and interpret the message payload. + */ +static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)), + uint8_t len __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; +} + +#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; + + // High precision solution available + gps_piksi_available = TRUE; +} +#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; + 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_available = TRUE; +} + +static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), + uint8_t len __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 +} + +//#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 + +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; + 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); +#if GPS_USE_LATLONG + gps.gspeed = int32_sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y); + gps.course = (int32_t)(1e7 * atan2(gps.ned_vel.y, gps.ned_vel.x)); +#endif +} + +static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)), + uint8_t len __attribute__((unused)), + uint8_t msg[], + void *context __attribute__((unused))) +{ + sbp_dops_t dops = *(sbp_dops_t *)msg; + gps.pdop = dops.pdop; +} + +static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)), + uint8_t len __attribute__((unused)), + uint8_t msg[], + void *context __attribute__((unused))) +{ + sbp_gps_time_t gps_time = *(sbp_gps_time_t *)msg; + gps.week = gps_time.wn; + gps.tow = gps_time.tow; +} + +bool_t gps_piksi_available; + +void gps_impl_init(void) +{ + gps_piksi_available = 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_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 +} + + +/* + * 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 + */ +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); +} + +/* + * 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. + */ +uint32_t fifo_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))) { + break; + } + } + return i; +} + +/* Return 1 if true, 0 otherwise. */ +uint8_t fifo_full(void) +{ + if (((tail + 1) % FIFO_LEN) == head) { + return 1; + } + return 0; +} + + diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h new file mode 100644 index 0000000000..4f43c46046 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_piksi.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * 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 subsystems/gps/gps_piksi.h + * + * Driver for Piksi modules from Swift-Nav + * + * http://docs.swiftnav.com/wiki/Piksi_Integration_Tutorial + * https://github.com/swift-nav/sbp_tutorial + */ + +#ifndef GPS_PIKSI_H +#define GPS_PIKSI_H + +extern bool_t gps_piksi_available; + +void gps_piksi_event(void); + +/* + * The GPS event + */ +#define GpsEvent(_sol_available_callback) { \ + gps_piksi_event(); \ + if (gps_piksi_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + if (gps.fix == GPS_FIX_3D) { \ + gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ + gps.last_3dfix_time = sys_time.nb_sec; \ + } \ + _sol_available_callback(); \ + gps_piksi_available = FALSE; \ + } \ + } + +#endif /* GPS_PIKSI_H */ + diff --git a/sw/ext/libswiftnav/include/common.h b/sw/ext/libswiftnav/include/common.h new file mode 100644 index 0000000000..74d4b2e78a --- /dev/null +++ b/sw/ext/libswiftnav/include/common.h @@ -0,0 +1,57 @@ +/* + * 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 new file mode 100644 index 0000000000..838029ef7a --- /dev/null +++ b/sw/ext/libswiftnav/include/edc.h @@ -0,0 +1,21 @@ +/* + * 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 new file mode 100644 index 0000000000..f94fff09e4 --- /dev/null +++ b/sw/ext/libswiftnav/include/sbp.h @@ -0,0 +1,84 @@ +/* + * 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 new file mode 100644 index 0000000000..9591fce391 --- /dev/null +++ b/sw/ext/libswiftnav/include/sbp_messages.h @@ -0,0 +1,182 @@ +/* + * 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 new file mode 100644 index 0000000000..9e607d195d --- /dev/null +++ b/sw/ext/libswiftnav/src/edc.c @@ -0,0 +1,141 @@ +/* + * 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 new file mode 100644 index 0000000000..1519a7782c --- /dev/null +++ b/sw/ext/libswiftnav/src/sbp.c @@ -0,0 +1,473 @@ +/* + * 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; +} + +/** \} */ +/** \} */ +