Skip to content

Commit 8e861cb

Browse files
committed
LP-518 Add Quality_Get to OpenLRS receiver and update OPlink_Receiver with Rssi and LinkQuality
1 parent bc7f576 commit 8e861cb

File tree

1 file changed

+35
-1
lines changed

1 file changed

+35
-1
lines changed

flight/pios/common/pios_openlrs_rcvr.c

Lines changed: 35 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636

3737
#include <uavobjectmanager.h>
3838
#include <oplinkreceiver.h>
39+
#include <oplinkstatus.h>
3940
#include <pios_openlrs_priv.h>
4041
#include <pios_openlrs_rcvr_priv.h>
4142

@@ -45,9 +46,11 @@
4546
static int32_t PIOS_OpenLRS_Rcvr_Get(uint32_t rcvr_id, uint8_t channel);
4647
static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t ppm_id);
4748
static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16_t *channels);
49+
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id);
4850

4951
const struct pios_rcvr_driver pios_openlrs_rcvr_driver = {
50-
.read = PIOS_OpenLRS_Rcvr_Get,
52+
.read = PIOS_OpenLRS_Rcvr_Get,
53+
.get_quality = PIOS_OpenLRSRCVR_Quality_Get
5154
};
5255

5356
/* Local Variables */
@@ -58,6 +61,8 @@ enum pios_openlrs_rcvr_dev_magic {
5861
struct pios_openlrs_rcvr_dev {
5962
enum pios_openlrs_rcvr_dev_magic magic;
6063
int16_t channels[OPENLRS_PPM_NUM_CHANNELS];
64+
int8_t RSSI;
65+
uint8_t LinkQuality;
6166
uint8_t supv_timer;
6267
bool fresh;
6368
};
@@ -134,6 +139,15 @@ static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16
134139
openlrs_rcvr_dev->channels[i] = channels[i];
135140
}
136141

142+
// Update the RSSI and quality fields.
143+
int8_t rssi;
144+
OPLinkStatusRSSIGet(&rssi);
145+
openlrs_rcvr_dev->RSSI = rssi;
146+
uint16_t quality;
147+
OPLinkStatusLinkQualityGet(&quality);
148+
// Link quality is 0-128, so scale it down to 0-100
149+
openlrs_rcvr_dev->LinkQuality = quality * 100 / 128;
150+
137151
openlrs_rcvr_update_uavo(openlrs_rcvr_dev);
138152

139153
// let supervisor know we have new data
@@ -191,6 +205,8 @@ static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t openlrs_rcvr_id)
191205
i++) {
192206
openlrs_rcvr_dev->channels[i] = PIOS_RCVR_TIMEOUT;
193207
}
208+
openlrs_rcvr_dev->RSSI = -127;
209+
openlrs_rcvr_dev->LinkQuality = 0;
194210
}
195211

196212
openlrs_rcvr_dev->fresh = false;
@@ -211,9 +227,27 @@ static void openlrs_rcvr_update_uavo(struct pios_openlrs_rcvr_dev *rcvr_dev)
211227
for (int i = OPENLRS_PPM_NUM_CHANNELS - 1; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
212228
rcvr.Channel[i] = PIOS_RCVR_INVALID;
213229
}
230+
231+
rcvr.RSSI = rcvr_dev->RSSI;
232+
rcvr.LinkQuality = rcvr_dev->LinkQuality;
233+
214234
OPLinkReceiverSet(&rcvr);
215235
}
216236

237+
238+
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id)
239+
{
240+
/* Recover our device context */
241+
struct pios_openlrs_rcvr_dev *openlrs_rcvr_dev = (struct pios_openlrs_rcvr_dev *)openlrs_rcvr_id;
242+
243+
if (!PIOS_OpenLRS_Rcvr_Validate(openlrs_rcvr_dev)) {
244+
/* Invalid device specified */
245+
return 0;
246+
}
247+
248+
return openlrs_rcvr_dev->LinkQuality;
249+
}
250+
217251
#endif /* PIOS_INCLUDE_OPENLRS */
218252

219253
/**

0 commit comments

Comments
 (0)