From f95dea962abf594b9c3c4ef7d62838581eab6abf Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sat, 2 May 2015 23:44:39 +0200 Subject: [PATCH] [gps] use libsbp as git submodule currently matching libswiftnav v0.15.1 --- .gitmodules | 3 + .../subsystems/fixedwing/gps_piksi.makefile | 6 +- .../subsystems/rotorcraft/gps_piksi.makefile | 6 +- sw/airborne/subsystems/gps/gps_piksi.c | 34 +- sw/ext/libsbp | 1 + sw/ext/libswiftnav/include/common.h | 57 --- sw/ext/libswiftnav/include/edc.h | 21 - sw/ext/libswiftnav/include/sbp.h | 84 ---- sw/ext/libswiftnav/include/sbp_messages.h | 182 ------- sw/ext/libswiftnav/src/edc.c | 141 ------ sw/ext/libswiftnav/src/sbp.c | 473 ------------------ 11 files changed, 27 insertions(+), 981 deletions(-) create mode 160000 sw/ext/libsbp delete mode 100644 sw/ext/libswiftnav/include/common.h delete mode 100644 sw/ext/libswiftnav/include/edc.h delete mode 100644 sw/ext/libswiftnav/include/sbp.h delete mode 100644 sw/ext/libswiftnav/include/sbp_messages.h delete mode 100644 sw/ext/libswiftnav/src/edc.c delete mode 100644 sw/ext/libswiftnav/src/sbp.c diff --git a/.gitmodules b/.gitmodules index c98d5fa341e..7909bedfbe1 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/swift-nav/libsbp.git diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile index 5b0cfd2c953..75622a6361e 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/libsbp +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 d3c2617dc86..e578144f68a 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/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 nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index ae9ccc8afa0..e0921e821a4 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.c +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -30,7 +30,7 @@ */ #include -#include +#include #include "subsystems/gps.h" #include "subsystems/abi.h" #include "mcu_periph/uart.h" @@ -90,7 +90,7 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)), return; } #endif - sbp_pos_ecef_t pos_ecef = *(sbp_pos_ecef_t *)msg; + 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); @@ -106,7 +106,7 @@ static void sbp_baseline_ecef_callback(uint16_t sender_id __attribute__((unused) uint8_t msg[], void *context __attribute__((unused))) { - sbp_baseline_ecef_t baseline_ecef = *(sbp_baseline_ecef_t *)msg; + msg_baseline_ecef_t baseline_ecef = *(msg_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); @@ -127,7 +127,7 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __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); @@ -153,7 +153,7 @@ 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; + 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.); @@ -194,7 +194,7 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), // uint8_t msg[], // void *context __attribute__((unused))) //{ -// sbp_baseline_ned_t baseline_ned = *(sbp_baseline_ned_t *)msg; +// msg_baseline_ned_t baseline_ned = *(sbp_baseline_ned_t *)msg; //} //#endif @@ -203,7 +203,7 @@ static void sbp_vel_ned_callback(uint16_t sender_id __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); @@ -218,7 +218,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; } @@ -227,7 +227,7 @@ 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; } @@ -240,17 +240,17 @@ 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); + 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); #if USE_PIKSI_BASELINE_ECEF - sbp_register_callback(&sbp_state, SBP_BASELINE_ECEF, &sbp_baseline_ecef_callback, NULL, &baseline_ecef_node); + 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_BASELINE_NED, &sbp_baseline_ned_callback, NULL, &baseline_ned_node); +// sbp_register_callback(&sbp_state, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback, NULL, &baseline_ned_node); //#endif } diff --git a/sw/ext/libsbp b/sw/ext/libsbp new file mode 160000 index 00000000000..b0773d8a9ea --- /dev/null +++ b/sw/ext/libsbp @@ -0,0 +1 @@ +Subproject commit b0773d8a9eaacc2b78e85d787f060f0ccf8c3282 diff --git a/sw/ext/libswiftnav/include/common.h b/sw/ext/libswiftnav/include/common.h deleted file mode 100644 index 74d4b2e78a4..00000000000 --- 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 838029ef7a2..00000000000 --- 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 f94fff09e40..00000000000 --- 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 9591fce3911..00000000000 --- 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 9e607d195df..00000000000 --- 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 1519a7782c8..00000000000 --- 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; -} - -/** \} */ -/** \} */ -