rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-28 06:24:10 +01:00
Code Issues Releases Activity

Merged in f5soh/librepilot/LP-518_OpenLRS_issues (pull request #519)

Browse Source 
LP-518 OpenLRS issues

Approved-by: Lalanne Laurent <f5soh@free.fr>
This commit is contained in:
Lalanne Laurent 2019-02-21 21:43:08 +00:00
commit 19de5fbf44
10 changed files with 160 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/System/systemmod.c
View File

@ -131,6 +131,7 @@ static uint8_t i2c_error_activity[PIOS_I2C_ERROR_COUNT_NUMELEM];
#ifdef PIOS_INCLUDE_RFM22B
static uint8_t previousRFXtalCap;
static uint8_t protocol;
static void oplinkSettingsUpdatedCb(UAVObjEvent *ev);
#endif
@ -239,6 +240,8 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Initialize previousRFXtalCap used by callback
OPLinkSettingsRFXtalCapGet(&previousRFXtalCap);
OPLinkSettingsConnectCallback(oplinkSettingsUpdatedCb);
// Get protocol
OPLinkSettingsProtocolGet(&protocol);
#endif
#ifdef DIAG_TASKS
@ -332,7 +335,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
oplinkStatus.RXSeq = radio_stats.rx_seq;
oplinkStatus.LinkState = radio_stats.link_state;
} else {
} else if (protocol != OPLINKSETTINGS_PROTOCOL_OPENLRS) {
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
}
OPLinkStatusSet(&oplinkStatus);

7
flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
View File

