rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add support for IRC Tramp race lock flag, and extended VTX stat… (#7128) 

Add support for IRC Tramp race lock flag, and extended VTX status.
This commit is contained in:
Michael Keller 2019-07-29 02:13:26 +12:00 committed by GitHub
parent 3857f6cb75 aeca5665e6
commit 5123e8bb38
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 154 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/drivers/vtx_common.c
View File

@ -136,16 +136,16 @@ bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
}
bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{
return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff);
}
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
{
return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
}
bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
{
return vtxDevice->vTable->getStatus(vtxDevice, status);
}
const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band)
{
if (vtxDevice) {

10
src/main/drivers/vtx_common.h
View File

@ -82,6 +82,12 @@ typedef enum {
#define VTX_TRAMP_POWER_400 4
#define VTX_TRAMP_POWER_600 5
// VTX status flags
enum {
VTX_STATUS_PIT_MODE = 1 << 0,
VTX_STATUS_LOCKED = 1 << 1,
};
struct vtxVTable_s;
typedef struct vtxDevice_s {
const struct vtxVTable_s *const vTable;
@ -104,8 +110,8 @@ typedef struct vtxVTable_s {
bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool (*getStatus)(const vtxDevice_t *vtxDevice, unsigned *status);
} vtxVTable_t;
// 3.1.0
@ -127,8 +133,8 @@ void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t freq);
bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status);
const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band);
char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band);
char vtxCommonGetBandLetter(const vtxDevice_t *vtxDevice, int band);

10
src/main/io/ledstrip.c
View File

@ -617,7 +617,7 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
{
static uint16_t frequency = 0;
static uint8_t power = 255;
static uint8_t pit = 255;
static unsigned vtxStatus = UINT32_MAX;
static uint8_t showSettings = false;
static uint16_t lastCheck = 0;
static bool blink = false;
@ -634,12 +634,12 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
// keep counter running, so it stays in sync with vtx
vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
vtxCommonGetPowerIndex(vtxDevice, &power);
vtxCommonGetPitMode(vtxDevice, &pit);
vtxCommonGetStatus(vtxDevice, &vtxStatus);
frequency = vtxCommonLookupFrequency(vtxDevice, band, channel);
// check if last vtx values have changed.
check = pit + (power << 1) + (band << 4) + (channel << 8);
check = ((vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0) + (power << 1) + (band << 4) + (channel << 8);
if (!showSettings && check != lastCheck) {
// display settings for 3 seconds.
showSettings = 15;
@ -664,7 +664,7 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
color.s = HSV(GREEN).s;
color.v = blink ? 15 : 0; // blink received settings
}
else if (vtxLedCount > 0 && power >= vtxLedCount && !pit) { // show power
else if (vtxLedCount > 0 && power >= vtxLedCount && !(vtxStatus & VTX_STATUS_PIT_MODE)) { // show power
color.h = HSV(ORANGE).h;
color.s = HSV(ORANGE).s;
color.v = blink ? 15 : 0; // blink received settings
@ -700,7 +700,7 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
colorIndex = COLOR_DEEP_PINK;
}
hsvColor_t color = ledStripStatusModeConfig()->colors[colorIndex];
color.v = pit ? (blink ? 15 : 0) : 255; // blink when in pit mode
color.v = (vtxStatus & VTX_STATUS_PIT_MODE) ? (blink ? 15 : 0) : 255; // blink when in pit mode
applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_VTX)), &color);
}
}

6
src/main/io/spektrum_vtx_control.c
View File

