rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

RFM22B: Finised handshacking on ACK and changed the link quality metric to use resent packets rather than missed packets.

Browse Source
This commit is contained in:
Brian Webb 2012-11-04 17:50:12 -07:00
parent 7aaa02268f
commit 0dce12e984
3 changed files with 92 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/Libraries/inc/packet_handler.h
View File

@ -63,9 +63,9 @@ typedef struct {
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle; } PHPacket, *PHPacketHandle;
#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data))
typedef struct { typedef struct {
PHPacketHeader header; PHPacketHeader header;
uint16_t seq_num;
bool ready_to_send; bool ready_to_send;
uint8_t ecc[RS_ECC_NPARITY]; uint8_t ecc[RS_ECC_NPARITY];
} PHAckNackPacket, *PHAckNackPacketHandle; } PHAckNackPacket, *PHAckNackPacketHandle;

145
flight/PiOS/Common/pios_rfm22b.c
View File

@ -189,6 +189,7 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz);
static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev);
// SPI read/write functions // SPI read/write functions
static void rfm22_assertCs(); static void rfm22_assertCs();
@ -304,6 +305,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S
.next_state = { .next_state = {
[RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA,
[RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
[RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK,
[RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS,
[RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION,
[RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK,
@ -555,7 +557,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
} }
// Initlize the link stats. // Initlize the link stats.
rfm22b_dev->prev_rx_seq_num = 0;
for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i)
rfm22b_dev->rx_packet_stats[i] = 0; rfm22b_dev->rx_packet_stats[i] = 0;
@ -590,6 +591,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->tx_packet = NULL; rfm22b_dev->tx_packet = NULL;
rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL;
rfm22b_dev->tx_seq = 0; rfm22b_dev->tx_seq = 0;
rfm22b_dev->prev_rx_seq_num = 0;
rfm22b_dev->data_packet.header.data_size = 0;
*rfm22b_id = (uint32_t)rfm22b_dev; *rfm22b_id = (uint32_t)rfm22b_dev;
g_rfm22b_dev = rfm22b_dev; g_rfm22b_dev = rfm22b_dev;
@ -828,7 +831,6 @@ static void PIOS_RFM22B_Task(void *parameters)
portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastEventTicks = xTaskGetTickCount();
portTickType lastStatusTicks = lastEventTicks; portTickType lastStatusTicks = lastEventTicks;
portTickType lastPPMTicks = lastEventTicks; portTickType lastPPMTicks = lastEventTicks;
portTickType lastConnectTicks = lastEventTicks;
while(1) while(1)
{ {
@ -873,12 +875,6 @@ static void PIOS_RFM22B_Task(void *parameters)
if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3)))
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT);
// Queue up a connection request packet if it's necessary
else if (rfm22b_dev->coordinator && (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) && (timeDifferenceMs(lastConnectTicks, curTicks) > CONNECT_ATTEMPT_PERIOD_MS))
{
lastConnectTicks = curTicks;
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE);
}
else else
{ {
// Queue up a PPM packet if it's time. // Queue up a PPM packet if it's time.
@ -891,7 +887,10 @@ static void PIOS_RFM22B_Task(void *parameters)
// Can we kick of a transmit? // Can we kick of a transmit?
if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay)
{
rfm22b_dev->tx_complete_ticks = curTicks;
rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED);
}
} }
} }
} }
@ -1186,7 +1185,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->stats.rx_good = 0; rfm22b_dev->stats.rx_good = 0;
rfm22b_dev->stats.rx_corrected = 0; rfm22b_dev->stats.rx_corrected = 0;
rfm22b_dev->stats.rx_error = 0; rfm22b_dev->stats.rx_error = 0;
rfm22b_dev->stats.rx_missed = 0; rfm22b_dev->stats.tx_resent = 0;
for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i)
{ {
uint32_t val = rfm22b_dev->rx_packet_stats[i]; uint32_t val = rfm22b_dev->rx_packet_stats[i];
@ -1203,8 +1202,8 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
case RFM22B_ERROR_RX_PACKET: case RFM22B_ERROR_RX_PACKET:
rfm22b_dev->stats.rx_error++; rfm22b_dev->stats.rx_error++;
break; break;
case RFM22B_MISSED_RX_PACKET: case RFM22B_RESENT_TX_PACKET:
rfm22b_dev->stats.rx_missed++; rfm22b_dev->stats.tx_resent++;
break; break;
} }
} }
@ -1212,9 +1211,9 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
// Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets.
// Note: This assumes that the number of packets sampled for the stats is 64. // Note: This assumes that the number of packets sampled for the stats is 64.
// Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1.
// The rage is 0 (all error or missed packets) to 128 (all good packets). // The range is 0 (all error or resent packets) to 128 (all good packets).
rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_missed; rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent;
} }
// ************************************ // ************************************
@ -1258,6 +1257,26 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev
// ************************************ // ************************************
static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev)
{
// Is there a status of PPM packet ready to send?
if (rfm22b_dev->send_ppm || rfm22b_dev->send_status)
return true;
// Is there some data ready to sent?
PHPacketHandle dp = &rfm22b_dev->data_packet;
if (dp->header.data_size > 0)
return true;
bool need_yield = false;
dp->header.data_size =
(rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data,
PH_MAX_DATA, NULL, &need_yield);
if (dp->header.data_size > 0)
return true;
return false;
}
static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
{ {
PHPacketHandle p = NULL; PHPacketHandle p = NULL;
@ -1302,23 +1321,28 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
p = &rfm22b_dev->data_packet; p = &rfm22b_dev->data_packet;
p->header.type = PACKET_TYPE_DATA; p->header.type = PACKET_TYPE_DATA;
p->header.destination_id = rfm22b_dev->destination_id; p->header.destination_id = rfm22b_dev->destination_id;
p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, if (p->header.data_size == 0)
PH_MAX_DATA, NULL, &need_yield); p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data,
PH_MAX_DATA, NULL, &need_yield);
if (p->header.data_size == 0) if (p->header.data_size == 0)
p = NULL; p = NULL;
// Don't send any data until we're connected. // Don't send any data until we're connected.
else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED)
{
p->header.data_size = 0;
return RFM22B_EVENT_RX_MODE; return RFM22B_EVENT_RX_MODE;
}
} }
if (p)
p->header.seq_num = rfm22b_dev->tx_seq++;
} }
if (!p) if (!p)
return RFM22B_EVENT_RX_MODE; return RFM22B_EVENT_RX_MODE;
// Add the error correcting code. // Add the error correcting code.
p->header.source_id = rfm22b_dev->deviceID; p->header.source_id = rfm22b_dev->deviceID;
p->header.seq_num = rfm22b_dev->tx_seq++;
encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
rfm22b_dev->tx_packet = p; rfm22b_dev->tx_packet = p;
@ -1549,46 +1573,47 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de
static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len) static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len)
{ {
static bool first_packet = true;
uint16_t seq_num = p->header.seq_num;
uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num;
uint16_t missed_packets = ((seq_num < prev_seq_num) ? (0xffff - prev_seq_num + seq_num) : (seq_num - prev_seq_num)) - 1;
rfm22b_dev->prev_rx_seq_num = seq_num;
if (first_packet)
{
missed_packets = 0;
first_packet = false;
}
// Add any missed packets into the stats.
for ( ; missed_packets > 0; --missed_packets)
rfm22b_add_rx_status(rfm22b_dev, RFM22B_MISSED_RX_PACKET);
// Attempt to correct any errors in the packet. // Attempt to correct any errors in the packet.
decode_data((unsigned char*)p, rx_len); decode_data((unsigned char*)p, rx_len);
// Check if there were any errors bool good_packet = check_syndrome() == 0;
bool rx_error = check_syndrome() != 0; bool corrected_packet = false;
if(rx_error) // We have an error. Try to correct it.
if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0))
// We corrected it
corrected_packet = true;
// Add any missed packets into the stats.
bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_NACK));
if (!ack_nack_packet && (good_packet || corrected_packet) && (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED))
{ {
// We have an error. Try to correct it. static bool first_time = true;
if (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) == 0) uint16_t seq_num = p->header.seq_num;
{ uint16_t missed_packets = 0;
// We couldn't correct the error, so drop the packet. if (first_time)
rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); first_time = false;
}
else else
{ {
// We corrected the error. uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num;
rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); if (seq_num > prev_seq_num)
rx_error = false; missed_packets = seq_num - prev_seq_num - 1;
} }
rfm22b_dev->stats.rx_missed += missed_packets;
rfm22b_dev->prev_rx_seq_num = seq_num;
} }
else
rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET);
return !rx_error; // Set the packet status
if (good_packet)
rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET);
else if(corrected_packet)
// We corrected the error.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET);
else
// We couldn't correct the error, so drop the packet.
rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET);
return good_packet || corrected_packet;
} }
static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
@ -1689,6 +1714,8 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
break; break;
} }
} }
else
ret_event = RFM22B_EVENT_RX_ERROR;
rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->rx_buffer_wr = 0;
} }
@ -1769,10 +1796,9 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id;
aph->header.type = PACKET_TYPE_ACK; aph->header.type = PACKET_TYPE_ACK;
aph->header.data_size = 0; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
//aph->ready_to_send = (uxQueueMessagesWaiting(rfm22b_dev->packetQueue) > 0); aph->ready_to_send = rfm22_ready_to_send(rfm22b_dev);
aph->ready_to_send = false;
rfm22b_dev->tx_packet = (PHPacketHandle)aph; rfm22b_dev->tx_packet = (PHPacketHandle)aph;
return RFM22B_EVENT_START_TRANSFER; return RFM22B_EVENT_START_TRANSFER;
} }
@ -1786,8 +1812,8 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id;
aph->header.type = PACKET_TYPE_NACK; aph->header.type = PACKET_TYPE_NACK;
aph->header.data_size = 0; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
rfm22b_dev->tx_packet = (PHPacketHandle)aph; rfm22b_dev->tx_packet = (PHPacketHandle)aph;
return RFM22B_EVENT_START_TRANSFER; return RFM22B_EVENT_START_TRANSFER;
} }
@ -1799,16 +1825,22 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
{ {
PHPacketHandle prev = rfm22b_dev->prev_tx_packet; PHPacketHandle prev = rfm22b_dev->prev_tx_packet;
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet);
// Clear the previous TX packet. // Clear the previous TX packet.
rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL;
// Was this a connection request? // Was this a connection request?
if (prev->header.type == PACKET_TYPE_CON_REQUEST) switch (prev->header.type)
{
case PACKET_TYPE_CON_REQUEST:
return RFM22B_EVENT_CONNECTION_ACCEPTED; return RFM22B_EVENT_CONNECTION_ACCEPTED;
case PACKET_TYPE_DATA:
rfm22b_dev->data_packet.header.data_size = 0;
break;
}
// Should we try to start another TX? // Should we try to start another TX?
PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet);
if (aph->ready_to_send) if (aph->ready_to_send)
return RFM22B_EVENT_RX_MODE; return RFM22B_EVENT_RX_MODE;
else else
@ -1824,7 +1856,8 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d
// Resend the previous TX packet. // Resend the previous TX packet.
rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet;
rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL;
rfm22b_dev->stats.tx_resent++; if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET);
return RFM22B_EVENT_START_TRANSFER; return RFM22B_EVENT_START_TRANSFER;
} }

