rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

LP-209 Update Rssi and LinkQuality values - Set Connected/Disconnected state in both sides - Add checkTimeOut()

Browse Source
This commit is contained in:
Laurent Lalanne 2016-01-03 22:15:21 +01:00
parent 648eed8a25
commit db83d1860e
1 changed files with 51 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -188,7 +188,8 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r
static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan);
static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel);
static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev);
static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
static void rfm22_updateStats(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_checkTimeOut(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev);
static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev);
static uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev);
@ -414,7 +415,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->rx_in_cb = NULL;
rfm22b_dev->tx_out_cb = NULL;
// Initialzie the PPM callback.
// Initialize the PPM callback.
rfm22b_dev->ppm_callback = NULL;
// Initialize the stats.
@ -666,8 +667,8 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats)
return;
}
// Calculate the current link quality
rfm22_calculateLinkQuality(rfm22b_dev);
// Update the current stats
rfm22_updateStats(rfm22b_dev);
// Return the stats.
*stats = rfm22b_dev->stats;
@ -1120,6 +1121,14 @@ extern void PIOS_RFM22B_PPMGet(uint32_t rfm22b_id, int16_t *channels)
return;
}
if (!rfm22_isCoordinator(rfm22b_dev) && !rfm22_isConnected(rfm22b_dev)) {
// Set the PPM channels values to INVALID
for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
channels[i] = PIOS_RCVR_INVALID;
}
return;
}
for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
channels[i] = rfm22b_dev->ppm[i];
}
@ -1998,10 +2007,10 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
if (!rfm22_isCoordinator(radio_dev) &&
radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
rfm22_synchronizeClock(radio_dev);
radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
}
radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
radio_dev->last_contact = xTaskGetTickCount();
radio_dev->stats.rssi = radio_dev->rssi_dBm;
} else {
ret_event = RADIO_EVENT_RX_COMPLETE;
}
@ -2096,17 +2105,31 @@ static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev)
}
/**
* Calculate the link quality from the packet receipt, tranmittion statistics.
* Calculate stats from the packet receipt, transmission statistics.
*
* @param[in] rfm22b_dev The device structure
*/
static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
static void rfm22_updateStats(struct pios_rfm22b_dev *rfm22b_dev)
{
// Add the RX packet statistics
rfm22b_dev->stats.rx_good = 0;
rfm22b_dev->stats.rx_corrected = 0;
rfm22b_dev->stats.rx_error = 0;
rfm22b_dev->stats.rx_failure = 0;
if(!rfm22_isConnected(rfm22b_dev)) {
// Set link_quality to 0 and Rssi to noise floor if disconnected
rfm22b_dev->stats.link_quality = 0;
rfm22b_dev->stats.rssi = -127;
return;
}
// Check if connection is timed out
if (rfm22_checkTimeOut(rfm22b_dev)) {
// Set the link state to disconnected.
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
}
for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) {
uint32_t val = rfm22b_dev->rx_packet_stats[i];
for (uint8_t j = 0; j < 16; ++j) {
@ -2134,6 +2157,16 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_failure;
}
/**
* A timeout occured ?
*
* @param[in] rfm22b_dev The device structure
*/
static bool rfm22_checkTimeOut(struct pios_rfm22b_dev *rfm22b_dev)
{
return pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >= CONNECTED_TIMEOUT;
}
/**
* Add a status value to the RX packet status array.
*
@ -2262,25 +2295,16 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind
uint8_t idx = index % rfm22b_dev->num_channels;
// Are we switching to a new channel?
if (idx != rfm22b_dev->channel_index) {
if (!rfm22_isCoordinator(rfm22b_dev) &&
pios_rfm22_time_difference_ms(rfm22b_dev->last_contact, xTaskGetTickCount()) >=
CONNECTED_TIMEOUT) {
if ((idx != rfm22b_dev->channel_index) && !rfm22_isCoordinator(rfm22b_dev) && rfm22_checkTimeOut(rfm22b_dev)) {
// Set the link state to disconnected.
if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
// Set the PPM outputs to INVALID
for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
rfm22b_dev->ppm[i] = PIOS_RCVR_INVALID;
}
}
// Update stats
rfm22_updateStats(rfm22b_dev);
// Stay on first channel.
idx = 0;
}
rfm22b_dev->channel_index = idx;
}
return rfm22b_dev->channels[idx];
}