@ -208,9 +208,9 @@ void spektrumVtxControl(void)
newSettings.power = power;
}
// Everyone seems to agree on what PIT ON/OFF means
uint8_t currentPitMode = 0;
if (vtxCommonGetPitMode(vtxDevice, &currentPitMode)) {
if (currentPitMode != vtx.pitMode) {
unsigned vtxCurrentStatus;
if (vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus)) {
if ((vtxCurrentStatus & VTX_STATUS_PIT_MODE) != vtx.pitMode) {
vtxCommonSetPitMode(vtxDevice, vtx.pitMode);
}
}

8
src/main/io/vtx.c
View File

@ -179,8 +179,8 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
{
static bool prevPmSwitchState = false;
uint8_t pitOnOff;
if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
unsigned vtxStatus;
if (!ARMING_FLAG(ARMED) && vtxCommonGetStatus(vtxDevice, &vtxStatus)) {
bool currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
if (currPmSwitchState != prevPmSwitchState) {
@ -192,13 +192,13 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
return false;
}
#endif
if (!pitOnOff) {
if (!(vtxStatus & VTX_STATUS_PIT_MODE)) {
vtxCommonSetPitMode(vtxDevice, true);
return true;
}
} else {
if (pitOnOff) {
if (vtxStatus & VTX_STATUS_PIT_MODE) {
vtxCommonSetPitMode(vtxDevice, false);
return true;

16
src/main/io/vtx_rtc6705.c
View File

@ -212,13 +212,6 @@ static bool vtxRTC6705GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pInde
return true;
}
static bool vtxRTC6705GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{
UNUSED(vtxDevice);
UNUSED(pOnOff);
return false;
}
static bool vtxRTC6705GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
{
UNUSED(vtxDevice);
@ -226,6 +219,13 @@ static bool vtxRTC6705GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFrequency
return true;
}
static bool vtxRTC6705GetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
{
UNUSED(vtxDevice);
UNUSED(status);
return false;
}
static vtxVTable_t rtc6705VTable = {
.process = vtxRTC6705Process,
.getDeviceType = vtxRTC6705GetDeviceType,
@ -236,8 +236,8 @@ static vtxVTable_t rtc6705VTable = {
.setFrequency = vtxRTC6705SetFrequency,
.getBandAndChannel = vtxRTC6705GetBandAndChannel,
.getPowerIndex = vtxRTC6705GetPowerIndex,
.getPitMode = vtxRTC6705GetPitMode,
.getFrequency = vtxRTC6705GetFreq,
.getStatus = vtxRTC6705GetStatus,
};
#endif // VTX_COMMON

23
src/main/io/vtx_smartaudio.c
View File

@ -997,16 +997,6 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
return true;
}
static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{
if (!(vtxSAIsReady(vtxDevice) && (saDevice.version >= 2))) {
return false;
}
*pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
return true;
}
static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{
if (!vtxSAIsReady(vtxDevice)) {
@ -1021,6 +1011,17 @@ static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true;
}
static bool vtxSAGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
{
if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
return false;
}
*status = (saDevice.mode & SA_MODE_GET_PITMODE) ? VTX_STATUS_PIT_MODE : 0;
return true;
}
static const vtxVTable_t saVTable = {
.process = vtxSAProcess,
.getDeviceType = vtxSAGetDeviceType,
@ -1031,8 +1032,8 @@ static const vtxVTable_t saVTable = {
.setFrequency = vtxSASetFreq,
.getBandAndChannel = vtxSAGetBandAndChannel,
.getPowerIndex = vtxSAGetPowerIndex,
.getPitMode = vtxSAGetPitMode,
.getFrequency = vtxSAGetFreq,
.getStatus = vtxSAGetStatus,
};
#endif // VTX_COMMON

108
src/main/io/vtx_tramp.c
View File

@ -76,15 +76,20 @@ typedef enum {
trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
uint32_t trampRFFreqMin;
uint32_t trampRFFreqMax;
uint32_t trampRFPowerMax;
static uint32_t trampRFFreqMin = 0;
static uint32_t trampRFFreqMax = 0;
static uint32_t trampRFPowerMax;
uint32_t trampCurFreq = 0;
static uint32_t trampLastFreq = 0;
uint16_t trampPower = 0; // Actual transmitting power
uint16_t trampConfiguredPower = 0; // Configured transmitting power
static uint16_t trampLastPower = 0;
int16_t trampTemperature = 0;
uint8_t trampPitMode = 0;
static uint8_t trampControlMode = 0;
#define TRAMP_CONTROL_RACE_LOCK 0x01
// Maximum number of requests sent to try a config change
#define TRAMP_MAX_RETRIES 2
@ -111,9 +116,19 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
return cksum;
}
static void trampCmdU16(uint8_t cmd, uint16_t param)
static bool trampVtxControlEnabled(void)
{
if (!trampSerialPort || IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
return !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE);
}
static bool trampVtxRaceLockEnabled(void)
{
return trampControlMode & TRAMP_CONTROL_RACE_LOCK;
}
static void trampSendU16(uint8_t cmd, uint16_t param)
{
if (!trampSerialPort) {
return;
}
@ -126,9 +141,20 @@ static void trampCmdU16(uint8_t cmd, uint16_t param)
trampWriteBuf(trampReqBuffer);
}
static void trampSendCommand(uint8_t cmd, uint16_t param)
{
if (trampVtxControlEnabled()) {
trampSendU16(cmd, param);
}
}
static bool trampValidateFreq(uint16_t freq)
{
return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
if (trampRFFreqMin != 0 && trampRFFreqMax != 0) {
return (freq >= trampRFFreqMin && freq <= trampRFFreqMax);
} else {
return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
}
}
static void trampDevSetFreq(uint16_t freq)
@ -146,20 +172,24 @@ void trampSetFreq(uint16_t freq)
void trampSendFreq(uint16_t freq)
{
trampCmdU16('F', freq);
if (!trampVtxRaceLockEnabled()) {
trampSendCommand('F', freq);
}
}
void trampSetRFPower(uint16_t level)
{
trampConfPower = level;
if (trampConfPower != trampPower) {
if (trampConfPower != trampConfiguredPower) {
trampPowerRetries = TRAMP_MAX_RETRIES;
}
}
void trampSendRFPower(uint16_t level)
{
trampCmdU16('P', level);
if (!trampVtxRaceLockEnabled()) {
trampSendCommand('P', level);
}
}
// return false if error
@ -175,7 +205,7 @@ bool trampCommitChanges(void)
void trampSetPitMode(uint8_t onoff)
{
trampCmdU16('I', onoff ? 0 : 1);
trampSendCommand('I', onoff ? 0 : 1);
}
// returns completed response code
@ -203,12 +233,29 @@ static char trampHandleResponse(void)
const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
if (freq != 0) {
trampCurFreq = freq;
if (trampLastFreq != trampCurFreq) {
trampFreqRetries = TRAMP_MAX_RETRIES;
}
trampLastFreq = trampCurFreq;
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
if (trampConfiguredPower != trampLastPower) {
trampPowerRetries = TRAMP_MAX_RETRIES;
}
trampLastPower = trampConfiguredPower;
trampControlMode = trampRespBuffer[6]; // Currently only used for race lock
trampPitMode = trampRespBuffer[7];
trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
if (trampConfFreq == 0) trampConfFreq = trampCurFreq;
if (trampConfPower == 0) trampConfPower = trampPower;
if (trampConfFreq == 0) {
trampConfFreq = trampCurFreq;
}
if (trampConfPower == 0) {
trampConfPower = trampConfiguredPower;
}
return 'v';
}
@ -290,7 +337,7 @@ static char trampReceive(uint32_t currentTimeUs)
trampResetReceiver();
if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0) && !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
return trampHandleResponse();
}
}
@ -308,7 +355,7 @@ static char trampReceive(uint32_t currentTimeUs)
void trampQuery(uint8_t cmd)
{
trampResetReceiver();
trampCmdU16(cmd, 0);
trampSendU16(cmd, 0);
}
void trampQueryR(void)
@ -362,9 +409,8 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
break;
case 'v':
if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
}
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
break;
}
@ -418,7 +464,7 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
trampStatus = TRAMP_STATUS_ONLINE;
// reset configuration value in case it failed (no more retries)
trampConfFreq = trampCurFreq;
trampConfPower = trampPower;
trampConfPower = trampConfiguredPower;
trampFreqRetries = trampPowerRetries = 0;
}
}
@ -473,7 +519,7 @@ static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint
static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{
uint16_t powerValue = 0;
if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue) && (trampRFPowerMax == 0 || powerValue <= trampRFPowerMax)) {
trampSetRFPower(powerValue);
trampCommitChanges();
}
@ -524,16 +570,6 @@ static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
return true;
}
static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{
if (!vtxTrampIsReady(vtxDevice)) {
return false;
}
*pOnOff = trampPitMode;
return true;
}
static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{
if (!vtxTrampIsReady(vtxDevice)) {
@ -544,6 +580,17 @@ static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true;
}
static bool vtxTrampGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
{
if (!vtxTrampIsReady(vtxDevice)) {
return false;
}
*status = (trampPitMode ? VTX_STATUS_PIT_MODE : 0)
| ((trampControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
return true;
}
static const vtxVTable_t trampVTable = {
.process = vtxTrampProcess,
.getDeviceType = vtxTrampGetDeviceType,
@ -554,10 +601,9 @@ static const vtxVTable_t trampVTable = {
.setFrequency = vtxTrampSetFreq,
.getBandAndChannel = vtxTrampGetBandAndChannel,
.getPowerIndex = vtxTrampGetPowerIndex,
.getPitMode = vtxTrampGetPitMode,
.getFrequency = vtxTrampGetFreq,
.getStatus = vtxTrampGetStatus,
};
#endif
bool vtxTrampInit(void)

12
src/main/msp/msp.c
View File

@ -1545,11 +1545,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
case MSP_VTX_CONFIG:
{
const vtxDevice_t *vtxDevice = vtxCommonDevice();
uint8_t pitmode = 0;
unsigned vtxStatus;
vtxDevType_e vtxType = VTXDEV_UNKNOWN;
uint8_t deviceIsReady = 0;
if (vtxDevice) {
vtxCommonGetPitMode(vtxDevice, &pitmode);
vtxCommonGetStatus(vtxDevice, &vtxStatus);
vtxType = vtxCommonGetDeviceType(vtxDevice);
deviceIsReady = vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0;
}
@ -1557,7 +1557,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, vtxSettingsConfig()->band);
sbufWriteU8(dst, vtxSettingsConfig()->channel);
sbufWriteU8(dst, vtxSettingsConfig()->power);
sbufWriteU8(dst, pitmode);
sbufWriteU8(dst, (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0);
sbufWriteU16(dst, vtxSettingsConfig()->freq);
sbufWriteU8(dst, deviceIsReady);
sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
@ -2303,9 +2303,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (vtxType != VTXDEV_UNKNOWN) {
// Delegate pitmode to vtx directly
const uint8_t newPitmode = sbufReadU8(src);
uint8_t currentPitmode = 0;
vtxCommonGetPitMode(vtxDevice, &currentPitmode);
if (currentPitmode != newPitmode) {
unsigned vtxCurrentStatus;
vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus);
if ((bool)(vtxCurrentStatus & VTX_STATUS_PIT_MODE) != (bool)newPitmode) {
vtxCommonSetPitMode(vtxDevice, newPitmode);
}

25
src/main/osd/osd_elements.c
View File

@ -1108,11 +1108,30 @@ static void osdElementVtxChannel(osdElementParms_t *element)
const vtxDevice_t *vtxDevice = vtxCommonDevice();
const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
unsigned vtxStatus = 0;
uint8_t vtxPower = vtxSettingsConfig()->power;
if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
if (vtxDevice) {
vtxCommonGetStatus(vtxDevice, &vtxStatus);
if (vtxSettingsConfig()->lowPowerDisarm) {
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
}
}
char vtxStatusIndicator = '\0';
if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
vtxStatusIndicator = 'D';
} else if (vtxStatus & VTX_STATUS_PIT_MODE) {
vtxStatusIndicator = 'P';
}
if (vtxStatus & VTX_STATUS_LOCKED) {
tfp_sprintf(element->buff, "-:-:-:L");
} else if (vtxStatusIndicator) {
tfp_sprintf(element->buff, "%c:%s:%1d:%c", vtxBandLetter, vtxChannelName, vtxPower, vtxStatusIndicator);
} else {
tfp_sprintf(element->buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
}
tfp_sprintf(element->buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
}
#endif // USE_VTX_COMMON

5
src/main/telemetry/srxl.c
View File

@ -535,14 +535,17 @@ static void collectVtxTmData(spektrumVtx_t * vtx)
vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
// Collect all data from VTX, if VTX is ready
unsigned vtxStatus;
if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
vtxCommonGetPitMode(vtxDevice, &vtx->pitMode) &&
vtxCommonGetStatus(vtxDevice, &vtxStatus) &&
vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
{
vtx->band = 0;
vtx->channel = 0;
vtx->power = 0;
vtx->pitMode = 0;
} else {
vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0;
}
vtx->powerValue = 0;