mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Add gps passthrough and spistream files
This commit is contained in:
Allen Ibara
@@ -0,0 +1,271 @@
|
||||
#ifndef SPISTREAM_PROTOCOL_H__
|
||||
#define SPISTREAM_PROTOCOL_H__
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#ifndef SPISTREAM_MAX_MESSAGE_LENGTH
|
||||
#define SPISTREAM_MAX_MESSAGE_LENGTH 720
|
||||
#endif
|
||||
|
||||
#ifndef SPISTREAM_MAX_RX_MESSAGE_LENGTH
|
||||
#define SPISTREAM_MAX_RX_MESSAGE_LENGTH SPISTREAM_MAX_MESSAGE_LENGTH
|
||||
#endif
|
||||
#ifndef SPISTREAM_RX_BUFFER_SIZE
|
||||
#define SPISTREAM_RX_BUFFER_SIZE SPISTREAM_MAX_RX_MESSAGE_LENGTH
|
||||
#endif
|
||||
|
||||
#ifndef SPISTREAM_MAX_TX_MESSAGE_LENGTH
|
||||
#define SPISTREAM_MAX_TX_MESSAGE_LENGTH SPISTREAM_MAX_MESSAGE_LENGTH
|
||||
#endif
|
||||
#ifndef SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS
|
||||
#define SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS 4
|
||||
#endif
|
||||
#ifndef SPISTREAM_TX_MAX_BUFFER_PACKAGES
|
||||
#define SPISTREAM_TX_MAX_BUFFER_PACKAGES ( \
|
||||
(SPISTREAM_MAX_TX_MESSAGE_LENGTH / \
|
||||
SPISTREAM_PACKAGE_SIZE) * \
|
||||
SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS)
|
||||
#endif
|
||||
|
||||
#define SPISTREAM_INVALID_MESSAGE_ID 0
|
||||
|
||||
struct spistream_state_t {
|
||||
uint8_t tx_message_cnt; // message cnt of next message to be sent
|
||||
uint8_t rx_package_cntd;
|
||||
};
|
||||
|
||||
struct spistream_message_range_t {
|
||||
uint8_t index;
|
||||
uint8_t size;
|
||||
};
|
||||
|
||||
struct spistream_buffers_t {
|
||||
uint16_t rx_num_packages; // number of packages in buffer
|
||||
|
||||
uint16_t tx_insert; // next index for package insertion
|
||||
uint16_t tx_read; // next index to read package from
|
||||
uint16_t tx_num_packages; // number of packages in buffer
|
||||
|
||||
// RX stores data as array
|
||||
uint8_t rx[SPISTREAM_RX_BUFFER_SIZE];
|
||||
// TX stores packages
|
||||
struct AutopilotMessagePTStream tx[SPISTREAM_TX_MAX_BUFFER_PACKAGES];
|
||||
};
|
||||
|
||||
typedef void (*spistream_message_rx_handler_t)(uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
|
||||
typedef void (*spistream_message_tx_handler_t)(uint8_t msg_id);
|
||||
struct spistream_config_t {
|
||||
// Handler to call for processing received message
|
||||
spistream_message_rx_handler_t message_rx_handler;
|
||||
// Handler to call after message transmission
|
||||
spistream_message_tx_handler_t message_tx_handler;
|
||||
};
|
||||
|
||||
enum spistream_flag { SPISTREAM_NO_WAIT=0, SPISTREAM_WAIT_FOR_READ };
|
||||
|
||||
/* Function declarations */
|
||||
|
||||
static inline void spistream_init(spistream_message_rx_handler_t message_rx_handler,
|
||||
spistream_message_tx_handler_t message_tx_handler);
|
||||
static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in);
|
||||
static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out);
|
||||
static inline uint8_t spistream_send_msg(uint8_t * data, uint16_t num_bytes, enum spistream_flag);
|
||||
|
||||
/* Definitions */
|
||||
|
||||
static struct spistream_state_t spistream_state;
|
||||
static struct spistream_buffers_t spistream_buffers;
|
||||
static struct spistream_config_t spistream;
|
||||
|
||||
static inline void spistream_init(spistream_message_rx_handler_t message_rx_handler,
|
||||
spistream_message_tx_handler_t message_tx_handler)
|
||||
{
|
||||
memset(&spistream_buffers, 0, sizeof(struct spistream_buffers_t));
|
||||
memset(&spistream_state, 0, sizeof(struct spistream_state_t));
|
||||
spistream.message_rx_handler = message_rx_handler;
|
||||
spistream.message_tx_handler = message_tx_handler;
|
||||
spistream_buffers.rx_num_packages = 0;
|
||||
spistream_state.rx_package_cntd = 0;
|
||||
spistream_buffers.tx_num_packages = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Read a single package into internal RX buffer.
|
||||
* Converts data from package domain to byte array.
|
||||
* After receiving a full message, the registered spistream.message_rx_handler
|
||||
* function is called.
|
||||
* Called on every SPI event.
|
||||
*/
|
||||
static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in)
|
||||
{
|
||||
uint8_t package_cntd;
|
||||
|
||||
if(pkg_in->message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
|
||||
return;
|
||||
}
|
||||
|
||||
// In the last package of every message, the package_cntd is expected to be
|
||||
// negative or 0. It indicates the number of zero-bytes that are padded to
|
||||
// the end of the message to fill a package.
|
||||
if(pkg_in->package_cntd <= 0) { package_cntd = 1; }
|
||||
else { package_cntd = pkg_in->package_cntd; }
|
||||
|
||||
if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) {
|
||||
spistream_buffers.rx_num_packages = pkg_in->package_cntd;
|
||||
}
|
||||
|
||||
if(spistream_state.rx_package_cntd == 0) { // Beginning of new message
|
||||
// Message length is first value of package countdown:
|
||||
spistream_buffers.rx_num_packages = package_cntd;
|
||||
spistream_state.rx_package_cntd = package_cntd;
|
||||
}
|
||||
memcpy(spistream_buffers.rx +
|
||||
((spistream_buffers.rx_num_packages - package_cntd) *
|
||||
SPISTREAM_PACKAGE_SIZE),
|
||||
pkg_in->pkg_data,
|
||||
SPISTREAM_PACKAGE_SIZE);
|
||||
|
||||
if(pkg_in->package_cntd <= 0) {
|
||||
// Message is ready, pass to handler:
|
||||
spistream.message_rx_handler(pkg_in->message_cnt,
|
||||
(uint8_t *)(spistream_buffers.rx),
|
||||
(spistream_buffers.rx_num_packages *
|
||||
SPISTREAM_PACKAGE_SIZE) +
|
||||
pkg_in->package_cntd);
|
||||
spistream_state.rx_package_cntd = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Fill given SPI package with next package from TX buffer.
|
||||
* Called on every SPI event.
|
||||
*/
|
||||
static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out)
|
||||
{
|
||||
if(spistream_buffers.tx_num_packages == 0) {
|
||||
memset(pkg_out, 0, sizeof(struct AutopilotMessagePTStream));
|
||||
pkg_out->message_cnt = SPISTREAM_INVALID_MESSAGE_ID;
|
||||
return;
|
||||
}
|
||||
|
||||
memcpy(pkg_out,
|
||||
spistream_buffers.tx + spistream_buffers.tx_read,
|
||||
sizeof(struct AutopilotMessagePTStream));
|
||||
if(pkg_out->package_cntd <= 0) {
|
||||
spistream.message_tx_handler(pkg_out->message_cnt);
|
||||
}
|
||||
|
||||
spistream_buffers.tx_read++;
|
||||
if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
|
||||
spistream_buffers.tx_read = 0;
|
||||
}
|
||||
|
||||
spistream_buffers.tx_num_packages--;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enqueue given message in TX buffer.
|
||||
* This function is directly wrapped by spistream_send_message
|
||||
* at the moment.
|
||||
*/
|
||||
static inline uint8_t spistream_enqueue_msg(uint8_t * data,
|
||||
uint16_t num_bytes,
|
||||
enum spistream_flag wait_for_read)
|
||||
{
|
||||
uint16_t pkg_idx, num_packages, num_padding;
|
||||
uint16_t idx;
|
||||
// Enough space in buffer?
|
||||
|
||||
if(wait_for_read == SPISTREAM_NO_WAIT ||
|
||||
spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES)
|
||||
{
|
||||
spistream_state.tx_message_cnt++;
|
||||
// Message id 0 is reserved for invalid packages:
|
||||
if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
|
||||
spistream_state.tx_message_cnt = 1;
|
||||
}
|
||||
// How many packages we need for this message:
|
||||
num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE);
|
||||
if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) {
|
||||
num_packages++;
|
||||
}
|
||||
// How many zero-bytes we will have at the end of the last package:
|
||||
if(num_bytes > SPISTREAM_PACKAGE_SIZE) {
|
||||
num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes;
|
||||
}
|
||||
else {
|
||||
num_padding = SPISTREAM_PACKAGE_SIZE - num_bytes;
|
||||
}
|
||||
|
||||
pkg_idx = spistream_buffers.tx_insert;
|
||||
|
||||
// Convert data to packages and add them to TX buffer:
|
||||
for(idx = 0; num_packages > 0; idx++) {
|
||||
if(idx < num_bytes) {
|
||||
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx];
|
||||
}
|
||||
else { // padding
|
||||
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0;
|
||||
}
|
||||
// Last byte in current package:
|
||||
if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) {
|
||||
|
||||
// Finish configuration of current package
|
||||
// Last package uses field package_cntd to indicate the number
|
||||
// of padding bytes it contains, as negative number:
|
||||
if(num_packages == 1) {
|
||||
spistream_buffers.tx[pkg_idx].package_cntd = -num_padding;
|
||||
}
|
||||
else {
|
||||
spistream_buffers.tx[pkg_idx].package_cntd = num_packages;
|
||||
}
|
||||
spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt;
|
||||
|
||||
// Prepare next package:
|
||||
num_packages--;
|
||||
// Increment insert pointer with ring buffer overflow:
|
||||
spistream_buffers.tx_insert++;
|
||||
if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
|
||||
spistream_buffers.tx_insert = 0;
|
||||
}
|
||||
// Continue with next package:
|
||||
pkg_idx = spistream_buffers.tx_insert;
|
||||
spistream_buffers.tx_num_packages++;
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
printf("Enqueue finished. Buffer: \n");
|
||||
for(pkg_idx = 0; pkg_idx < spistream_buffers.tx_num_packages; pkg_idx++) {
|
||||
printf("Package %2d | %3d |: ", pkg_idx, spistream_buffers.tx[pkg_idx].package_cntd);
|
||||
for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) {
|
||||
printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void spistream_dequeue_msg(uint8_t message_id) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Used from userland: Send num_bytes bytes from buffer over spistream.
|
||||
* Flags are:
|
||||
* - SPISTREAM_WAIT_FOR_READ: Reject packages when TX buffer is full
|
||||
* and return 0, otherwise enqueue message and return 1.
|
||||
* - SPISTREAM_NO_WAIT: Overwrite data if TX buffer is full, enqueue
|
||||
* message and always return 1.
|
||||
*/
|
||||
static inline uint8_t spistream_send_msg(uint8_t * data,
|
||||
uint16_t num_bytes,
|
||||
enum spistream_flag wait_for_read)
|
||||
{
|
||||
return spistream_enqueue_msg(data, num_bytes, wait_for_read);
|
||||
}
|
||||
|
||||
#endif /* SPISTREAM_PROTOCOL_H__ */
|
||||
|
||||
Reference in New Issue
Block a user