Multiple telemtry providers can now be active at the same time on any
serial port. MSP telemetry can now be at any baud rate. A pattern is emerging in each telemetry provider, code is duplicated to get things working, refactoring can come later.
This commit is contained in:
parent
3e64ce883c
commit
7dcc7b2fb5
|
@ -24,6 +24,8 @@
|
||||||
|
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
|
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
|
@ -118,6 +120,19 @@ typedef struct findSharedSerialPortState_s {
|
||||||
uint8_t lastIndex;
|
uint8_t lastIndex;
|
||||||
} findSharedSerialPortState_t;
|
} findSharedSerialPortState_t;
|
||||||
|
|
||||||
|
portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function)
|
||||||
|
{
|
||||||
|
if (!portConfig || (portConfig->functionMask & function) == 0) {
|
||||||
|
return PORTSHARING_UNUSED;
|
||||||
|
}
|
||||||
|
return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction)
|
||||||
|
{
|
||||||
|
return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask);
|
||||||
|
}
|
||||||
|
|
||||||
static findSharedSerialPortState_t findSharedSerialPortState;
|
static findSharedSerialPortState_t findSharedSerialPortState;
|
||||||
|
|
||||||
serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
|
serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
|
||||||
|
@ -132,7 +147,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
|
||||||
while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) {
|
while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) {
|
||||||
serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++];
|
serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++];
|
||||||
|
|
||||||
if ((candidate->functionMask & sharedWithFunction) && (candidate->functionMask & functionMask)) {
|
if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) {
|
||||||
serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
|
serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
|
||||||
if (!serialPortUsage) {
|
if (!serialPortUsage) {
|
||||||
continue;
|
continue;
|
||||||
|
@ -143,10 +158,50 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_FRSKY_TELEMETRY | FUNCTION_HOTT_TELEMETRY | FUNCTION_MSP_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY)
|
||||||
|
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK)
|
||||||
|
|
||||||
bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
|
bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
|
||||||
{
|
{
|
||||||
UNUSED(serialConfigToCheck);
|
UNUSED(serialConfigToCheck);
|
||||||
return true; // FIXME implement rules - 1 MSP port minimum.
|
/*
|
||||||
|
* rules:
|
||||||
|
* - 1 MSP port minimum, max MSP ports is defined and must be adhered to.
|
||||||
|
* - Only MSP is allowed to be shared with EITHER any telemetry OR blackbox.
|
||||||
|
* - No other sharing combinations are valid.
|
||||||
|
*/
|
||||||
|
uint8_t mspPortCount = 0;
|
||||||
|
|
||||||
|
uint8_t index;
|
||||||
|
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||||
|
serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index];
|
||||||
|
|
||||||
|
if (portConfig->functionMask & FUNCTION_MSP) {
|
||||||
|
mspPortCount++;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t bitCount = BITCOUNT(portConfig->functionMask);
|
||||||
|
if (bitCount > 1) {
|
||||||
|
// shared
|
||||||
|
if (bitCount > 2) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!(portConfig->functionMask & FUNCTION_MSP)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!(portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) {
|
||||||
|
// some other bit must have been set.
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mspPortCount == 0 || mspPortCount > MAX_MSP_PORT_COUNT) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool doesConfigurationUsePort(serialPortIdentifier_e identifier)
|
bool doesConfigurationUsePort(serialPortIdentifier_e identifier)
|
||||||
|
|
|
@ -17,17 +17,22 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
PORTSHARING_UNUSED = 0,
|
||||||
|
PORTSHARING_NOT_SHARED,
|
||||||
|
PORTSHARING_SHARED
|
||||||
|
} portSharing_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
FUNCTION_NONE = 0,
|
FUNCTION_NONE = 0,
|
||||||
FUNCTION_MSP = (1 << 0),
|
FUNCTION_MSP = (1 << 0),
|
||||||
FUNCTION_CLI = (1 << 1),
|
FUNCTION_GPS = (1 << 1),
|
||||||
FUNCTION_FRSKY_TELEMETRY = (1 << 2),
|
FUNCTION_FRSKY_TELEMETRY = (1 << 2),
|
||||||
FUNCTION_HOTT_TELEMETRY = (1 << 3),
|
FUNCTION_HOTT_TELEMETRY = (1 << 3),
|
||||||
FUNCTION_MSP_TELEMETRY = (1 << 4),
|
FUNCTION_MSP_TELEMETRY = (1 << 4),
|
||||||
FUNCTION_SMARTPORT_TELEMETRY = (1 << 5),
|
FUNCTION_SMARTPORT_TELEMETRY = (1 << 5),
|
||||||
FUNCTION_SERIAL_RX = (1 << 6),
|
FUNCTION_SERIAL_RX = (1 << 6),
|
||||||
FUNCTION_GPS = (1 << 7),
|
FUNCTION_BLACKBOX = (1 << 7)
|
||||||
FUNCTION_BLACKBOX = (1 << 8)
|
|
||||||
} serialPortFunction_e;
|
} serialPortFunction_e;
|
||||||
|
|
||||||
// serial port identifiers are now fixed, these values are used by MSP commands.
|
// serial port identifiers are now fixed, these values are used by MSP commands.
|
||||||
|
@ -62,7 +67,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
|
||||||
|
|
||||||
typedef struct serialPortConfig_s {
|
typedef struct serialPortConfig_s {
|
||||||
serialPortIdentifier_e identifier;
|
serialPortIdentifier_e identifier;
|
||||||
serialPortFunction_e functionMask;
|
uint16_t functionMask;
|
||||||
uint32_t baudrate; // not used for all functions, e.g. HoTT only works at 19200.
|
uint32_t baudrate; // not used for all functions, e.g. HoTT only works at 19200.
|
||||||
} serialPortConfig_t;
|
} serialPortConfig_t;
|
||||||
|
|
||||||
|
@ -80,6 +85,10 @@ bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier);
|
||||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function);
|
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function);
|
||||||
serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function);
|
serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function);
|
||||||
|
|
||||||
|
portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function);
|
||||||
|
bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// runtime
|
// runtime
|
||||||
|
|
|
@ -362,8 +362,9 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
if (feature(FEATURE_TELEMETRY))
|
if (feature(FEATURE_TELEMETRY)) {
|
||||||
telemetryInit();
|
telemetryInit();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
#ifdef BLACKBOX
|
||||||
|
|
|
@ -56,7 +56,7 @@ bool rxMspFrameComplete(void)
|
||||||
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||||
{
|
{
|
||||||
UNUSED(rxConfig);
|
UNUSED(rxConfig);
|
||||||
rxRuntimeConfig->channelCount = 8; // See MSP_SET_RAW_RC
|
rxRuntimeConfig->channelCount = 8; // Limited to 8 channels due to MSP_SET_RAW_RC command.
|
||||||
if (callback)
|
if (callback)
|
||||||
*callback = rxMspReadRawRC;
|
*callback = rxMspReadRawRC;
|
||||||
|
|
||||||
|
|
|
@ -80,8 +80,6 @@ static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
|
||||||
|
|
||||||
static serialPort_t *sBusPort = NULL;
|
static serialPort_t *sBusPort = NULL;
|
||||||
|
|
||||||
static uint32_t sbusSignalLostEventCount = 0;
|
|
||||||
|
|
||||||
bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
|
||||||
{
|
{
|
||||||
int b;
|
int b;
|
||||||
|
|
|
@ -62,10 +62,15 @@
|
||||||
#include "telemetry/frsky.h"
|
#include "telemetry/frsky.h"
|
||||||
|
|
||||||
static serialPort_t *frskyPort = NULL;
|
static serialPort_t *frskyPort = NULL;
|
||||||
|
static serialPortConfig_t *portConfig;
|
||||||
|
|
||||||
#define FRSKY_BAUDRATE 9600
|
#define FRSKY_BAUDRATE 9600
|
||||||
#define FRSKY_INITIAL_PORT_MODE MODE_TX
|
#define FRSKY_INITIAL_PORT_MODE MODE_TX
|
||||||
|
|
||||||
static telemetryConfig_t *telemetryConfig;
|
static telemetryConfig_t *telemetryConfig;
|
||||||
|
static bool frskyTelemetryEnabled = false;
|
||||||
|
static portSharing_e frskyPortSharing;
|
||||||
|
|
||||||
|
|
||||||
extern batteryConfig_t *batteryConfig;
|
extern batteryConfig_t *batteryConfig;
|
||||||
|
|
||||||
|
@ -402,22 +407,29 @@ static void sendHeading(void)
|
||||||
void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
||||||
{
|
{
|
||||||
telemetryConfig = initialTelemetryConfig;
|
telemetryConfig = initialTelemetryConfig;
|
||||||
|
portConfig = findSerialPortConfig(FUNCTION_FRSKY_TELEMETRY);
|
||||||
|
frskyPortSharing = determinePortSharing(portConfig, FUNCTION_FRSKY_TELEMETRY);
|
||||||
}
|
}
|
||||||
|
|
||||||
void freeFrSkyTelemetryPort(void)
|
void freeFrSkyTelemetryPort(void)
|
||||||
{
|
{
|
||||||
closeSerialPort(frskyPort);
|
closeSerialPort(frskyPort);
|
||||||
frskyPort = NULL;
|
frskyPort = NULL;
|
||||||
|
frskyTelemetryEnabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void configureFrSkyTelemetryPort(void)
|
void configureFrSkyTelemetryPort(void)
|
||||||
{
|
{
|
||||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_TELEMETRY);
|
|
||||||
if (!portConfig) {
|
if (!portConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_FRSKY_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion);
|
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_FRSKY_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion);
|
||||||
|
if (!frskyPort) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
frskyTelemetryEnabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -431,8 +443,26 @@ bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
|
||||||
return currentMillis - lastCycleTime >= CYCLETIME;
|
return currentMillis - lastCycleTime >= CYCLETIME;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void checkFrSkyTelemetryState(void)
|
||||||
|
{
|
||||||
|
bool newTelemetryEnabledValue = determineNewTelemetryEnabledState(frskyPortSharing);
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue == frskyTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue)
|
||||||
|
configureFrSkyTelemetryPort();
|
||||||
|
else
|
||||||
|
freeFrSkyTelemetryPort();
|
||||||
|
}
|
||||||
|
|
||||||
void handleFrSkyTelemetry(void)
|
void handleFrSkyTelemetry(void)
|
||||||
{
|
{
|
||||||
|
if (!frskyTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (!canSendFrSkyTelemetry()) {
|
if (!canSendFrSkyTelemetry()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,6 +25,4 @@ void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig);
|
||||||
void configureFrSkyTelemetryPort(void);
|
void configureFrSkyTelemetryPort(void);
|
||||||
void freeFrSkyTelemetryPort(void);
|
void freeFrSkyTelemetryPort(void);
|
||||||
|
|
||||||
uint32_t getFrSkyTelemetryProviderBaudRate(void);
|
|
||||||
|
|
||||||
#endif /* TELEMETRY_FRSKY_H_ */
|
#endif /* TELEMETRY_FRSKY_H_ */
|
||||||
|
|
|
@ -104,8 +104,11 @@ static uint8_t hottMsgCrc;
|
||||||
#define HOTT_INITIAL_PORT_MODE MODE_RX
|
#define HOTT_INITIAL_PORT_MODE MODE_RX
|
||||||
|
|
||||||
static serialPort_t *hottPort = NULL;
|
static serialPort_t *hottPort = NULL;
|
||||||
|
static serialPortConfig_t *portConfig;
|
||||||
|
|
||||||
static telemetryConfig_t *telemetryConfig;
|
static telemetryConfig_t *telemetryConfig;
|
||||||
|
static bool hottTelemetryEnabled = false;
|
||||||
|
static portSharing_e hottPortSharing;
|
||||||
|
|
||||||
static HOTT_GPS_MSG_t hottGPSMessage;
|
static HOTT_GPS_MSG_t hottGPSMessage;
|
||||||
static HOTT_EAM_MSG_t hottEAMMessage;
|
static HOTT_EAM_MSG_t hottEAMMessage;
|
||||||
|
@ -253,18 +256,20 @@ void freeHoTTTelemetryPort(void)
|
||||||
{
|
{
|
||||||
closeSerialPort(hottPort);
|
closeSerialPort(hottPort);
|
||||||
hottPort = NULL;
|
hottPort = NULL;
|
||||||
|
hottTelemetryEnabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
||||||
{
|
{
|
||||||
telemetryConfig = initialTelemetryConfig;
|
telemetryConfig = initialTelemetryConfig;
|
||||||
|
portConfig = findSerialPortConfig(FUNCTION_HOTT_TELEMETRY);
|
||||||
|
hottPortSharing = determinePortSharing(portConfig, FUNCTION_HOTT_TELEMETRY);
|
||||||
|
|
||||||
initialiseMessages();
|
initialiseMessages();
|
||||||
}
|
}
|
||||||
|
|
||||||
void configureHoTTTelemetryPort(void)
|
void configureHoTTTelemetryPort(void)
|
||||||
{
|
{
|
||||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HOTT_TELEMETRY);
|
|
||||||
if (!portConfig) {
|
if (!portConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -285,6 +290,7 @@ void configureHoTTTelemetryPort(void)
|
||||||
useSoftserialRxFailureWorkaround = true;
|
useSoftserialRxFailureWorkaround = true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
hottTelemetryEnabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void hottSendResponse(uint8_t *buffer, int length)
|
static void hottSendResponse(uint8_t *buffer, int length)
|
||||||
|
@ -449,11 +455,29 @@ static inline bool shouldCheckForHoTTRequest()
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void checkHoTTTelemetryState(void)
|
||||||
|
{
|
||||||
|
bool newTelemetryEnabledValue = determineNewTelemetryEnabledState(hottPortSharing);
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue == hottTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue)
|
||||||
|
configureHoTTTelemetryPort();
|
||||||
|
else
|
||||||
|
freeHoTTTelemetryPort();
|
||||||
|
}
|
||||||
|
|
||||||
void handleHoTTTelemetry(void)
|
void handleHoTTTelemetry(void)
|
||||||
{
|
{
|
||||||
static uint32_t serialTimer;
|
static uint32_t serialTimer;
|
||||||
uint32_t now = micros();
|
|
||||||
|
|
||||||
|
if (!hottTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t now = micros();
|
||||||
|
|
||||||
if (shouldPrepareHoTTMessages(now)) {
|
if (shouldPrepareHoTTMessages(now)) {
|
||||||
hottPrepareMessages();
|
hottPrepareMessages();
|
||||||
|
|
|
@ -173,8 +173,8 @@ typedef struct HOTT_GAM_MSG_s {
|
||||||
uint8_t batt1_H; //#14
|
uint8_t batt1_H; //#14
|
||||||
uint8_t batt2_L; //#15 battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
|
uint8_t batt2_L; //#15 battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
|
||||||
uint8_t batt2_H; //#16
|
uint8_t batt2_H; //#16
|
||||||
uint8_t temperature1; //#17 temperature 1. offset of 20. a value of 20 = 0°C
|
uint8_t temperature1; //#17 temperature 1. offset of 20. a value of 20 = 0<EFBFBD>C
|
||||||
uint8_t temperature2; //#18 temperature 2. offset of 20. a value of 20 = 0°C
|
uint8_t temperature2; //#18 temperature 2. offset of 20. a value of 20 = 0<EFBFBD>C
|
||||||
uint8_t fuel_procent; //#19 Fuel capacity in %. Values 0--100
|
uint8_t fuel_procent; //#19 Fuel capacity in %. Values 0--100
|
||||||
// graphical display ranges: 0-25% 50% 75% 100%
|
// graphical display ranges: 0-25% 50% 75% 100%
|
||||||
uint8_t fuel_ml_L; //#20 Fuel in ml scale. Full = 65535!
|
uint8_t fuel_ml_L; //#20 Fuel in ml scale. Full = 65535!
|
||||||
|
@ -249,7 +249,7 @@ typedef struct HOTT_VARIO_MSG_s {
|
||||||
uint8_t free_char1; //#39 Free ASCII character. appears right to home distance
|
uint8_t free_char1; //#39 Free ASCII character. appears right to home distance
|
||||||
uint8_t free_char2; //#40 Free ASCII character. appears right to home direction
|
uint8_t free_char2; //#40 Free ASCII character. appears right to home direction
|
||||||
uint8_t free_char3; //#41 Free ASCII character. appears? TODO: Check where this char appears
|
uint8_t free_char3; //#41 Free ASCII character. appears? TODO: Check where this char appears
|
||||||
uint8_t compass_direction; //#42 Compass heading in 2° steps. 1 = 2°
|
uint8_t compass_direction; //#42 Compass heading in 2<EFBFBD> steps. 1 = 2<>
|
||||||
uint8_t version; //#43 version number TODO: more info?
|
uint8_t version; //#43 version number TODO: more info?
|
||||||
uint8_t stop_byte; //#44 stop uint8_t, constant value 0x7d
|
uint8_t stop_byte; //#44 stop uint8_t, constant value 0x7d
|
||||||
} HOTT_VARIO_MSG_t;
|
} HOTT_VARIO_MSG_t;
|
||||||
|
@ -321,7 +321,7 @@ typedef struct HOTT_EAM_MSG_s {
|
||||||
uint8_t batt2_voltage_L; //#23 battery 2 voltage lower value in 100mv steps, 50=5V. optionally cell8_H value. 0.02V steps
|
uint8_t batt2_voltage_L; //#23 battery 2 voltage lower value in 100mv steps, 50=5V. optionally cell8_H value. 0.02V steps
|
||||||
uint8_t batt2_voltage_H; //#24
|
uint8_t batt2_voltage_H; //#24
|
||||||
|
|
||||||
uint8_t temp1; //#25 Temperature sensor 1. 20=0°, 46=26° - offset of 20.
|
uint8_t temp1; //#25 Temperature sensor 1. 20=0<EFBFBD>, 46=26<32> - offset of 20.
|
||||||
uint8_t temp2; //#26 temperature sensor 2
|
uint8_t temp2; //#26 temperature sensor 2
|
||||||
|
|
||||||
uint8_t altitude_L; //#27 Attitude lower value. unit: meters. Value of 500 = 0m
|
uint8_t altitude_L; //#27 Attitude lower value. unit: meters. Value of 500 = 0m
|
||||||
|
@ -379,13 +379,13 @@ typedef struct HOTT_GPS_MSG_s {
|
||||||
uint8_t gps_speed_H; //#09
|
uint8_t gps_speed_H; //#09
|
||||||
|
|
||||||
uint8_t pos_NS; //#10 north = 0, south = 1
|
uint8_t pos_NS; //#10 north = 0, south = 1
|
||||||
uint8_t pos_NS_dm_L; //#11 degree minutes ie N48°39’988
|
uint8_t pos_NS_dm_L; //#11 degree minutes ie N48<EFBFBD>39<EFBFBD>988
|
||||||
uint8_t pos_NS_dm_H; //#12
|
uint8_t pos_NS_dm_H; //#12
|
||||||
uint8_t pos_NS_sec_L; //#13 position seconds
|
uint8_t pos_NS_sec_L; //#13 position seconds
|
||||||
uint8_t pos_NS_sec_H; //#14
|
uint8_t pos_NS_sec_H; //#14
|
||||||
|
|
||||||
uint8_t pos_EW; //#15 east = 0, west = 1
|
uint8_t pos_EW; //#15 east = 0, west = 1
|
||||||
uint8_t pos_EW_dm_L; //#16 degree minutes ie. E9°25’9360
|
uint8_t pos_EW_dm_L; //#16 degree minutes ie. E9<EFBFBD>25<EFBFBD>9360
|
||||||
uint8_t pos_EW_dm_H; //#17
|
uint8_t pos_EW_dm_H; //#17
|
||||||
uint8_t pos_EW_sec_L; //#18 position seconds
|
uint8_t pos_EW_sec_L; //#18 position seconds
|
||||||
uint8_t pos_EW_sec_H; //#19
|
uint8_t pos_EW_sec_H; //#19
|
||||||
|
@ -407,7 +407,7 @@ typedef struct HOTT_GPS_MSG_s {
|
||||||
uint8_t home_direction;//#29 direction from starting point to Model position (2 degree steps)
|
uint8_t home_direction;//#29 direction from starting point to Model position (2 degree steps)
|
||||||
uint8_t angle_roll; //#30 angle roll in 2 degree steps
|
uint8_t angle_roll; //#30 angle roll in 2 degree steps
|
||||||
uint8_t angle_nick; //#31 angle in 2degree steps
|
uint8_t angle_nick; //#31 angle in 2degree steps
|
||||||
uint8_t angle_compass; //#32 angle in 2degree steps. 1 = 2°, 255 = - 2° (1 uint8_t) North = 0°
|
uint8_t angle_compass; //#32 angle in 2degree steps. 1 = 2<EFBFBD>, 255 = - 2<> (1 uint8_t) North = 0<>
|
||||||
|
|
||||||
uint8_t gps_time_h; //#33 UTC time hours
|
uint8_t gps_time_h; //#33 UTC time hours
|
||||||
uint8_t gps_time_m; //#34 UTC time minutes
|
uint8_t gps_time_m; //#34 UTC time minutes
|
||||||
|
@ -488,7 +488,7 @@ typedef struct HOTT_AIRESC_MSG_s {
|
||||||
} HOTT_AIRESC_MSG_t;
|
} HOTT_AIRESC_MSG_t;
|
||||||
|
|
||||||
void handleHoTTTelemetry(void);
|
void handleHoTTTelemetry(void);
|
||||||
void checkTelemetryState(void);
|
void checkHoTTTelemetryState(void);
|
||||||
|
|
||||||
void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);
|
void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);
|
||||||
void configureHoTTTelemetryPort(void);
|
void configureHoTTTelemetryPort(void);
|
||||||
|
|
|
@ -32,13 +32,18 @@
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
|
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "telemetry/telemetry.h"
|
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
#include "io/serial_msp.h"
|
#include "io/serial_msp.h"
|
||||||
|
|
||||||
static telemetryConfig_t *telemetryConfig;
|
#include "telemetry/telemetry.h"
|
||||||
|
#include "telemetry/msp.h"
|
||||||
|
|
||||||
|
static telemetryConfig_t *telemetryConfig;
|
||||||
|
static serialPortConfig_t *portConfig;
|
||||||
|
|
||||||
|
static bool mspTelemetryEnabled = false;
|
||||||
|
static portSharing_e mspPortSharing;
|
||||||
|
|
||||||
#define MSP_TELEMETRY_BAUDRATE 19200 // TODO make this configurable
|
|
||||||
#define MSP_TELEMETRY_INITIAL_PORT_MODE MODE_TX
|
#define MSP_TELEMETRY_INITIAL_PORT_MODE MODE_TX
|
||||||
|
|
||||||
static serialPort_t *mspTelemetryPort = NULL;
|
static serialPort_t *mspTelemetryPort = NULL;
|
||||||
|
@ -46,10 +51,30 @@ static serialPort_t *mspTelemetryPort = NULL;
|
||||||
void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
||||||
{
|
{
|
||||||
telemetryConfig = initialTelemetryConfig;
|
telemetryConfig = initialTelemetryConfig;
|
||||||
|
portConfig = findSerialPortConfig(FUNCTION_MSP_TELEMETRY);
|
||||||
|
mspPortSharing = determinePortSharing(portConfig, FUNCTION_MSP_TELEMETRY);
|
||||||
|
}
|
||||||
|
|
||||||
|
void checkMSPTelemetryState(void)
|
||||||
|
{
|
||||||
|
bool newTelemetryEnabledValue = determineNewTelemetryEnabledState(mspPortSharing);
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue == mspTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue)
|
||||||
|
configureMSPTelemetryPort();
|
||||||
|
else
|
||||||
|
freeMSPTelemetryPort();
|
||||||
}
|
}
|
||||||
|
|
||||||
void handleMSPTelemetry(void)
|
void handleMSPTelemetry(void)
|
||||||
{
|
{
|
||||||
|
if (!mspTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (!mspTelemetryPort) {
|
if (!mspTelemetryPort) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -61,21 +86,23 @@ void freeMSPTelemetryPort(void)
|
||||||
{
|
{
|
||||||
closeSerialPort(mspTelemetryPort);
|
closeSerialPort(mspTelemetryPort);
|
||||||
mspTelemetryPort = NULL;
|
mspTelemetryPort = NULL;
|
||||||
|
mspTelemetryEnabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void configureMSPTelemetryPort(void)
|
void configureMSPTelemetryPort(void)
|
||||||
{
|
{
|
||||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_TELEMETRY);
|
|
||||||
if (!portConfig) {
|
if (!portConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
mspTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_MSP_TELEMETRY, NULL, MSP_TELEMETRY_BAUDRATE, MSP_TELEMETRY_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
|
mspTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_MSP_TELEMETRY, NULL, portConfig->baudrate, MSP_TELEMETRY_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
|
||||||
|
|
||||||
if (!mspTelemetryPort) {
|
if (!mspTelemetryPort) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
mspSetTelemetryPort(mspTelemetryPort);
|
mspSetTelemetryPort(mspTelemetryPort);
|
||||||
|
|
||||||
|
mspTelemetryEnabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -27,11 +27,9 @@
|
||||||
|
|
||||||
void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig);
|
void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig);
|
||||||
void handleMSPTelemetry(void);
|
void handleMSPTelemetry(void);
|
||||||
|
void checkMSPTelemetryState(void);
|
||||||
|
|
||||||
void freeMSPTelemetryPort(void);
|
void freeMSPTelemetryPort(void);
|
||||||
void configureMSPTelemetryPort(void);
|
void configureMSPTelemetryPort(void);
|
||||||
|
|
||||||
uint32_t getMSPTelemetryProviderBaudRate(void);
|
|
||||||
|
|
||||||
|
|
||||||
#endif /* TELEMETRY_MSP_H_ */
|
#endif /* TELEMETRY_MSP_H_ */
|
||||||
|
|
|
@ -139,7 +139,11 @@ const uint16_t frSkyDataIdTable[] = {
|
||||||
#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
|
#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
|
||||||
|
|
||||||
static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
|
static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
|
||||||
|
static serialPortConfig_t *portConfig;
|
||||||
|
|
||||||
static telemetryConfig_t *telemetryConfig;
|
static telemetryConfig_t *telemetryConfig;
|
||||||
|
static bool smartPortTelemetryEnabled = false;
|
||||||
|
static portSharing_e smartPortPortSharing;
|
||||||
|
|
||||||
extern void serialInit(serialConfig_t *); // from main.c // FIXME remove this dependency
|
extern void serialInit(serialConfig_t *); // from main.c // FIXME remove this dependency
|
||||||
|
|
||||||
|
@ -210,6 +214,8 @@ static void smartPortSendPackage(uint16_t id, uint32_t val)
|
||||||
void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
|
||||||
{
|
{
|
||||||
telemetryConfig = initialTelemetryConfig;
|
telemetryConfig = initialTelemetryConfig;
|
||||||
|
portConfig = findSerialPortConfig(FUNCTION_SMARTPORT_TELEMETRY);
|
||||||
|
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_SMARTPORT_TELEMETRY);
|
||||||
}
|
}
|
||||||
|
|
||||||
void freeSmartPortTelemetryPort(void)
|
void freeSmartPortTelemetryPort(void)
|
||||||
|
@ -218,11 +224,11 @@ void freeSmartPortTelemetryPort(void)
|
||||||
smartPortSerialPort = NULL;
|
smartPortSerialPort = NULL;
|
||||||
|
|
||||||
smartPortState = SPSTATE_UNINITIALIZED;
|
smartPortState = SPSTATE_UNINITIALIZED;
|
||||||
|
smartPortTelemetryEnabled = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void configureSmartPortTelemetryPort(void)
|
void configureSmartPortTelemetryPort(void)
|
||||||
{
|
{
|
||||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_SMARTPORT_TELEMETRY);
|
|
||||||
if (!portConfig) {
|
if (!portConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -234,6 +240,7 @@ void configureSmartPortTelemetryPort(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
smartPortState = SPSTATE_INITIALIZED;
|
smartPortState = SPSTATE_INITIALIZED;
|
||||||
|
smartPortTelemetryEnabled = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool canSendSmartPortTelemetry(void)
|
bool canSendSmartPortTelemetry(void)
|
||||||
|
@ -246,8 +253,26 @@ bool isSmartPortTimedOut(void)
|
||||||
return smartPortState >= SPSTATE_TIMEDOUT;
|
return smartPortState >= SPSTATE_TIMEDOUT;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void checkSmartPortTelemetryState(void)
|
||||||
|
{
|
||||||
|
bool newTelemetryEnabledValue = determineNewTelemetryEnabledState(smartPortPortSharing);
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue == smartPortTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (newTelemetryEnabledValue)
|
||||||
|
configureSmartPortTelemetryPort();
|
||||||
|
else
|
||||||
|
freeSmartPortTelemetryPort();
|
||||||
|
}
|
||||||
|
|
||||||
void handleSmartPortTelemetry(void)
|
void handleSmartPortTelemetry(void)
|
||||||
{
|
{
|
||||||
|
if (!smartPortTelemetryEnabled) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (!canSendSmartPortTelemetry()) {
|
if (!canSendSmartPortTelemetry()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -16,9 +16,6 @@ void checkSmartPortTelemetryState(void);
|
||||||
void configureSmartPortTelemetryPort(void);
|
void configureSmartPortTelemetryPort(void);
|
||||||
void freeSmartPortTelemetryPort(void);
|
void freeSmartPortTelemetryPort(void);
|
||||||
|
|
||||||
uint32_t getSmartPortTelemetryProviderBaudRate(void);
|
|
||||||
|
|
||||||
bool canSmartPortAllowOtherSerial(void);
|
|
||||||
bool isSmartPortTimedOut(void);
|
bool isSmartPortTimedOut(void);
|
||||||
|
|
||||||
#endif /* TELEMETRY_SMARTPORT_H_ */
|
#endif /* TELEMETRY_SMARTPORT_H_ */
|
||||||
|
|
|
@ -41,9 +41,6 @@
|
||||||
#include "telemetry/msp.h"
|
#include "telemetry/msp.h"
|
||||||
#include "telemetry/smartport.h"
|
#include "telemetry/smartport.h"
|
||||||
|
|
||||||
|
|
||||||
static bool telemetryEnabled = false;
|
|
||||||
|
|
||||||
static telemetryConfig_t *telemetryConfig;
|
static telemetryConfig_t *telemetryConfig;
|
||||||
|
|
||||||
void useTelemetryConfig(telemetryConfig_t *telemetryConfigToUse)
|
void useTelemetryConfig(telemetryConfig_t *telemetryConfigToUse)
|
||||||
|
@ -51,77 +48,40 @@ void useTelemetryConfig(telemetryConfig_t *telemetryConfigToUse)
|
||||||
telemetryConfig = telemetryConfigToUse;
|
telemetryConfig = telemetryConfigToUse;
|
||||||
}
|
}
|
||||||
|
|
||||||
void telemetryInit()
|
void telemetryInit(void)
|
||||||
{
|
{
|
||||||
initFrSkyTelemetry(telemetryConfig);
|
initFrSkyTelemetry(telemetryConfig);
|
||||||
|
|
||||||
initHoTTTelemetry(telemetryConfig);
|
initHoTTTelemetry(telemetryConfig);
|
||||||
|
|
||||||
initMSPTelemetry(telemetryConfig);
|
initMSPTelemetry(telemetryConfig);
|
||||||
|
|
||||||
initSmartPortTelemetry(telemetryConfig);
|
initSmartPortTelemetry(telemetryConfig);
|
||||||
|
|
||||||
checkTelemetryState();
|
checkTelemetryState();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool determineNewTelemetryEnabledState(void)
|
bool determineNewTelemetryEnabledState(portSharing_e portSharing)
|
||||||
{
|
{
|
||||||
bool enabled = true;
|
bool enabled = portSharing == PORTSHARING_NOT_SHARED;
|
||||||
|
|
||||||
|
if (portSharing == PORTSHARING_SHARED) {
|
||||||
if (telemetryConfig->telemetry_switch)
|
if (telemetryConfig->telemetry_switch)
|
||||||
enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);
|
enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);
|
||||||
else
|
else
|
||||||
enabled = ARMING_FLAG(ARMED);
|
enabled = ARMING_FLAG(ARMED);
|
||||||
|
}
|
||||||
|
|
||||||
return enabled;
|
return enabled;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool shouldChangeTelemetryStateNow(bool newState)
|
|
||||||
{
|
|
||||||
return newState != telemetryEnabled;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void configureTelemetryPort(void)
|
|
||||||
{
|
|
||||||
configureFrSkyTelemetryPort();
|
|
||||||
configureHoTTTelemetryPort();
|
|
||||||
configureMSPTelemetryPort();
|
|
||||||
configureSmartPortTelemetryPort();
|
|
||||||
}
|
|
||||||
|
|
||||||
void freeTelemetryPort(void)
|
|
||||||
{
|
|
||||||
freeFrSkyTelemetryPort();
|
|
||||||
freeHoTTTelemetryPort();
|
|
||||||
freeMSPTelemetryPort();
|
|
||||||
freeSmartPortTelemetryPort();
|
|
||||||
}
|
|
||||||
|
|
||||||
void checkTelemetryState(void)
|
void checkTelemetryState(void)
|
||||||
{
|
{
|
||||||
bool newEnabledState = determineNewTelemetryEnabledState();
|
checkFrSkyTelemetryState();
|
||||||
|
checkHoTTTelemetryState();
|
||||||
if (!shouldChangeTelemetryStateNow(newEnabledState)) {
|
checkMSPTelemetryState();
|
||||||
return;
|
checkSmartPortTelemetryState();
|
||||||
}
|
|
||||||
|
|
||||||
if (newEnabledState)
|
|
||||||
configureTelemetryPort();
|
|
||||||
else
|
|
||||||
freeTelemetryPort();
|
|
||||||
|
|
||||||
telemetryEnabled = newEnabledState;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void handleTelemetry(void)
|
void handleTelemetry(void)
|
||||||
{
|
{
|
||||||
if (!determineNewTelemetryEnabledState())
|
|
||||||
return;
|
|
||||||
|
|
||||||
if (!telemetryEnabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
handleFrSkyTelemetry();
|
handleFrSkyTelemetry();
|
||||||
handleHoTTTelemetry();
|
handleHoTTTelemetry();
|
||||||
handleMSPTelemetry();
|
handleMSPTelemetry();
|
||||||
|
|
|
@ -47,8 +47,8 @@ typedef struct telemetryConfig_s {
|
||||||
void checkTelemetryState(void);
|
void checkTelemetryState(void);
|
||||||
void handleTelemetry(void);
|
void handleTelemetry(void);
|
||||||
|
|
||||||
|
bool determineNewTelemetryEnabledState(portSharing_e portSharing);
|
||||||
|
|
||||||
void useTelemetryConfig(telemetryConfig_t *telemetryConfig);
|
void useTelemetryConfig(telemetryConfig_t *telemetryConfig);
|
||||||
bool telemetryAllowsOtherSerial(int serialPortFunction);
|
|
||||||
bool isTelemetryPortShared(void);
|
|
||||||
|
|
||||||
#endif /* TELEMETRY_COMMON_H_ */
|
#endif /* TELEMETRY_COMMON_H_ */
|
||||||
|
|
Loading…
Reference in New Issue