3
flight/PiOS/inc/pios_rfm22b_priv.h
View File

@ -615,6 +615,7 @@ enum pios_rfm22b_event {
RFM22B_EVENT_PREAMBLE_DETECTED, RFM22B_EVENT_PREAMBLE_DETECTED,
RFM22B_EVENT_SYNC_DETECTED, RFM22B_EVENT_SYNC_DETECTED,
RFM22B_EVENT_RX_COMPLETE, RFM22B_EVENT_RX_COMPLETE,
RFM22B_EVENT_RX_ERROR,
RFM22B_EVENT_STATUS_RECEIVED, RFM22B_EVENT_STATUS_RECEIVED,
RFM22B_EVENT_SEND_PACKET, RFM22B_EVENT_SEND_PACKET,
RFM22B_EVENT_START_TRANSFER, RFM22B_EVENT_START_TRANSFER,
@ -633,7 +634,7 @@ enum pios_rfm22b_rx_packet_status {
RFM22B_GOOD_RX_PACKET = 0x00, RFM22B_GOOD_RX_PACKET = 0x00,
RFM22B_CORRECTED_RX_PACKET = 0x01, RFM22B_CORRECTED_RX_PACKET = 0x01,
RFM22B_ERROR_RX_PACKET = 0x2, RFM22B_ERROR_RX_PACKET = 0x2,
RFM22B_MISSED_RX_PACKET = 0x3 RFM22B_RESENT_TX_PACKET = 0x3
}; };
typedef struct { typedef struct {