@ -43,7 +43,7 @@
#include "gpspositionsensor.h"
#include "manualcontrolcommand.h"
#include "manualcontrolsettings.h"
#include "oplinkreceiver.h"
#include "oplinkstatus.h"
#include "accessorydesired.h"
#include "attitudestate.h"
#include "airspeedstate.h"
@ -479,9 +479,10 @@ static void msp_send_analog(struct msp_bridge *m)
ManualControlSettingsChannelGroupsGet(&channelGroups);
#ifdef PIOS_INCLUDE_OPLINKRCVR
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
int8_t rssi;
OPLinkReceiverRSSIGet(&rssi);
OPLinkStatusRSSIGet(&rssi);
// 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
if (rssi < OPLINK_LOW_RSSI) {

7
flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c
View File

@ -52,7 +52,7 @@
#include "taskinfo.h"
#include "mavlink.h"
#include "hwsettings.h"
#include "oplinkreceiver.h"
#include "oplinkstatus.h"
#include "receiverstatus.h"
#include "manualcontrolsettings.h"
@ -248,9 +248,10 @@ static void mavlink_send_rc_channels()
ManualControlSettingsChannelGroupsGet(&channelGroups);
#ifdef PIOS_INCLUDE_OPLINKRCVR
if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
if ((channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) ||
(channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS)) {
int8_t rssi;
OPLinkReceiverRSSIGet(&rssi);
OPLinkStatusRSSIGet(&rssi);
if (rssi < OPLINK_LOW_RSSI) {
rssi = OPLINK_LOW_RSSI;

118
flight/pios/common/pios_openlrs.c
View File

@ -465,6 +465,57 @@ static void tx_packet(struct pios_openlrs_dev *openlrs_dev, uint8_t *pkt, uint8_
}
}
#ifdef OPENLRS_SIMPLE_BEACON_TONE
// Basic tone
static void beacon_simpletone(struct pios_openlrs_dev *openlrs_dev, uint8_t tone, int16_t len __attribute__((unused)))
{
DEBUG_PRINTF(2, "beacon_morse: %d\r\n", len);
#if defined(PIOS_LED_LINK)
PIOS_LED_On(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
rfm22_claimBus(openlrs_dev);
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
uint8_t remap = openlrs_dev->cfg.spi_cfg->remap;
GPIO_InitTypeDef init = {
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
};
init.GPIO_Pin = pin_source;
// Set MOSI to digital out for bit banging
GPIO_PinAFConfig(gpio, pin_source, 0);
GPIO_Init(gpio, &init);
int16_t cycles = (len * 50) / tone;
for (int16_t i = 0; i < cycles; i++) {
GPIO_SetBits(gpio, pin_source);
PIOS_Thread_Sleep(tone);
GPIO_ResetBits(gpio, pin_source);
PIOS_Thread_Sleep(tone);
}
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
GPIO_PinAFConfig(gpio, pin_source, remap);
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_LED_LINK)
PIOS_LED_Off(PIOS_LED_LINK);
#endif /* PIOS_LED_LINK */
#if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B)
// Update the watchdog timer
PIOS_WDG_UpdateFlag(PIOS_WDG_RFM22B);
#endif /* PIOS_WDG_RFM22B */
}
#else /* OPENLRS_SIMPLE_BEACON_TONE */
static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_t len __attribute__((unused))) // duration is now in half seconds.
{
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_
rfm22_claimBus(openlrs_dev);
// This need fixed for F1
#ifdef GPIO_Mode_OUT
#ifdef GPIO_MODE_OUT
GPIO_TypeDef *gpio = openlrs_dev->cfg.spi_cfg->mosi.gpio;
uint16_t pin_source = openlrs_dev->cfg.spi_cfg->mosi.init.GPIO_Pin;
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_
GPIO_Init(gpio, (GPIO_InitTypeDef *)&openlrs_dev->cfg.spi_cfg->mosi.init);
GPIO_PinAFConfig(gpio, pin_source, remap);
#endif /* ifdef GPIO_Mode_OUT */
#endif /* ifdef GPIO_MODE_OUT */
rfm22_releaseBus(openlrs_dev);
#if defined(PIOS_LED_LINK)
@ -528,6 +579,7 @@ static void beacon_tone(struct pios_openlrs_dev *openlrs_dev, int16_t hz, int16_
#endif /* PIOS_WDG_RFM22B */
}
#endif /* OPENLRS_SIMPLE_BEACON_TONE */
static uint8_t beaconGetRSSI(struct pios_openlrs_dev *openlrs_dev)
{
@ -588,9 +640,40 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
if (static_tone) {
uint8_t i = 0;
while (i++ < 20) {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
beacon_simpletone(openlrs_dev, 2, 3);
#else
beacon_tone(openlrs_dev, 440, 1);
#endif
}
} else {
#ifdef OPENLRS_SIMPLE_BEACON_TONE
for (uint32_t tone = 1; tone < 4; tone++) {
if (tone == 1) {
rfm22_write(openlrs_dev, 0x6d, 0x05); // 5 set min power 25mW
} else {
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
}
// L - --- - -
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(600);
// P - --- --- -
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 3);
PIOS_Thread_Sleep(100);
beacon_simpletone(openlrs_dev, tone, 1);
PIOS_Thread_Sleep(2000);
}
#else /* ifdef OPENLRS_SIMPLE_BEACON_TONE */
// close encounters tune
// G, A, F, F(lower octave), C
// octave 3: 392 440 349 175 261
@ -612,6 +695,7 @@ static void beacon_send(struct pios_openlrs_dev *openlrs_dev, bool static_tone)
rfm22_write(openlrs_dev, 0x6d, 0x00); // 0 set min power 1.3mW
PIOS_Thread_Sleep(10);
beacon_tone(openlrs_dev, 261, 2);
#endif /* #ifdef OPENLRS_SIMPLE_BEACON_TONE */
}
rfm22_write_claim(openlrs_dev, 0x07, RF22B_PWRSTATE_READY);
}
@ -829,9 +913,6 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
OPLinkStatusData oplink_status;
OPLinkStatusGet(&oplink_status);
// Update the RSSI
oplink_status.RSSI = openlrs_dev->rssi;
timeUs = PIOS_DELAY_GetuS();
timeMs = PIOS_Thread_Systime();
@ -860,8 +941,8 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
openlrs_dev->lastPacketTimeUs = timeUs;
openlrs_dev->numberOfLostPackets = 0;
oplink_status.LinkQuality <<= 1;
oplink_status.LinkQuality |= 1;
openlrs_dev->link_quality <<= 1;
openlrs_dev->link_quality |= 1;
if ((openlrs_dev->rx_buf[0] & 0x3e) == 0x00) {
// This flag indicates receiving PPM data
@ -928,7 +1009,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
}
tx_buf[4] = (openlrs_dev->lastAFCCvalue >> 8);
tx_buf[5] = openlrs_dev->lastAFCCvalue & 0xff;
tx_buf[6] = countSetBits(oplink_status.LinkQuality & 0x7fff);
tx_buf[6] = countSetBits(openlrs_dev->link_quality & 0x7fff);
}
}
@ -949,7 +1030,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
if ((openlrs_dev->numberOfLostPackets < openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) + packet_timeout_us))) {
DEBUG_PRINTF(2, "OLRS WARN: Lost packet: %d\r\n", openlrs_dev->numberOfLostPackets);
// we lost packet, hop to next channel
oplink_status.LinkQuality <<= 1;
openlrs_dev->link_quality <<= 1;
openlrs_dev->willhop = 1;
if (openlrs_dev->numberOfLostPackets == 0) {
openlrs_dev->linkLossTimeMs = timeMs;
@ -961,7 +1042,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
} else if ((openlrs_dev->numberOfLostPackets >= openlrs_dev->hopcount) && (PIOS_DELAY_GetuSSince(openlrs_dev->lastPacketTimeUs) > (getInterval(&openlrs_dev->bind_data) * openlrs_dev->hopcount))) {
DEBUG_PRINTF(2, "ORLS WARN: Trying to resync\r\n");
// hop slowly to allow resync with TX
oplink_status.LinkQuality = 0;
openlrs_dev->link_quality = 0;
openlrs_dev->willhop = 1;
openlrs_dev->lastPacketTimeUs = timeUs;
}
@ -977,7 +1058,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
DEBUG_PRINTF(2, "Failsafe activated: %d %d\r\n", timeMs, openlrs_dev->linkLossTimeMs);
oplink_status.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
// failsafeApply();
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // beacon activating...
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_delay)) | 1; // beacon activating...
}
if ((openlrs_dev->beacon_frequency) && (openlrs_dev->nextBeaconTimeMs) &&
@ -993,7 +1074,7 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
beacon_send(openlrs_dev, false); // play cool tune
init_rfm(openlrs_dev, 0); // go back to normal RX
rx_reset(openlrs_dev);
openlrs_dev->nextBeaconTimeMs = (timeMs + 1000UL * openlrs_dev->beacon_period) | 1; // avoid 0 in time
openlrs_dev->nextBeaconTimeMs = (timeMs + (1000UL * openlrs_dev->beacon_period)) | 1; // avoid 0 in time
}
}
}
@ -1029,6 +1110,19 @@ static void pios_openlrs_rx_loop(struct pios_openlrs_dev *openlrs_dev)
openlrs_dev->willhop = 0;
}
if (oplink_status.LinkState > OPLINKSTATUS_LINKSTATE_DISCONNECTED) {
// Convert raw Rssi to dBm
oplink_status.RSSI = (int8_t)(openlrs_dev->rssi >> 1) - 122;
// Count number of bits set in link_quality
uint8_t linkquality_bits = countSetBits(openlrs_dev->link_quality & 0x7fff);
// Translate link quality to 0 - 128 range
oplink_status.LinkQuality = (linkquality_bits + 1) * 8;
} else {
oplink_status.LinkQuality = 0;
oplink_status.RSSI = -127;
}
// Update UAVO
OPLinkStatusSet(&oplink_status);
}

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

