Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 12:28:03 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Merge pull request #1043 from paparazzi/piksi

Browse Source 
[gps] add support for Piksi gps from Swift-Nav

closes #957
This commit is contained in:
Felix Ruess
2014-12-17 12:55:43 +01:00
parent 6a2f6aba3f b915775fb1
commit 9119313887
10 changed files with 1411 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/gps_piksi.makefile
+36
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile
+29
View File
@@ -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
sw/airborne/subsystems/gps/gps_piksi.c
+331
View File
@@ -0,0 +1,331 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 <sbp.h>
#include <sbp_messages.h>
#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;
}
sw/airborne/subsystems/gps/gps_piksi.h
+57
View File
@@ -0,0 +1,57 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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 */
sw/ext/libswiftnav/include/common.h
+57
View File
@@ -0,0 +1,57 @@
/*
* Copyright (C) 2012 Swift Navigation Inc.
* Contact: Henry Hallam <henry@swift-nav.com>
* Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_COMMON_H
#define LIBSWIFTNAV_COMMON_H
/** \defgroup common Common definitions
* Common definitions used throughout the library.
* \{ */
#define MIN(x, y) (((x) < (y)) ? (x) : (y))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
/** \defgroup common_inttypes Integer types
* Specified-width integer type definitions for shorter and nicer code.
*
* These should be used in preference to unspecified width types such as
* `int` which can lead to portability issues between different platforms.
* \{ */
/** Signed 8-bit integer. */
typedef int8_t s8;
/** Signed 16-bit integer. */
typedef int16_t s16;
/** Signed 32-bit integer. */
typedef int32_t s32;
/** Signed 64-bit integer. */
typedef int64_t s64;
/** Unsigned 8-bit integer. */
typedef uint8_t u8;
/** Unsigned 16-bit integer. */
typedef uint16_t u16;
/** Unsigned 32-bit integer. */
typedef uint32_t u32;
/** Unsigned 64-bit integer. */
typedef uint64_t u64;
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_COMMON_H */
sw/ext/libswiftnav/include/edc.h
+21
View File
@@ -0,0 +1,21 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_EDC_H
#define LIBSWIFTNAV_EDC_H
#include "common.h"
u16 crc16_ccitt(const u8 *buf, u32 len, u16 crc);
u32 crc24q(const u8 *buf, u32 len, u32 crc);
#endif /* LIBSWIFTNAV_EDC_H */
sw/ext/libswiftnav/include/sbp.h
+84
View File
@@ -0,0 +1,84 @@
/*
* Copyright (C) 2011-2014 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_SBP_H
#define LIBSWIFTNAV_SBP_H
#include "common.h"
/** \addtogroup sbp
* \{ */
/** Return value indicating success. */
#define SBP_OK 0
/** Return value indicating message decoded and callback executed by sbp_process. */
#define SBP_OK_CALLBACK_EXECUTED 1
/** Return value indicating message decoded with no associated callback in sbp_process. */
#define SBP_OK_CALLBACK_UNDEFINED 2
/** Return value indicating an error with the callback (function defined). */
#define SBP_CALLBACK_ERROR -1
/** Return value indicating a CRC error. */
#define SBP_CRC_ERROR -2
/** Return value indicating an error occured whilst sending an SBP message. */
#define SBP_SEND_ERROR -3
/** Return value indicating an error occured because an argument was NULL. */
#define SBP_NULL_ERROR -4
/** SBP callback function prototype definition. */
typedef void (*sbp_msg_callback_t)(u16 sender_id, u8 len, u8 msg[], void *context);
/** SBP callback node.
* Forms a linked list of callbacks.
* \note Must be statically allocated for use with sbp_register_callback().
*/
typedef struct sbp_msg_callbacks_node {
u16 msg_type; /**< Message ID associated with callback. */
sbp_msg_callback_t cb; /**< Pointer to callback function. */
void *context; /**< Pointer to a context */
struct sbp_msg_callbacks_node *next; /**< Pointer to next node in list. */
} sbp_msg_callbacks_node_t;
/** State structure for processing SBP messages. */
typedef struct {
enum {
WAITING = 0,
GET_TYPE,
GET_SENDER,
GET_LEN,
GET_MSG,
GET_CRC
} state;
u16 msg_type;
u16 sender_id;
u16 crc;
u8 msg_len;
u8 n_read;
u8 msg_buff[256];
void* io_context;
sbp_msg_callbacks_node_t* sbp_msg_callbacks_head;
} sbp_state_t;
/** \} */
s8 sbp_register_callback(sbp_state_t* s, u16 msg_type, sbp_msg_callback_t cb, void* context,
sbp_msg_callbacks_node_t *node);
void sbp_clear_callbacks(sbp_state_t* s);
sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t* s, u16 msg_type);
void sbp_state_init(sbp_state_t *s);
void sbp_state_set_io_context(sbp_state_t *s, void* context);
s8 sbp_process(sbp_state_t *s, u32 (*read)(u8 *buff, u32 n, void* context));
s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
u32 (*write)(u8 *buff, u32 n, void* context));
#endif /* LIBSWIFTNAV_SBP_H */
sw/ext/libswiftnav/include/sbp_messages.h
+182
View File
@@ -0,0 +1,182 @@
/*
* Copyright (C) 2013 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
/*****************************************************************************
* Automatically generated from sbp.yaml with generate.py, do not hand edit! *
*****************************************************************************/
#ifndef LIBSWIFTNAV_SBP_MESSAGES_H
#define LIBSWIFTNAV_SBP_MESSAGES_H
#include "common.h"
/** System start-up message
* The system start-up message is sent once on system start-up. It is
* intended to be used to notify the host or other attached devices that
* the system has started and is now ready to respond to commands or
* configuration requests.
*/
#define SBP_STARTUP 0xFF00
typedef struct __attribute__((packed)) {
u32 reserved; /**< Reserved */
} sbp_startup_t;
/** System heartbeat message
* The heartbeat message is sent periodically to inform the host or
* other attached devices that the system is running. It is intended to
* be used to monitor for system malfunctions and also contains
* status flags that indicate to the host the status of the system and
* if it is operating correctly.
*
* The system error flag is used to indicate that an error has occurred in
* the system. To determine the source of the error the remaining error
* flags should be inspected.
*/
#define SBP_HEARTBEAT 0xFFFF
typedef struct __attribute__((packed)) {
u32 flags; /**< Status flags */
} sbp_heartbeat_t;
/** GPS Time
* GPS Time.
*/
#define SBP_GPS_TIME 0x0100
typedef struct __attribute__((packed)) {
u16 wn; /**< GPS week number [weeks] */
u32 tow; /**< GPS Time of Week rounded to the nearest ms [ms] */
s32 ns; /**< Nanosecond remainder of rounded tow [ns] */
u8 flags; /**< Status flags (reserved) */
} sbp_gps_time_t;
/** Dilution of Precision
* Dilution of Precision.
*/
#define SBP_DOPS 0x0206
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
u16 gdop; /**< Geometric Dilution of Precision [0.01] */
u16 pdop; /**< Position Dilution of Precision [0.01] */
u16 tdop; /**< Time Dilution of Precision [0.01] */
u16 hdop; /**< Horizontal Dilution of Precision [0.01] */
u16 vdop; /**< Vertical Dilution of Precision [0.01] */
} sbp_dops_t;
/** Position in ECEF
* Position solution in absolute Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_POS_ECEF 0x0200
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
double x; /**< ECEF X coordinate [m] */
double y; /**< ECEF Y coordinate [m] */
double z; /**< ECEF Z coordinate [m] */
u16 accuracy; /**< Position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_pos_ecef_t;
/** Geodetic Position
* Geodetic position solution.
*/
#define SBP_POS_LLH 0x0201
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
double lat; /**< Latitude [deg] */
double lon; /**< Longitude [deg] */
double height; /**< Height [m] */
u16 h_accuracy; /**< Horizontal position accuracy estimate [mm] */
u16 v_accuracy; /**< Vertical position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_pos_llh_t;
/** Baseline in ECEF
* Baseline in Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_BASELINE_ECEF 0x0202
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 x; /**< Baseline ECEF X coordinate [mm] */
s32 y; /**< Baseline ECEF Y coordinate [mm] */
s32 z; /**< Baseline ECEF Z coordinate [mm] */
u16 accuracy; /**< Position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_baseline_ecef_t;
/** Baseline in NED
* Baseline in local North East Down (NED) coordinates.
*/
#define SBP_BASELINE_NED 0x0203
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 n; /**< Baseline North coordinate [mm] */
s32 e; /**< Baseline East coordinate [mm] */
s32 d; /**< Baseline Down coordinate [mm] */
u16 h_accuracy; /**< Horizontal position accuracy estimate [mm] */
u16 v_accuracy; /**< Vertical position accuracy estimate [mm] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags */
} sbp_baseline_ned_t;
/** Velocity in ECEF
* Velocity in Earth Centered Earth Fixed (ECEF) coordinates.
*/
#define SBP_VEL_ECEF 0x0204
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 x; /**< Velocity ECEF X coordinate [mm/s] */
s32 y; /**< Velocity ECEF Y coordinate [mm/s] */
s32 z; /**< Velocity ECEF Z coordinate [mm/s] */
u16 accuracy; /**< Velocity accuracy estimate [mm/s] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags (reserved) */
} sbp_vel_ecef_t;
/** Velocity in NED
* Velocity in local North East Down (NED) coordinates.
*/
#define SBP_VEL_NED 0x0205
typedef struct __attribute__((packed)) {
u32 tow; /**< GPS Time of Week [ms] */
s32 n; /**< Velocity North coordinate [mm/s] */
s32 e; /**< Velocity East coordinate [mm/s] */
s32 d; /**< Velocity Down coordinate [mm/s] */
u16 h_accuracy; /**< Horizontal velocity accuracy estimate [mm/s] */
u16 v_accuracy; /**< Vertical velocity accuracy estimate [mm/s] */
u8 n_sats; /**< Number of satellites used in solution */
u8 flags; /**< Status flags (reserved) */
} sbp_vel_ned_t;
#endif /* LIBSWIFTNAV_SBP_MESSAGES_H */
sw/ext/libswiftnav/src/edc.c
+141
View File
@@ -0,0 +1,141 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
/** \defgroup edc Error Detection and Correction
* Error detection and correction functions.
* \{ */
/** \defgroup crc CRC
* Cyclic redundancy checks.
* \{ */
/* CRC16 implementation acording to CCITT standards */
static const u16 crc16tab[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
/** Calculate CCITT 16-bit Cyclical Redundancy Check (CRC16).
*
* This implementation uses parameters used by XMODEM i.e. polynomial is:
* \f[
* x^{16} + x^{12} + x^5 + 1
* \f]
* Mask 0x11021, not reversed, not XOR'd
* (there are several slight variants on the CCITT CRC-16).
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC16 value
*/
u16 crc16_ccitt(const u8 *buf, u32 len, u16 crc)
{
for (u32 i = 0; i < len; i++)
crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
return crc;
}
static const u32 crc24qtab[256] = {
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
*
* The CRC polynomial used is:
* \f[
* x^{24} + x^{23} + x^{18} + x^{17} + x^{14} + x^{11} + x^{10} +
* x^7 + x^6 + x^5 + x^4 + x^3 + x+1
* \f]
* Mask 0x1864CFB, not reversed, not XOR'd
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC-24Q value
*/
u32 crc24q(const u8 *buf, u32 len, u32 crc)
{
for (u32 i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[(crc >> 16) ^ buf[i]];
return crc;
}
/** \} */
/** \} */
sw/ext/libswiftnav/src/sbp.c
+473
View File
@@ -0,0 +1,473 @@
/*
* Copyright (C) 2011-2014 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
#include "sbp.h"
#define SBP_PREAMBLE 0x55
/** \addtogroup io Input / Output
* \{ */
/** \defgroup sbp SBP
* Send and receive messages using Swift Binary Protocol.
*
* Examples
* ========
*
* Receiving
* ---------
*
* First setup a callback for the message you will be receiving. Our callback
* function must have type #sbp_msg_callback_t, i.e. it must be of the form:
*
* ~~~
* void my_callback(u16 sender_id, u8 len, u8 msg[], void *context)
* {
* // Process msg.
* }
* ~~~
*
* You must also statically allocate a #sbp_msg_callbacks_node_t that will be
* used to keep track of the callback function. You do not need to initialize
* it as this will be done by sbp_register_callback().
*
* ~~~
* static sbp_msg_callbacks_node_t my_callback_node;
* ~~~
*
* Now register your callback function with the SBP library as follows:
*
* ~~~
* sbp_register_callback(&sbp_state, SBP_MY_MSG_TYPE, &my_callback, &context, &my_callback_node);
* ~~~
*
* where `SBP_MY_MSG_TYPE` is the numerical identifier of your message type.
*
* You must now call sbp_process() periodically whenever you have received SBP
* data to be processed, e.g. from the serial port. Remember sbp_process() may
* not use all available data so keep calling sbp_process() until all the
* received serial data has been consumed.
*
* sbp_process() stores its internal state in an #sbp_state_t struct which must
* be initialized by calling sbp_state_init() before its first use.
*
* Here is an example based on reading from a typical UART interface:
*
* ~~~
* u32 my_read(u8 *buff, u32 n, void *context)
* {
* for (u32 i=0; i<n; i++) {
* if (uart_has_data())
* buff[i] = uart_read_char();
* else
* break;
* }
* return i;
* }
*
* int main()
* {
* ...
*
* sbp_state_t s;
* sbp_state_init(&s);
*
* while(uart_has_data()) {
* sbp_process(&s, &my_read);
* }
*
* ...
* }
* ~~~
*
* If you're writing C++ code that wants to reference a pointer to
* an object in the my_read function, you can use the context set
* by calling sbp_state_set_io_context()
*
*
* Sending
* -------
*
* To send an SBP message simply call the sbp_send_message() function,
* providing a `write` function that writes data to your output.
*
* Often the data to be sent will simply be a struct cast to a `u8` buffer. As
* a convenience you may want to define a macro that automatically includes
* your write function and calculates the size of the item to be sent.
*
* ~~~
* // Convenience macro for sending an SBP message.
* #define SBP_MSG(sbp_state, msg_type, item) \
* sbp_send_message(&sbp_state, msg_type, MY_SENDER_ID, \
* sizeof(item), (u8 *)&(item), &my_write)
*
* typedef struct {
* u8 x, y;
* } my_awesome_struct;
*
* u32 my_write(u8 *buff, u32 n, void *context)
* {
* for (u32 i=0; i<n; i++) {
* if (uart_write_char(buff[i]) == ERROR)
* break;
* }
* return i;
* }
*
* int main()
* {
* ...
*
* sbp_state_t s;
* sbp_state_init(&s);
*
* my_awesome_struct payload = { 0x22, 0x33 };
*
* sbp_send_message(&s, SBP_MY_MSG_TYPE, MY_SENDER_ID,
* sizeof(payload), (u8*)&payload, &my_write);
*
* // or
*
* SBP_MSG(s, SBP_MY_MSG_TYPE, payload);
*
* ...
* }
* ~~~
*
*
* \{ */
/** Register a callback for a message type.
* Register a callback that is called when a message
* with type msg_type is received.
*
* \param msg_type Message type associated with callback
* \param cb Pointer to message callback function
* \param context Pointer to context for callback function
* \param node Statically allocated #sbp_msg_callbacks_node_t struct
* \return `SBP_OK` (0) if successful, `SBP_CALLBACK_ERROR` if callback was
* already registered for that message type.
*/
s8 sbp_register_callback(sbp_state_t *s, u16 msg_type, sbp_msg_callback_t cb, void *context,
sbp_msg_callbacks_node_t *node)
{
/* Check our callback function pointer isn't NULL. */
if (cb == 0)
return SBP_NULL_ERROR;
/* Check our callback node pointer isn't NULL. */
if (node == 0)
return SBP_NULL_ERROR;
/* Check if callback was already registered for this type. */
if (sbp_find_callback(s, msg_type) != 0)
return SBP_CALLBACK_ERROR;
/* Fill in our new sbp_msg_callback_node_t. */
node->msg_type = msg_type;
node->cb = cb;
node->context = context;
/* The next pointer is set to NULL, i.e. this
* will be the new end of the linked list.
*/
node->next = 0;
/* If our linked list is empty then just
* add the new node to the start.
*/
if (s->sbp_msg_callbacks_head == 0) {
s->sbp_msg_callbacks_head = node;
return SBP_OK;
}
/* Find the tail of our linked list and
* add our new node to the end.
*/
sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
while (p->next)
p = p->next;
p->next = node;
return SBP_OK;
}
/** Clear all registered callbacks.
* This is probably only useful for testing but who knows!
*/
void sbp_clear_callbacks(sbp_state_t *s)
{
/* Reset the head of the callbacks list to NULL. */
s->sbp_msg_callbacks_head = 0;
}
/** Find the callback function associated with a message type.
* Searches through the list of registered callbacks to find the callback
* associated with the passed message type.
*
* \param msg_type Message type to find callback for
* \return Pointer to callback node (#sbp_msg_callbacks_node_t) or `NULL` if
* callback not found for that message type.
*/
sbp_msg_callbacks_node_t* sbp_find_callback(sbp_state_t *s, u16 msg_type)
{
/* If our list is empty, return NULL. */
if (!s->sbp_msg_callbacks_head)
return 0;
/* Traverse the linked list and return the callback
* function pointer if we find a node with a matching
* message id.
*/
sbp_msg_callbacks_node_t *p = s->sbp_msg_callbacks_head;
do
if (p->msg_type == msg_type)
return p;
while ((p = p->next));
/* Didn't find a matching callback, return NULL. */
return 0;
}
/** Initialize an #sbp_state_t struct before use.
* This resets the entire state, including all callbacks.
* Remember to use this function to initialize the state before calling
* sbp_process() for the first time.
*
* \param s State structure
*/
void sbp_state_init(sbp_state_t *s)
{
s->state = WAITING;
/* Set the IO context pointer, passed to read and write functions, to NULL. */
s->io_context = 0;
/* Clear the callbacks, if any, currently in s */
sbp_clear_callbacks(s);
}
/** Set a context to pass to all function pointer calls made by sbp functions
* This helper function sets a void* context pointer in sbp_state.
* Whenever `sbp_process` calls the `read` function pointer, it passes this context.
* Whenever `sbp_send_message` calls the `write` function pointer, it passes this context.
* This allows C++ code to get a pointer to an object inside these functions.
*/
void sbp_state_set_io_context(sbp_state_t *s, void *context)
{
s->io_context = context;
}
/** Read and process SBP messages.
* Reads bytes from an input source using the provided `read` function, decodes
* the SBP framing and performs a CRC check on the message.
*
* When an SBP message is successfully received then the list of callbacks is
* searched for a callback corresponding to the received message type. If a
* callback is found then it is called with the ID of the sender, the message
* length and the message payload data buffer as arguments.
*
* \note sbp_process will always call `read` with n > 0
* (aka it will attempt to always read something)
*
* The supplied `read` function must have the prototype:
*
* ~~~
* u32 read(u8 *buff, u32 n, void* context)
* ~~~
*
* where `n` is the number of bytes requested and `buff` is the buffer into
* which to write the received data, and `context` is the arbitrary pointer
* set by `sbp_state_set_io_context`.
* The function should return the number of
* bytes successfully written into `buff` which may be between 0 and `n`
* inclusive, but must never be greater than `n`.
*
* Note that `sbp_process` may not read all available bytes from the `read`
* function so the caller should loop until all bytes available from the input
* source have been consumed.
*
* \param s State structure
* \param read Function pointer to a function that reads `n` bytes from the
* input source into `buff` and returns the number of bytes
* successfully read.
* \return `SBP_OK` (0) if successful but no complete message yet,
* `SBP_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed,
* `SBP_OK_CALLBACK_UNDEFINED` (2) if message decoded with no associated
* callback, and `SBP_CRC_ERROR` (-2) if a CRC error
* has occurred. Thus can check for >0 to ensure good processing.
*/
s8 sbp_process(sbp_state_t *s, u32 (*read)(u8 *buff, u32 n, void *context))
{
u8 temp;
u16 crc;
switch (s->state) {
case WAITING:
if ((*read)(&temp, 1, s->io_context) == 1)
if (temp == SBP_PREAMBLE) {
s->n_read = 0;
s->state = GET_TYPE;
}
break;
case GET_TYPE:
s->n_read += (*read)((u8*)&(s->msg_type) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
/* Swap bytes to little endian. */
s->n_read = 0;
s->state = GET_SENDER;
}
break;
case GET_SENDER:
s->n_read += (*read)((u8*)&(s->sender_id) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
/* Swap bytes to little endian. */
s->state = GET_LEN;
}
break;
case GET_LEN:
if ((*read)(&(s->msg_len), 1, s->io_context) == 1) {
s->n_read = 0;
s->state = GET_MSG;
}
break;
case GET_MSG:
/* Not received whole message yet, try and read some more. */
s->n_read += (*read)(
&(s->msg_buff[s->n_read]),
s->msg_len - s->n_read,
s->io_context
);
if (s->msg_len - s->n_read <= 0) {
s->n_read = 0;
s->state = GET_CRC;
}
break;
case GET_CRC:
s->n_read += (*read)((u8*)&(s->crc) + s->n_read,
2-s->n_read, s->io_context);
if (s->n_read >= 2) {
s->state = WAITING;
/* Swap bytes to little endian. */
crc = crc16_ccitt((u8*)&(s->msg_type), 2, 0);
crc = crc16_ccitt((u8*)&(s->sender_id), 2, crc);
crc = crc16_ccitt(&(s->msg_len), 1, crc);
crc = crc16_ccitt(s->msg_buff, s->msg_len, crc);
if (s->crc == crc) {
/* Message complete, process it. */
sbp_msg_callbacks_node_t* node = sbp_find_callback(s, s->msg_type);
if (node) {
(*node->cb)(s->sender_id, s->msg_len, s->msg_buff, node->context);
return SBP_OK_CALLBACK_EXECUTED;
} else {
return SBP_OK_CALLBACK_UNDEFINED;
}
} else
return SBP_CRC_ERROR;
}
break;
default:
s->state = WAITING;
break;
}
return SBP_OK;
}
/** Send SBP messages.
* Takes an SBP message payload, type and sender ID then writes a message to
* the output stream using the supplied `write` function with the correct
* framing and CRC.
*
* The supplied `write` function must have the prototype:
*
* ~~~
* u32 write(u8 *buff, u32 n, void* context)
* ~~~
*
* where `n` is the number of bytes to be written and `buff` is the buffer from
* which to read the data to be written, and `context` is the arbitrary pointer
* set by `sbp_state_set_io_context`. The function should return the number
* of bytes successfully written which may be between 0 and `n`. Currently, if
* the number of bytes written is different from `n` then `sbp_send_message`
* will immediately return with an error.
*
* Note that `sbp_send_message` makes multiple calls to write and therefore if
* a `write` call fails then this may result in a partial message being written
* to the output. This should be caught by the CRC check on the receiving end
* but will result in lost messages.
*
* \param write Function pointer to a function that writes `n` bytes from
* `buff` to the output stream and returns the number of bytes
* successfully written.
* \return `SBP_OK` (0) if successful, `SBP_WRITE_ERROR` if the message could
* not be sent or was only partially sent.
*/
s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
u32 (*write)(u8 *buff, u32 n, void *context))
{
/* Check our payload data pointer isn't NULL unless len = 0. */
if (len != 0 && payload == 0)
return SBP_NULL_ERROR;
/* Check our write function pointer isn't NULL. */
if (write == 0)
return SBP_NULL_ERROR;
u16 crc;
u8 preamble = SBP_PREAMBLE;
if ((*write)(&preamble, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if ((*write)((u8*)&msg_type, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)((u8*)&sender_id, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)(&len, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if (len > 0) {
if ((*write)(payload, len, s->io_context) != len)
return SBP_SEND_ERROR;
}
crc = crc16_ccitt((u8*)&(msg_type), 2, 0);
crc = crc16_ccitt((u8*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
if ((*write)((u8*)&crc, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
return SBP_OK;
}
/** \} */
/** \} */