rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
Code Issues Releases Activity
7a8424e98e
LibrePilot/flight/AHRS/ahrs_fsm.c
peabody124 4a4932e5ff AHRS/FSM: Small initialization detail to make compile with -Os 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1610 ebee16cc-31ac-478f-84a7-5cbb03baadba
2010-09-13 19:04:37 +00:00

621 lines
18 KiB
C
Raw Blame History

#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 = NULL;
struct opahrs_msg_link_tail * tail = NULL;
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);
}
}
Reference in New Issue View Git Blame Copy Permalink