Skip to content

Commit 6ab47fd

Browse files
f5sohf5soh
committed
LP-518 OpenLRS: Simple beacon tone, cpu free. Apply to Sparky2 and Revo
1 parent 794c182 commit 6ab47fd

4 files changed

Lines changed: 91 additions & 5 deletions

File tree

‎flight/pios/common/pios_openlrs.c‎

Lines changed: 88 additions & 4 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
}
@@ -974,7 +1058,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9741058
DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs);
9751059
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
9761060
// failsafeApply();
977-
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...
9781062
}
9791063

9801064
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) &&
@@ -990,7 +1074,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
9901074
beacon_send(openlrs_dev, false); // play cool tune
9911075
init_rfm(openlrs_dev, 0); // go back to normal RX
9921076
rx_reset(openlrs_dev);
993-
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
9941078
}
9951079
}
9961080
}

‎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

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -141,6 +141,7 @@
141141
#define PIOS_INCLUDE_RFM22B
142142
#define PIOS_INCLUDE_RFM22B_COM
143143
#define PIOS_INCLUDE_OPENLRS
144+
#define OPENLRS_SIMPLE_BEACON_TONE
144145
/* #define PIOS_INCLUDE_PPM_OUT */
145146
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */
146147

‎shared/uavobjectdefinition/oplinksettings.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
<!-- beacon options -->
4040
<field name="BeaconFrequency" units="Hz" type="uint32" elements="1" defaultvalue="462712500"/>
4141
<field name="BeaconDelay" units="s" type="uint8" elements="1" defaultvalue="30"/>
42-
<field name="BeaconPeriod" units="s" type="uint8" elements="1" defaultvalue="15"/>
42+
<field name="BeaconPeriod" units="s" type="uint8" elements="1" defaultvalue="30"/>
4343

4444
<access gcs="readwrite" flight="readwrite"/>
4545
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

0 commit comments

Comments
 (0)