diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c
index 2cf86e751..d1ff52fd3 100644
--- a/flight/modules/System/systemmod.c
+++ b/flight/modules/System/systemmod.c
@@ -292,18 +292,19 @@ static void systemTask(__attribute__((unused)) void *parameters)
static uint16_t prev_tx_seq = 0;
static uint16_t prev_rx_seq = 0;
- oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
- oplinkStatus.RxGood = radio_stats.rx_good;
- oplinkStatus.RxCorrected = radio_stats.rx_corrected;
- oplinkStatus.RxErrors = radio_stats.rx_error;
- oplinkStatus.RxMissed = radio_stats.rx_missed;
- oplinkStatus.RxFailure = radio_stats.rx_failure;
- oplinkStatus.TxDropped = radio_stats.tx_dropped;
- oplinkStatus.TxFailure = radio_stats.tx_failure;
- oplinkStatus.Resets = radio_stats.resets;
- oplinkStatus.Timeouts = radio_stats.timeouts;
+ oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
+ oplinkStatus.RxGood = radio_stats.rx_good;
+ oplinkStatus.RxCorrected = radio_stats.rx_corrected;
+ oplinkStatus.RxErrors = radio_stats.rx_error;
+ oplinkStatus.RxMissed = radio_stats.rx_missed;
+ oplinkStatus.RxFailure = radio_stats.rx_failure;
+ oplinkStatus.TxDropped = radio_stats.tx_dropped;
+ oplinkStatus.TxFailure = radio_stats.tx_failure;
+ oplinkStatus.Resets = radio_stats.resets;
+ oplinkStatus.Timeouts = radio_stats.timeouts;
oplinkStatus.RSSI = radio_stats.rssi;
- oplinkStatus.LinkQuality = radio_stats.link_quality;
+ oplinkStatus.LinkQuality = radio_stats.link_quality;
+ oplinkStatus.AFCCorrection = radio_stats.afc_correction;
if (first_time) {
first_time = false;
} else {
diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c
index 4d4a9aa46..07254fd29 100644
--- a/flight/pios/common/pios_rfm22b.c
+++ b/flight/pios/common/pios_rfm22b.c
@@ -427,16 +427,17 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
rfm22b_dev->stats.packets_per_sec = 0;
rfm22b_dev->stats.rx_good = 0;
rfm22b_dev->stats.rx_corrected = 0;
- rfm22b_dev->stats.rx_error = 0;
- rfm22b_dev->stats.rx_missed = 0;
- rfm22b_dev->stats.tx_dropped = 0;
- rfm22b_dev->stats.resets = 0;
- rfm22b_dev->stats.timeouts = 0;
- rfm22b_dev->stats.link_quality = 0;
+ rfm22b_dev->stats.rx_error = 0;
+ rfm22b_dev->stats.rx_missed = 0;
+ rfm22b_dev->stats.tx_dropped = 0;
+ rfm22b_dev->stats.resets = 0;
+ rfm22b_dev->stats.timeouts = 0;
+ rfm22b_dev->stats.link_quality = 0;
rfm22b_dev->stats.rssi = 0;
- rfm22b_dev->stats.tx_seq = 0;
- rfm22b_dev->stats.rx_seq = 0;
- rfm22b_dev->stats.tx_failure = 0;
+ rfm22b_dev->stats.afc_correction = 0;
+ rfm22b_dev->stats.tx_seq = 0;
+ rfm22b_dev->stats.rx_seq = 0;
+ rfm22b_dev->stats.tx_failure = 0;
// Set the frequency band
switch (band) {
@@ -1072,13 +1073,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id)
// 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;
+ int16_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 |= (int16_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);
+ rfm22b_dev->afc_correction_Hz = afc_corr;
// read rx signal strength .. 45 = -100dBm, 205 = -20dBm
uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
@@ -1699,8 +1700,8 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
// Set the frequency hopping step size.
rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, freq_hop_step_size);
- // frequency hopping channel (0-255)
- rfm22b_dev->frequency_step_size = 156.25f * hbsel;
+ // frequency step
+ rfm22b_dev->frequency_step_size = 156.25f * (hbsel + 1);
// frequency hopping channel (0-255)
rfm22b_dev->channel = init_chan;
@@ -2063,9 +2064,10 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
rfm22_synchronizeClock(radio_dev);
}
- radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
- radio_dev->last_contact = xTaskGetTickCount();
+ radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
+ radio_dev->last_contact = xTaskGetTickCount();
radio_dev->stats.rssi = radio_dev->rssi_dBm;
+ radio_dev->stats.afc_correction = radio_dev->afc_correction_Hz;
} else {
ret_event = RADIO_EVENT_RX_COMPLETE;
}
diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h
index 6158771ad..14b6fbaa4 100644
--- a/flight/pios/inc/pios_rfm22b.h
+++ b/flight/pios/inc/pios_rfm22b.h
@@ -96,7 +96,7 @@ struct rfm22b_stats {
uint8_t timeouts;
uint8_t link_quality;
int8_t rssi;
- int8_t afc_correction;
+ int32_t afc_correction;
uint8_t link_state;
};
diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h
index 5a66bd180..9d267d7d6 100644
--- a/flight/pios/inc/pios_rfm22b_priv.h
+++ b/flight/pios/inc/pios_rfm22b_priv.h
@@ -297,7 +297,7 @@ struct pios_rfm22b_dev {
// current frequency hop channel index
uint8_t channel_index;
// afc correction reading (in Hz)
- int8_t afc_correction_Hz;
+ int32_t afc_correction_Hz;
// The packet timers.
portTickType packet_start_ticks;
diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml
index fdb3d5053..7d9605c61 100644
--- a/shared/uavobjectdefinition/oplinkstatus.xml
+++ b/shared/uavobjectdefinition/oplinkstatus.xml
@@ -19,6 +19,7 @@
+