36
36
37
37
#include <uavobjectmanager.h>
38
38
#include <oplinkreceiver.h>
39
+ #include <oplinkstatus.h>
39
40
#include <pios_openlrs_priv.h>
40
41
#include <pios_openlrs_rcvr_priv.h>
41
42
45
46
static int32_t PIOS_OpenLRS_Rcvr_Get (uint32_t rcvr_id , uint8_t channel );
46
47
static void PIOS_OpenLRS_Rcvr_Supervisor (uint32_t ppm_id );
47
48
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 );
48
50
49
51
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
51
54
};
52
55
53
56
/* Local Variables */
@@ -58,6 +61,8 @@ enum pios_openlrs_rcvr_dev_magic {
58
61
struct pios_openlrs_rcvr_dev {
59
62
enum pios_openlrs_rcvr_dev_magic magic ;
60
63
int16_t channels [OPENLRS_PPM_NUM_CHANNELS ];
64
+ int8_t RSSI ;
65
+ uint8_t LinkQuality ;
61
66
uint8_t supv_timer ;
62
67
bool fresh ;
63
68
};
@@ -134,6 +139,15 @@ static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16
134
139
openlrs_rcvr_dev -> channels [i ] = channels [i ];
135
140
}
136
141
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
+
137
151
openlrs_rcvr_update_uavo (openlrs_rcvr_dev );
138
152
139
153
// let supervisor know we have new data
@@ -191,6 +205,8 @@ static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t openlrs_rcvr_id)
191
205
i ++ ) {
192
206
openlrs_rcvr_dev -> channels [i ] = PIOS_RCVR_TIMEOUT ;
193
207
}
208
+ openlrs_rcvr_dev -> RSSI = -127 ;
209
+ openlrs_rcvr_dev -> LinkQuality = 0 ;
194
210
}
195
211
196
212
openlrs_rcvr_dev -> fresh = false;
@@ -211,9 +227,27 @@ static void openlrs_rcvr_update_uavo(struct pios_openlrs_rcvr_dev *rcvr_dev)
211
227
for (int i = OPENLRS_PPM_NUM_CHANNELS - 1 ; i < OPLINKRECEIVER_CHANNEL_NUMELEM ; i ++ ) {
212
228
rcvr .Channel [i ] = PIOS_RCVR_INVALID ;
213
229
}
230
+
231
+ rcvr .RSSI = rcvr_dev -> RSSI ;
232
+ rcvr .LinkQuality = rcvr_dev -> LinkQuality ;
233
+
214
234
OPLinkReceiverSet (& rcvr );
215
235
}
216
236
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
+
217
251
#endif /* PIOS_INCLUDE_OPENLRS */
218
252
219
253
/**
0 commit comments