2
flight/pios/common/pios_oplinkrcvr.c
View File

@ -232,6 +232,8 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
for (int32_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) {
oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
}
oplinkrcvr_dev->oplinkreceiverdata.RSSI = -127;
oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = 0;
}
oplinkrcvr_dev->Fresh = false;

3
flight/pios/inc/pios_openlrs_priv.h
View File

@ -183,7 +183,8 @@ struct pios_openlrs_dev {
uint8_t rx_buf[64];
uint8_t tx_buf[9];
int8_t rssi;
uint8_t rssi;
uint16_t link_quality;
// Variables from OpenLRS for radio control
uint8_t hopcount;

1
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -138,6 +138,7 @@
#define PIOS_INCLUDE_RFM22B
#define PIOS_INCLUDE_RFM22B_COM
#define PIOS_INCLUDE_OPENLRS
/* #define OPENLRS_SIMPLE_BEACON_TONE */
/* #define PIOS_INCLUDE_PPM_OUT */
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */

1
flight/targets/boards/sparky2/firmware/inc/pios_config.h
View File

@ -141,6 +141,7 @@
#define PIOS_INCLUDE_RFM22B
#define PIOS_INCLUDE_RFM22B_COM
#define PIOS_INCLUDE_OPENLRS
/* #define OPENLRS_SIMPLE_BEACON_TONE */
/* #define PIOS_INCLUDE_PPM_OUT */
/* #define PIOS_RFM22B_DEBUG_ON_TELEM */

2
shared/uavobjectdefinition/oplinksettings.xml
View File

@ -39,7 +39,7 @@
<!-- beacon options -->
<field name="BeaconFrequency" units="Hz" type="uint32" elements="1" defaultvalue="462712500"/>
<field name="BeaconDelay" units="s" type="uint8" elements="1" defaultvalue="30"/>
<field name="BeaconPeriod" units="s" type="uint8" elements="1" defaultvalue="15"/>
<field name="BeaconPeriod" units="s" type="uint8" elements="1" defaultvalue="30"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>