Skip to content

Commit 19de5fb

Browse files
f5sohf5soh
committed
Merged in f5soh/librepilot/LP-518_OpenLRS_issues (pull request #519)
LP-518 OpenLRS issues Approved-by: Lalanne Laurent <[email protected]>
2 parents 36f361d + ba7f9ed commit 19de5fb

File tree

10 files changed

+160
-22
lines changed

10 files changed

+160
-22
lines changed

‎flight/modules/System/systemmod.c

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,7 @@ static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM];
131131

132132
#ifdef PIOS_INCLUDE_RFM22B
133133
static uint8_t previousRFXtalCap;
134+
static uint8_t protocol;
134135
static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
135136
#endif
136137

@@ -239,6 +240,8 @@ static void systemTask(__attribute__((unused)) void *parameters)
239240
// Initialize previousRFXtalCap used by callback
240241
OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
241242
OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
243+
// Get protocol
244+
OPLinkSettingsProtocolGet(&protocol);
242245
#endif
243246

244247
#ifdef DIAG_TASKS
@@ -332,7 +335,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
332335
oplinkStatus.RXSeq = radio_stats.rx_seq;
333336

334337
oplinkStatus.LinkState = radio_stats.link_state;
335-
} else {
338+
} else if (protocol != OPLINKSETTINGS_PROTOCOL_OPENLRS) {
336339
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
337340
}
338341
OPLinkStatusSet(&oplinkStatus);

‎flight/modules/UAVOMSPBridge/UAVOMSPBridge.c

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@
4343
#include "gpspositionsensor.h"
4444
#include "manualcontrolcommand.h"
4545
#include "manualcontrolsettings.h"
46-
#include "oplinkreceiver.h"
46+
#include "oplinkstatus.h"
4747
#include "accessorydesired.h"
4848
#include "attitudestate.h"
4949
#include "airspeedstate.h"
@@ -479,9 +479,10 @@ static void msp_send_analog(struct msp_bridge *m)
479479
ManualControlSettingsChannelGroupsGet(&channelGroups);
480480

481481
#ifdef PIOS_INCLUDE_OPLINKRCVR
482-
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
482+
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
483+
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
483484
int8_t rssi;
484-
OPLinkReceiverRSSIGet(&rssi);
485+
OPLinkStatusRSSIGet(&rssi);
485486

486487
// MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate OPLINK_LOW_RSSI to OPLINK_HIGH_RSSI -> 0-1023
487488
if (rssi < OPLINK_LOW_RSSI) {

‎flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@
5252
#include "taskinfo.h"
5353
#include "mavlink.h"
5454
#include "hwsettings.h"
55-
#include "oplinkreceiver.h"
55+
#include "oplinkstatus.h"
5656
#include "receiverstatus.h"
5757
#include "manualcontrolsettings.h"
5858

@@ -248,9 +248,10 @@ static void mavlink_send_rc_channels()
248248
ManualControlSettingsChannelGroupsGet(&channelGroups);
249249

250250
#ifdef PIOS_INCLUDE_OPLINKRCVR
251-
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
251+
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
252+
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
252253
int8_t rssi;
253-
OPLinkReceiverRSSIGet(&rssi);
254+
OPLinkStatusRSSIGet(&rssi);
254255

255256
if (rssi < OPLINK_LOW_RSSI) {
256257
rssi = OPLINK_LOW_RSSI;

‎flight/pios/common/pios_openlrs.c

Lines changed: 106 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -465,6 +465,57 @@ static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_
465465
}
466466
}
467467

