rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

ahrs spi: Add AHRS SPI link management FSM

Browse Source 
To the outside world, the AHRS can be in one of only a few
primary states:
 Not present - AHRS is absent or non-responsive via SPI
 Inactive - Only link-level status messages are processed
 Ready - Ready to receive the next application level message
 Busy - Application level message is being processed

Internal to the AHRS, there are many more states that need to be
managed.  This FSM provides the necessary decoupling between the
ISR (which is being driven by the SPI link) and the AHRS main
processing loop which must continue to run its filters independently
of the SPI messaging rate.

With this structure, SPI messages can be received at any time but
processed at only specific points within the filter chains.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1005 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
stac 2010-07-04 02:21:22 +00:00 committed by stac
parent 89a6d6d912
commit e3824a6d78
7 changed files with 780 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/AHRS/Makefile
View File

@ -87,6 +87,7 @@ CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
## AHRS:
SRC = ahrs.c
SRC += pios_board.c
SRC += ahrs_fsm.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c

620
flight/AHRS/ahrs_fsm.c Normal file
View File

@ -0,0 +1,620 @@
#include <stdint.h> /* uint*_t */
#include <stddef.h> /* NULL */
#include "ahrs_fsm.h"
#include "pios_opahrs_proto.h"
#include "pios.h"
struct lfsm_context {
enum lfsm_state curr_state;
enum opahrs_msg_link_state link_state;
enum opahrs_msg_type user_payload_type;
uint32_t user_payload_len;
uint32_t errors;
uint8_t * rx;
uint8_t * tx;
uint8_t * link_rx;
uint8_t * link_tx;
uint8_t * user_rx;
uint8_t * user_tx;
struct lfsm_link_stats stats;
};
static struct lfsm_context context = { 0 };
static void lfsm_update_link_tx (struct lfsm_context * context);
static void lfsm_init_rx (struct lfsm_context * context);
/*
*
* Link Finite State Machine
*
*/
struct lfsm_transition {
void (*entry_fn)(struct lfsm_context * context);
enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
};
static void go_faulted(struct lfsm_context * context);
static void go_stopped(struct lfsm_context * context);
static void go_stopping(struct lfsm_context * context);
static void go_inactive(struct lfsm_context * context);
static void go_user_busy(struct lfsm_context * context);
static void go_user_busy_rx_pending(struct lfsm_context * context);
static void go_user_busy_tx_pending(struct lfsm_context * context);
static void go_user_busy_rxtx_pending(struct lfsm_context * context);
static void go_user_rx_pending(struct lfsm_context * context);
static void go_user_tx_pending(struct lfsm_context * context);
static void go_user_rxtx_pending(struct lfsm_context * context);
static void go_user_rx_active(struct lfsm_context * context);
static void go_user_tx_active(struct lfsm_context * context);
static void go_user_rxtx_active(struct lfsm_context * context);
const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES] = {
[LFSM_STATE_FAULTED] = {
.entry_fn = go_faulted,
},
[LFSM_STATE_STOPPED] = {
.entry_fn = go_stopped,
.next_state = {
[LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_STOPPING] = {
.entry_fn = go_stopping,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_INACTIVE] = {
.entry_fn = go_inactive,
.next_state = {
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_BUSY] = {
.entry_fn = go_user_busy,
.next_state = {
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY,
},
},
[LFSM_STATE_USER_BUSY_RX_PENDING] = {
.entry_fn = go_user_busy_rx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_TX_PENDING] = {
.entry_fn = go_user_busy_tx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
.entry_fn = go_user_busy_rxtx_pending,
.next_state = {
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
},
},
[LFSM_STATE_USER_RX_PENDING] = {
.entry_fn = go_user_rx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_PENDING] = {
.entry_fn = go_user_tx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
},
},
[LFSM_STATE_USER_RXTX_PENDING] = {
.entry_fn = go_user_rxtx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
},
},
[LFSM_STATE_USER_RX_ACTIVE] = {
.entry_fn = go_user_rx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_ACTIVE] = {
.entry_fn = go_user_tx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_RXTX_ACTIVE] = {
.entry_fn = go_user_rxtx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
};
/*
* FSM State Entry Functions
*/
static void go_faulted(struct lfsm_context * context)
{
PIOS_DEBUG_Assert(0);
}
static void go_stopped(struct lfsm_context * context)
{
#if 0
PIOS_SPI_Stop(PIOS_SPI_OP);
#endif
}
static void go_stopping(struct lfsm_context * context)
{
context->link_tx = NULL;
context->tx = NULL;
}
static void go_inactive(struct lfsm_context * context)
{
context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
lfsm_update_link_tx(context);
context->user_rx = NULL;
context->user_tx = NULL;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_busy(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->user_rx = NULL;
context->user_tx = NULL;
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_busy_rx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_busy_tx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_busy_rxtx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_rx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_tx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_rxtx_pending(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_rx_active(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->rx = context->user_rx;
context->tx = context->link_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
lfsm_update_link_tx(context);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_tx_active(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
context->rx = context->link_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
static void go_user_rxtx_active(struct lfsm_context * context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
context->rx = context->user_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
}
/*
*
* Misc Helper Functions
*
*/
static void lfsm_update_link_tx_v0 (struct opahrs_msg_v0 * msg, enum opahrs_msg_link_state state, uint16_t errors)
{
opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx_v1 (struct opahrs_msg_v1 * msg, enum opahrs_msg_link_state state, uint16_t errors)
{
opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx (struct lfsm_context * context)
{
PIOS_DEBUG_Assert(context->link_tx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
lfsm_update_link_tx_v0 ((struct opahrs_msg_v0 *)context->link_tx, context->link_state, context->errors);
break;
case OPAHRS_MSG_TYPE_USER_V1:
lfsm_update_link_tx_v1 ((struct opahrs_msg_v1 *)context->link_tx, context->link_state, context->errors);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
static void lfsm_init_rx (struct lfsm_context * context)
{
PIOS_DEBUG_Assert(context->rx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
opahrs_msg_v0_init_rx ((struct opahrs_msg_v0 *)context->rx);
break;
case OPAHRS_MSG_TYPE_USER_V1:
opahrs_msg_v1_init_rx ((struct opahrs_msg_v1 *)context->rx);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
/*
*
* External API
*
*/
void lfsm_inject_event(enum lfsm_event event)
{
PIOS_IRQ_Disable();
/*
* Move to the next state
*
* This is done prior to calling the new state's entry function to
* guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was.
*/
context.curr_state = lfsm_transitions[context.curr_state].next_state[event];
/* Call the entry function (if any) for the next state. */
if (lfsm_transitions[context.curr_state].entry_fn) {
lfsm_transitions[context.curr_state].entry_fn(&context);
}
PIOS_IRQ_Enable();
}
void lfsm_init(void)
{
context.curr_state = LFSM_STATE_STOPPED;
go_stopped(&context);
}
void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx)
{
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *)link_tx;
context.link_rx = (uint8_t *)link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx)
{
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *)link_tx;
context.link_rx = (uint8_t *)link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
context.user_tx = (uint8_t *)user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
context.user_rx = (uint8_t *)user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
context.user_tx = (uint8_t *)user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
context.user_rx = (uint8_t *)user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_done (void)
{
lfsm_inject_event(LFSM_EVENT_USER_DONE);
}
void lfsm_stop (void)
{
lfsm_inject_event(LFSM_EVENT_STOP);
}
void lfsm_get_link_stats (struct lfsm_link_stats * stats)
{
PIOS_DEBUG_Assert(stats);
*stats = context.stats;
}
enum lfsm_state lfsm_get_state (void)
{
return context.curr_state;
}
/*
*
* ISR Callback
*
*/
void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val)
{
if (!crc_ok) {
context.stats.rx_badcrc++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
if (!context.rx) {
/* No way to know what we just received, assume invalid */
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Recover the head and tail pointers from the message */
struct opahrs_msg_link_head * head;
struct opahrs_msg_link_tail * tail;
switch (context.user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
head = &((struct opahrs_msg_v0 *)context.rx)->head;
tail = &((struct opahrs_msg_v0 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_USER_V1:
head = &((struct opahrs_msg_v1 *)context.rx)->head;
tail = &((struct opahrs_msg_v1 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_LINK:
/* Should never be rx'ing before the link protocol version is known */
PIOS_DEBUG_Assert(0);
break;
}
/* Check for bad magic */
if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) ||
(tail->magic != OPAHRS_MSG_MAGIC_TAIL)) {
if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
context.stats.rx_badmagic_head++;
}
if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
context.stats.rx_badmagic_tail++;
}
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Good magic, find out what type of payload we've got */
switch (head->type) {
case OPAHRS_MSG_TYPE_LINK:
context.stats.rx_link++;
lfsm_inject_event (LFSM_EVENT_RX_LINK);
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
if (head->type == context.user_payload_type) {
context.stats.rx_user++;
lfsm_inject_event (LFSM_EVENT_RX_USER);
} else {
/* Mismatched user payload type */
context.stats.rx_badver++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
}
break;
default:
/* Unidentifiable payload type */
context.stats.rx_badtype++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
}
}

67
flight/AHRS/inc/ahrs_fsm.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef AHRS_FSM_H
#define AHRS_FSM_H
#include "pios_opahrs_proto.h"
enum lfsm_state {
LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
LFSM_STATE_STOPPED,
LFSM_STATE_STOPPING,
LFSM_STATE_INACTIVE,
LFSM_STATE_USER_BUSY,
LFSM_STATE_USER_BUSY_RX_PENDING,
LFSM_STATE_USER_BUSY_TX_PENDING,
LFSM_STATE_USER_BUSY_RXTX_PENDING,
LFSM_STATE_USER_RX_PENDING,
LFSM_STATE_USER_TX_PENDING,
LFSM_STATE_USER_RXTX_PENDING,
LFSM_STATE_USER_RX_ACTIVE,
LFSM_STATE_USER_TX_ACTIVE,
LFSM_STATE_USER_RXTX_ACTIVE,
LFSM_STATE_NUM_STATES, /* Must be last */
};
enum lfsm_event {
LFSM_EVENT_INIT_LINK,
LFSM_EVENT_STOP,
LFSM_EVENT_USER_SET_RX,
LFSM_EVENT_USER_SET_TX,
LFSM_EVENT_USER_DONE,
LFSM_EVENT_RX_LINK,
LFSM_EVENT_RX_USER,
LFSM_EVENT_RX_UNKNOWN,
LFSM_EVENT_NUM_EVENTS, /* Must be last */
};
struct lfsm_link_stats {
uint32_t rx_badcrc;
uint32_t rx_badmagic_head;
uint32_t rx_badmagic_tail;
uint32_t rx_link;
uint32_t rx_user;
uint32_t tx_user;
uint32_t rx_badtype;
uint32_t rx_badver;
};
extern void lfsm_init(void);
extern void lfsm_inject_event(enum lfsm_event event);
extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
extern void lfsm_get_link_stats (struct lfsm_link_stats * stats);
extern enum lfsm_state lfsm_get_state (void);
extern void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx);
extern void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx);
extern void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx);
extern void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx);
extern void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx);
extern void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx);
extern void lfsm_user_done (void);
#endif /* AHRS_FSM_H */

2
flight/AHRS/inc/pios_board.h
View File

@ -120,7 +120,7 @@ TIM8 | | | |
//
// See also pios_board.c
//-------------------------
#define PIOS_OP_SPI 0
#define PIOS_SPI_OP 0
//-------------------------
// PIOS_USART

2
flight/AHRS/pios_board.c
View File

@ -140,7 +140,7 @@ uint8_t pios_spi_num_devices = NELEMENTS(pios_spi_devs);
void PIOS_SPI_op_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(PIOS_OP_SPI);
PIOS_SPI_IRQ_Handler(PIOS_SPI_OP);
}
#endif /* PIOS_INCLUDE_SPI */

90
flight/Doc/Architecture/ahrs_spi_link_fsm.dot Normal file
View File

@ -0,0 +1,90 @@
// Regenerate using graphviz/dotty with this command
// dot -Tjpg ./flight/Doc/Architecture/ahrs_spi_link_fsm.dot > ./flight/Doc/Architecture/ahrs_spi_link_fsm.jpg
digraph ahrs_spi_protocol {
label="AHRS SPI Link State Machine"
labelloc=t
labeljust = l
// Stopped -- Undefined Wire State
{
node [ style=filled,color=lightgray ]
stopped [ shape="doublecircle" ]
stopping
}
// Wire State Inactive
{
node [ style=filled,color=lightblue ]
inactive
}
// Wire State Busy
{
node [ style=filled,color=red ]
user_busy
user_busy_tx_pending
user_busy_rx_pending
user_busy_rxtx_pending
}
{
node [ style=filled,color=yellow ]
user_tx_pending
user_rx_pending
user_rxtx_pending
}
// Wire State Ready
{
node [ style=filled,color=green ]
user_rx_active
user_tx_active
user_rxtx_active
}
//
// State transitions driven by the user
//
stopped -> inactive [ label="init link" ]
inactive -> stopping [ label="stop" ]
inactive -> user_busy_rx_pending [ label="user set rx" ]
inactive -> user_busy_tx_pending [ label="user set tx" ]
user_busy -> user_busy_tx_pending [ label="user set tx" ]
user_busy -> user_busy_rx_pending [ label="user set rx" ]
user_busy -> inactive [ label="user done" ]
user_busy -> stopping [ label="stop" ]
user_busy_tx_pending -> user_busy_rxtx_pending [ label="user set rx" ]
user_busy_tx_pending -> user_tx_pending [ label="user done" ]
user_busy_rx_pending -> user_busy_rxtx_pending [ label="user set tx" ]
user_busy_rx_pending -> user_rx_pending [ label="user done" ]
user_busy_rxtx_pending -> user_rxtx_pending [ label="user done" ]
//
// State transitions driven by messaging from the OP board
//
stopping -> stopped [ label="rx any" ]
user_tx_pending -> user_tx_active [ label="rx any" ]
user_rx_pending -> user_rx_active [ label="rx any" ]
user_rxtx_pending -> user_rxtx_active [ label="rx any" ]
// Active -> Busy
user_rx_active -> user_busy [ label="rx user" ]
user_tx_active -> inactive [ label="rx any" ]
user_rxtx_active -> user_busy [ label="rx user" ]
user_rxtx_active -> user_rx_active [ label="rx link" ]
user_rxtx_active -> user_rx_active [ label="rx unknown" ]
}

BIN
flight/Doc/Architecture/ahrs_spi_link_fsm.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 104 KiB