rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

OP-932: Continued cleanup and modularization of radio code. Also fixed a bug that held the SPI lock when an error occured in an Rx.

Browse Source
This commit is contained in:
Brian Webb 2013-05-01 02:47:53 +01:00
parent ef18319306
commit 2201d9d2bb
2 changed files with 152 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

318
flight/pios/common/pios_rfm22b.c
View File

@ -158,9 +158,9 @@ static const uint8_t OUT_FF[64] = {
static void pios_rfm22_task(void *parameters);
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR);
static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
@ -270,7 +270,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
[RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
[RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_RX_ERROR] = RADIO_STATE_SENDING_NACK,
[RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS,
[RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION,
[RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK,
@ -282,16 +281,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RADIO_STATE_RX_FAILURE] = {
.entry_fn = rfm22_rxFailure,
.next_state = {
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
},
[RADIO_STATE_RECEIVING_ACK] = {
.entry_fn = rfm22_receiveAck,
.next_state = {
@ -361,6 +350,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
.entry_fn = rfm22_sendAck,
.next_state = {
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
@ -370,6 +360,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
.entry_fn = rfm22_sendNack,
.next_state = {
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
@ -379,6 +370,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
.entry_fn = rfm22_timeout,
.next_state = {
[RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
[RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
@ -387,7 +379,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST
[RADIO_STATE_ERROR] = {
.entry_fn = rfm22_error,
.next_state = {
[RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
[RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
[RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
},
@ -793,7 +784,7 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) {
}
// Are we already in Rx mode?
if (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE || rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) {
if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT)) {
return false;
}
rfm22b_dev->rx_packet_handle = p;
@ -982,153 +973,144 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) {
return PIOS_RFM22B_INT_FAILURE;
}
uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle);
pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS;
// Read the device status registers
if (!pios_rfm22_readStatus(rfm22b_dev)) {
return RADIO_EVENT_FAILURE;
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// FIFO under/over flow error. Restart RX mode.
if (rfm22b_dev->status_regs.int_status_1.fifo_underoverflow_error ||
rfm22b_dev->status_regs.int_status_1.crc_error) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
switch (rfm22b_dev->rfm22b_state) {
// Valid packet received
if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
case RFM22B_STATE_RX_WAIT:
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Valid preamble detected
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
RX_LED_ON;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// We detected the preamble, now wait for sync.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
}
break;
case RFM22B_STATE_RX_WAIT_SYNC:
// Sync word detected
if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if (!rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Waiting for the preamble timed out.
return PIOS_RFM22B_INT_FAILURE;
}
break;
case RFM22B_STATE_RX_MODE:
// Valid packet received
if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
}
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Is there a length error?
if (rfm22b_dev->rx_buffer_wr != len) {
return PIOS_RFM22B_INT_FAILURE;
}
if (rfm22b_dev->rx_buffer_wr == 0) {
return RADIO_EVENT_RX_COMPLETE;
}
// Increment the total byte received count.
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
// We're finished with Rx mode
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
return PIOS_RFM22B_RX_COMPLETE;
} else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
// RX FIFO almost full, it needs emptying
// read data from the rf chips FIFO buffer
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
return PIOS_RFM22B_INT_FAILURE;
}
// read the total length of the packet data
uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// their must still be data in the RX FIFO we need to get
if (rfm22b_dev->rx_buffer_wr < len) {
int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
rfm22_deassertCs(rfm22b_dev);
}
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
return PIOS_RFM22B_INT_SUCCESS;
// Is there a length error?
if (rfm22b_dev->rx_buffer_wr != len) {
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
break;
// Increment the total byte received count.
rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
default:
// We're finished with Rx mode
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
ret = PIOS_RFM22B_RX_COMPLETE;
} else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
// RX FIFO almost full, it needs emptying
// read data from the rf chips FIFO buffer
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// Read the total length of the packet data
uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
// The received packet is going to be larger than the specified length
if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
rfm22_releaseBus(rfm22b_dev);
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
// Fetch the data from the RX FIFO
rfm22_assertCs(rfm22b_dev);
PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
rfm22_deassertCs(rfm22b_dev);
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Make sure that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Valid preamble detected
RX_LED_ON;
#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
D2_LED_ON;
#endif // PIOS_RFM22B_DEBUG_ON_TELEM
// We detected the preamble, now wait for sync.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
} else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
// Sync word detected
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// read the 10-bit signed afc correction value
// bits 9 to 2
uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
// bits 1 & 0
afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
afc_correction >>= 6;
// convert the afc value to Hz
int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
// convert to dBm
rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
// Release the SPI bus.
rfm22_releaseBus(rfm22b_dev);
// Indicate that we're in RX mode.
rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
} else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
// Waiting for the preamble timed out.
rfm22_rxFailure(rfm22b_dev);
return PIOS_RFM22B_INT_FAILURE;
}
return PIOS_RFM22B_INT_SUCCESS;
// Set the packet start time if necessary.
if ((rfm22b_dev->packet_start_ticks == 0) &&
((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) {
rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
if (rfm22b_dev->packet_start_ticks == 0)
rfm22b_dev->packet_start_ticks = 1;
}
return ret;
}
/**
@ -1272,20 +1254,23 @@ static void pios_rfm22_task(void *parameters)
*/
static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR)
{
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (inISR) {
portBASE_TYPE pxHigherPriorityTaskWoken;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) {
// Store the event.
portBASE_TYPE pxHigherPriorityTaskWoken1;
if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
portBASE_TYPE pxHigherPriorityTaskWoken2;
if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
}
portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken);
portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
} else {
// Store the event.
if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
return;
// Signal the semaphore to wake up the handler thread.
if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) {
// Something went fairly seriously wrong
rfm22b_dev->errors++;
@ -1304,7 +1289,7 @@ static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_d
{
// No event
if (event == RADIO_EVENT_NUM_EVENTS) {
if (event >= RADIO_EVENT_NUM_EVENTS) {
return RADIO_EVENT_NUM_EVENTS;
}
@ -1776,6 +1761,24 @@ static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
return true;
}
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
}
/*****************************************************************************
* Radio Transmit and Receive functions.
@ -2080,7 +2083,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_
}
} else {
ret_event = RADIO_EVENT_RX_ERROR;
ret_event = RADIO_EVENT_RX_COMPLETE;
}
return ret_event;
@ -2117,7 +2120,7 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev)
case PIOS_RFM22B_INT_FAILURE:
ret_event = RADIO_EVENT_RX_ERROR;
ret_event = RADIO_EVENT_RX_MODE;
break;
default:
@ -2605,24 +2608,6 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev)
* Error Handling Functions
*****************************************************************************/
/**
* Recover from a failure in receiving a packet.
*
* @param[in] rfm22b_dev The device structure
* @return enum pios_radio_event The next event to inject
*/
static enum pios_radio_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.rx_failure++;
rfm22b_dev->rx_buffer_wr = 0;
rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
if (rfm22b_dev->rx_complete_ticks == 0) {
rfm22b_dev->rx_complete_ticks = 1;
}
return RADIO_EVENT_RX_MODE;
}
/**
* Recover from a transmit failure.
*
@ -2632,6 +2617,7 @@ static enum pios_radio_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
{
rfm22b_dev->stats.tx_failure++;
rfm22b_dev->packet_start_ticks = 0;
rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
return RADIO_EVENT_TX_START;
}

1
flight/pios/inc/pios_rfm22b_priv.h
View File

@ -566,7 +566,6 @@ enum pios_radio_event {
RADIO_EVENT_ACK_TIMEOUT,
RADIO_EVENT_RX_MODE,
RADIO_EVENT_RX_COMPLETE,
RADIO_EVENT_RX_ERROR,
RADIO_EVENT_STATUS_RECEIVED,
RADIO_EVENT_TX_START,
RADIO_EVENT_FAILURE,