468+
#ifdef OPENLRS_SIMPLE_BEACON_TONE
469+
// Basic tone
470+
static void beacon_simpletone(struct pios_openlrs_dev *openlrs_dev, uint8_t tone, int16_t len __attribute__((unused)))
471+
{
472+
DEBUG_PRINTF(2, "beacon_morse: %d\r\n", len);
473+
474+
#if defined(PIOS_LED_LINK)
475+
PIOS_LED_On(PIOS_LED_LINK);
476+
#endif /* PIOS_LED_LINK */
477+
478+
rfm22_claimBus(openlrs_dev);
479+
480+
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
481+
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
482+
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
483+
484+
GPIO_InitTypeDef init = {
485+
.GPIO_Speed = GPIO_Speed_50MHz,
486+
.GPIO_Mode = GPIO_Mode_OUT,
487+
.GPIO_OType = GPIO_OType_PP,
488+
.GPIO_PuPd = GPIO_PuPd_UP
489+
};
490+
init.GPIO_Pin = pin_source;
491+
492+
// Set MOSI to digital out for bit banging
493+
GPIO_PinAFConfig(gpio, pin_source, 0);
494+
GPIO_Init(gpio, &init);
495+
496+
int16_t cycles = (len * 50) / tone;
497+
for (int16_t i = 0; i < cycles; i++) {
498+
GPIO_SetBits(gpio, pin_source);
499+
PIOS_Thread_Sleep(tone);
500+
GPIO_ResetBits(gpio, pin_source);
501+
PIOS_Thread_Sleep(tone);
502+
}
503+
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
504+
GPIO_PinAFConfig(gpio, pin_source, remap);
505+
rfm22_releaseBus(openlrs_dev);
506+
507+
#if defined(PIOS_LED_LINK)
508+
PIOS_LED_Off(PIOS_LED_LINK);
509+
#endif /* PIOS_LED_LINK */
510+
511+
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
512+
// Update the watchdog timer
513+
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
514+
#endif /* PIOS_WDG_RFM22B */
515+
}
516+
517+
#else /* OPENLRS_SIMPLE_BEACON_TONE */
518+
468519
static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_t len __attribute__((unused))) // duration is now in half seconds.
469520
{
470521
DEBUG_PRINTF(2, "beacon_tone: %d %d\r\n", hz, len * 2);
@@ -481,7 +532,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
481532
rfm22_claimBus(openlrs_dev);
482533

483534
// This need fixed for F1
484-
#ifdef GPIO_Mode_OUT
535+
#ifdef GPIO_MODE_OUT
485536
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
486537
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
487538
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
@@ -515,7 +566,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
515566

516567
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
517568
GPIO_PinAFConfig(gpio, pin_source, remap);
518-
#endif /* ifdef GPIO_Mode_OUT */
569+
#endif /* ifdef GPIO_MODE_OUT */
519570
rfm22_releaseBus(openlrs_dev);
520571

521572
#if defined(PIOS_LED_LINK)
@@ -528,6 +579,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
528579
#endif /* PIOS_WDG_RFM22B */
529580
}
530581

582+
#endif /* OPENLRS_SIMPLE_BEACON_TONE */
531583

532584
static uint8_t beaconGetRSSI(struct pios_openlrs_dev *openlrs_dev)
533585
{
@@ -588,9 +640,40 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
588640
if (static_tone) {
589641
uint8_t i = 0;
590642
while (i++ < 20) {
643+
#ifdef OPENLRS_SIMPLE_BEACON_TONE
644+
beacon_simpletone(openlrs_dev, 2, 3);
645+
#else
591646
beacon_tone(openlrs_dev, 440, 1);
647+
#endif
592648
}
593649
} else {
650+
#ifdef OPENLRS_SIMPLE_BEACON_TONE
651+
for (uint32_t tone = 1; tone < 4; tone++) {
652+
if (tone == 1) {
653+
rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set min power 25mW
654+
} else {
655+
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
656+
}
657+
// L - --- - -
658+
beacon_simpletone(openlrs_dev, tone, 1);
659+
PIOS_Thread_Sleep(100);
660+
beacon_simpletone(openlrs_dev, tone, 3);
661+
PIOS_Thread_Sleep(100);
662+
beacon_simpletone(openlrs_dev, tone, 1);
663+
PIOS_Thread_Sleep(100);
664+
beacon_simpletone(openlrs_dev, tone, 1);
665+
PIOS_Thread_Sleep(600);
666+
// P - --- --- -
667+
beacon_simpletone(openlrs_dev, tone, 1);
668+
PIOS_Thread_Sleep(100);
669+
beacon_simpletone(openlrs_dev, tone, 3);
670+
PIOS_Thread_Sleep(100);
671+
beacon_simpletone(openlrs_dev, tone, 3);
672+
PIOS_Thread_Sleep(100);
673+
beacon_simpletone(openlrs_dev, tone, 1);
674+
PIOS_Thread_Sleep(2000);
675+
}
676+
#else /* ifdef OPENLRS_SIMPLE_BEACON_TONE */
594677
// close encounters tune
595678
// G, A, F, F(lower octave), C
596679
// octave 3: 392 440 349 175 261
@@ -612,6 +695,7 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
612695
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
613696
PIOS_Thread_Sleep(10);
614697
beacon_tone(openlrs_dev, 261, 2);
698+
#endif /* #ifdef OPENLRS_SIMPLE_BEACON_TONE */
615699
}
616700
rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY);
617701
}
@@ -829,9 +913,6 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
829913
OPLinkStatusData oplink_status;
830914
OPLinkStatusGet(&oplink_status);
831915

832-
// Update the RSSI
833-
oplink_status.RSSI = openlrs_dev->rssi;
834-
835916
timeUs = PIOS_DELAY_GetuS();
836917
timeMs = PIOS_Thread_Systime();
837918

@@ -860,8 +941,8 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
860941

