rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

LP-518 Add Quality_Get to OpenLRS receiver and update OPlink_Receiver with Rssi and LinkQuality

Browse Source
This commit is contained in:
Laurent Lalanne 2018-10-15 00:09:37 +02:00
parent bc7f576b20
commit 8e861cbcae
1 changed files with 35 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
flight/pios/common/pios_openlrs_rcvr.c
View File

@ -36,6 +36,7 @@
#include <uavobjectmanager.h>
#include <oplinkreceiver.h>
#include <oplinkstatus.h>
#include <pios_openlrs_priv.h>
#include <pios_openlrs_rcvr_priv.h>
@ -45,9 +46,11 @@
static int32_t PIOS_OpenLRS_Rcvr_Get(uint32_t rcvr_id, uint8_t channel);
static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t ppm_id);
static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16_t *channels);
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id);
const struct pios_rcvr_driver pios_openlrs_rcvr_driver = {
.read = PIOS_OpenLRS_Rcvr_Get,
.read = PIOS_OpenLRS_Rcvr_Get,
.get_quality = PIOS_OpenLRSRCVR_Quality_Get
};
/* Local Variables */
@ -58,6 +61,8 @@ enum pios_openlrs_rcvr_dev_magic {
struct pios_openlrs_rcvr_dev {
enum pios_openlrs_rcvr_dev_magic magic;
int16_t channels[OPENLRS_PPM_NUM_CHANNELS];
int8_t RSSI;
uint8_t LinkQuality;
uint8_t supv_timer;
bool fresh;
};
@ -134,6 +139,15 @@ static void PIOS_OpenLRS_Rcvr_ppm_callback(uint32_t openlrs_rcvr_id, const int16
openlrs_rcvr_dev->channels[i] = channels[i];
}
// Update the RSSI and quality fields.
int8_t rssi;
OPLinkStatusRSSIGet(&rssi);
openlrs_rcvr_dev->RSSI = rssi;
uint16_t quality;
OPLinkStatusLinkQualityGet(&quality);
// Link quality is 0-128, so scale it down to 0-100
openlrs_rcvr_dev->LinkQuality = quality * 100 / 128;
openlrs_rcvr_update_uavo(openlrs_rcvr_dev);
// let supervisor know we have new data
@ -191,6 +205,8 @@ static void PIOS_OpenLRS_Rcvr_Supervisor(uint32_t openlrs_rcvr_id)
i++) {
openlrs_rcvr_dev->channels[i] = PIOS_RCVR_TIMEOUT;
}
openlrs_rcvr_dev->RSSI = -127;
openlrs_rcvr_dev->LinkQuality = 0;
}
openlrs_rcvr_dev->fresh = false;
@ -211,9 +227,27 @@ static void openlrs_rcvr_update_uavo(struct pios_openlrs_rcvr_dev *rcvr_dev)
for (int i = OPENLRS_PPM_NUM_CHANNELS - 1; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
rcvr.Channel[i] = PIOS_RCVR_INVALID;
}
rcvr.RSSI = rcvr_dev->RSSI;
rcvr.LinkQuality = rcvr_dev->LinkQuality;
OPLinkReceiverSet(&rcvr);
}
static uint8_t PIOS_OpenLRSRCVR_Quality_Get(uint32_t openlrs_rcvr_id)
{
/* Recover our device context */
struct pios_openlrs_rcvr_dev *openlrs_rcvr_dev = (struct pios_openlrs_rcvr_dev *)openlrs_rcvr_id;
if (!PIOS_OpenLRS_Rcvr_Validate(openlrs_rcvr_dev)) {
/* Invalid device specified */
return 0;
}
return openlrs_rcvr_dev->LinkQuality;
}
#endif /* PIOS_INCLUDE_OPENLRS */
/**