861942
openlrs_dev->lastPacketTimeUs = timeUs;
862943
openlrs_dev->numberOfLostPackets = 0;
863-
oplink_status.LinkQuality <<= 1;
864-
oplink_status.LinkQuality |= 1;
944+
openlrs_dev->link_quality <<= 1;
945+
openlrs_dev->link_quality |= 1;
865946

866947
if ((openlrs_dev->rx_buf[0] & 0x3e) == 0x00) {
867948
// This flag indicates receiving PPM data
@@ -928,7 +1009,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9281009
}
9291010
tx_buf[4] = (openlrs_dev->lastAFCCvalue >> 8);
9301011
tx_buf[5] = openlrs_dev->lastAFCCvalue & 0xff;
931-
tx_buf[6] = countSetBits(oplink_status.LinkQuality & 0x7fff);
1012+
tx_buf[6] = countSetBits(openlrs_dev->link_quality & 0x7fff);
9321013
}
9331014
}
9341015

@@ -949,7 +1030,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9491030
if ((openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) + packet_timeout_us))) {
9501031
DEBUG_PRINTF(2, "OLRS WARN: Lost packet: %d\r\n", openlrs_dev->numberOfLostPackets);
9511032
// we lost packet, hop to next channel
952-
oplink_status.LinkQuality <<= 1;
1033+
openlrs_dev->link_quality <<= 1;
9531034
openlrs_dev->willhop = 1;
9541035
if (openlrs_dev->numberOfLostPackets == 0) {
9551036
openlrs_dev->linkLossTimeMs = timeMs;
@@ -961,7 +1042,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9611042
} else if ((openlrs_dev->numberOfLostPackets >= openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount))) {
9621043
DEBUG_PRINTF(2, "ORLS WARN: Trying to resync\r\n");
9631044
// hop slowly to allow resync with TX
964-
oplink_status.LinkQuality = 0;
1045+
openlrs_dev->link_quality = 0;
9651046
openlrs_dev->willhop = 1;
9661047
openlrs_dev->lastPacketTimeUs = timeUs;
9671048
}
@@ -977,7 +1058,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9771058
DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs);
9781059
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
9791060
// failsafeApply();
980-
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // beacon activating...
1061+
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_delay)) | 1; // beacon activating...
9811062
}
9821063

9831064
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) &&
@@ -993,7 +1074,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9931074
beacon_send(openlrs_dev, false); // play cool tune
9941075
init_rfm(openlrs_dev, 0); // go back to normal RX
9951076
rx_reset(openlrs_dev);
996-
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time
1077+
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_period)) | 1; // avoid 0 in time
9971078
}
9981079
}
9991080
}
@@ -1029,6 +1110,19 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
10291110
openlrs_dev->willhop = 0;
10301111
}
10311112

1113+
if (oplink_status.LinkState > OPLINKSTATUS_LINKSTATE_DISCONNECTED) {
1114+
// Convert raw Rssi to dBm
1115+
oplink_status.RSSI = (int8_t)(openlrs_dev->rssi >> 1) - 122;
1116+
1117+
// Count number of bits set in link_quality
1118+
uint8_t linkquality_bits = countSetBits(openlrs_dev->link_quality & 0x7fff);
1119+
// Translate link quality to 0 - 128 range
1120+
oplink_status.LinkQuality = (linkquality_bits + 1) * 8;
1121+
} else {
1122+
oplink_status.LinkQuality = 0;
1123+
oplink_status.RSSI = -127;
1124+
}
1125+
10321126
// Update UAVO
10331127
OPLinkStatusSet(&oplink_status);
10341128
}

‎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
/**

‎flight/pios/common/pios_oplinkrcvr.c

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -232,6 +232,8 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
232232
for (int32_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
233233
oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
234234
}
235+
oplinkrcvr_dev->oplinkreceiverdata.RSSI = -127;
236+
oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = 0;
235237
}
236238

237239
oplinkrcvr_dev->Fresh = false;

‎flight/pios/inc/pios_openlrs_priv.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -183,7 +183,8 @@ struct pios_openlrs_dev {
183183
uint8_t rx_buf[64];
184184
uint8_t tx_buf[9];
185185

186-
int8_t rssi;
186+
uint8_t rssi;
187+
uint16_t link_quality;
187188

188189
// Variables from OpenLRS for radio control
189190
uint8_t hopcount;

‎flight/targets/boards/revolution/firmware/inc/pios_config.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,7 @@
138138
#define PIOS_INCLUDE_RFM22B
139139
#define PIOS_INCLUDE_RFM22B_COM
140140
#define PIOS_INCLUDE_OPENLRS
141+
/* #define OPENLRS_SIMPLE_BEACON_TONE */
141142
/* #define PIOS_INCLUDE_PPM_OUT */
142143
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */
143144

0 commit comments

Comments
 (0)