mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
Tidy of VTX code
This commit is contained in:
parent
b9b10ef88e
commit
19230a436f
10 changed files with 122 additions and 80 deletions
|
@ -16,9 +16,13 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
// Get target build configuration
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#if defined(VTX_CONTROL) && defined(VTX_COMMON)
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "config/config_eeprom.h"
|
#include "config/config_eeprom.h"
|
||||||
|
@ -38,7 +42,6 @@
|
||||||
#include "io/vtx_control.h"
|
#include "io/vtx_control.h"
|
||||||
|
|
||||||
|
|
||||||
#if defined(VTX_CONTROL) && defined(VTX_COMMON)
|
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
|
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
|
||||||
|
|
||||||
|
@ -176,7 +179,6 @@ void vtxCyclePower(const uint8_t powerStep)
|
||||||
void handleVTXControlButton(void)
|
void handleVTXControlButton(void)
|
||||||
{
|
{
|
||||||
#if defined(VTX_RTC6705) && defined(BUTTON_A_PIN)
|
#if defined(VTX_RTC6705) && defined(BUTTON_A_PIN)
|
||||||
bool buttonHeld;
|
|
||||||
bool buttonWasPressed = false;
|
bool buttonWasPressed = false;
|
||||||
uint32_t start = millis();
|
uint32_t start = millis();
|
||||||
uint32_t ledToggleAt = start;
|
uint32_t ledToggleAt = start;
|
||||||
|
@ -184,6 +186,7 @@ void handleVTXControlButton(void)
|
||||||
uint8_t flashesDone = 0;
|
uint8_t flashesDone = 0;
|
||||||
|
|
||||||
uint8_t actionCounter = 0;
|
uint8_t actionCounter = 0;
|
||||||
|
bool buttonHeld;
|
||||||
while ((buttonHeld = buttonAPressed())) {
|
while ((buttonHeld = buttonAPressed())) {
|
||||||
uint32_t end = millis();
|
uint32_t end = millis();
|
||||||
|
|
||||||
|
@ -232,21 +235,20 @@ void handleVTXControlButton(void)
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
|
|
||||||
switch (actionCounter) {
|
switch (actionCounter) {
|
||||||
case 4:
|
case 4:
|
||||||
vtxCycleBandOrChannel(0, +1);
|
vtxCycleBandOrChannel(0, +1);
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
vtxCycleBandOrChannel(+1, 0);
|
vtxCycleBandOrChannel(+1, 0);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
vtxCyclePower(+1);
|
vtxCyclePower(+1);
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
saveConfigAndNotify();
|
saveConfigAndNotify();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -17,6 +17,12 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "config/parameter_group.h"
|
||||||
#include "fc/rc_modes.h"
|
#include "fc/rc_modes.h"
|
||||||
|
|
||||||
#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10
|
#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10
|
||||||
|
|
|
@ -25,7 +25,9 @@
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#if defined(VTX_RTC6705) && defined(VTX_CONTROL)
|
#if defined(VTX_RTC6705) && defined(VTX_CONTROL)
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "cms/cms.h"
|
#include "cms/cms.h"
|
||||||
#include "cms/cms_types.h"
|
#include "cms/cms_types.h"
|
||||||
|
@ -46,8 +48,6 @@
|
||||||
#include "io/vtx_rtc6705.h"
|
#include "io/vtx_rtc6705.h"
|
||||||
#include "io/vtx_string.h"
|
#include "io/vtx_string.h"
|
||||||
|
|
||||||
#include "build/debug.h"
|
|
||||||
|
|
||||||
bool canUpdateVTX(void);
|
bool canUpdateVTX(void);
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0);
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
|
|
||||||
typedef struct vtxRTC6705Config_s {
|
typedef struct vtxRTC6705Config_s {
|
||||||
|
|
|
@ -25,10 +25,10 @@
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
|
#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
|
||||||
#include "cms/cms.h"
|
#include "cms/cms.h"
|
||||||
#include "cms/cms_types.h"
|
#include "cms/cms_types.h"
|
||||||
#include "cms/cms_menu_vtx_smartaudio.h"
|
#include "cms/cms_menu_vtx_smartaudio.h"
|
||||||
|
@ -194,8 +194,9 @@ static void saPrintSettings(void)
|
||||||
int saDacToPowerIndex(int dac)
|
int saDacToPowerIndex(int dac)
|
||||||
{
|
{
|
||||||
for (int idx = 3 ; idx >= 0 ; idx--) {
|
for (int idx = 3 ; idx >= 0 ; idx--) {
|
||||||
if (saPowerTable[idx].valueV1 <= dac)
|
if (saPowerTable[idx].valueV1 <= dac) {
|
||||||
return idx;
|
return idx;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -214,9 +215,10 @@ static int sa_baudstep = 50;
|
||||||
|
|
||||||
static void saAutobaud(void)
|
static void saAutobaud(void)
|
||||||
{
|
{
|
||||||
if (saStat.pktsent < 10)
|
if (saStat.pktsent < 10) {
|
||||||
// Not enough samples collected
|
// Not enough samples collected
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
#if 0
|
#if 0
|
||||||
dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
|
dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
|
||||||
|
@ -277,8 +279,9 @@ static void saProcessResponse(uint8_t *buf, int len)
|
||||||
switch (resp) {
|
switch (resp) {
|
||||||
case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
|
case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
|
||||||
case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
|
case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
|
||||||
if (len < 7)
|
if (len < 7) {
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
|
saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
|
||||||
saDevice.channel = buf[2];
|
saDevice.channel = buf[2];
|
||||||
|
@ -301,10 +304,11 @@ static void saProcessResponse(uint8_t *buf, int len)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case SA_CMD_SET_FREQ: // Set Frequency
|
case SA_CMD_SET_FREQ: // Set Frequency
|
||||||
if (len < 5)
|
if (len < 5) {
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
uint16_t freq = (buf[2] << 8)|buf[3];
|
const uint16_t freq = (buf[2] << 8)|buf[3];
|
||||||
|
|
||||||
if (freq & SA_FREQ_GETPIT) {
|
if (freq & SA_FREQ_GETPIT) {
|
||||||
saDevice.orfreq = freq & SA_FREQ_MASK;
|
saDevice.orfreq = freq & SA_FREQ_MASK;
|
||||||
|
@ -328,8 +332,9 @@ static void saProcessResponse(uint8_t *buf, int len)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Debug
|
// Debug
|
||||||
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t)))
|
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
|
||||||
saPrintSettings();
|
saPrintSettings();
|
||||||
|
}
|
||||||
saDevicePrev = saDevice;
|
saDevicePrev = saDevice;
|
||||||
|
|
||||||
#ifdef VTX_COMMON
|
#ifdef VTX_COMMON
|
||||||
|
@ -427,12 +432,11 @@ static void saReceiveFramer(uint8_t c)
|
||||||
|
|
||||||
static void saSendFrame(uint8_t *buf, int len)
|
static void saSendFrame(uint8_t *buf, int len)
|
||||||
{
|
{
|
||||||
int i;
|
|
||||||
|
|
||||||
serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
|
serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
|
||||||
|
|
||||||
for (i = 0 ; i < len ; i++)
|
for (int i = 0 ; i < len ; i++) {
|
||||||
serialWrite(smartAudioSerialPort, buf[i]);
|
serialWrite(smartAudioSerialPort, buf[i]);
|
||||||
|
}
|
||||||
|
|
||||||
serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
|
serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this
|
||||||
|
|
||||||
|
@ -467,10 +471,9 @@ static void saResendCmd(void)
|
||||||
|
|
||||||
static void saSendCmd(uint8_t *buf, int len)
|
static void saSendCmd(uint8_t *buf, int len)
|
||||||
{
|
{
|
||||||
int i;
|
for (int i = 0 ; i < len ; i++) {
|
||||||
|
|
||||||
for (i = 0 ; i < len ; i++)
|
|
||||||
sa_osbuf[i] = buf[i];
|
sa_osbuf[i] = buf[i];
|
||||||
|
}
|
||||||
|
|
||||||
sa_oslen = len;
|
sa_oslen = len;
|
||||||
sa_outstanding = (buf[2] >> 1);
|
sa_outstanding = (buf[2] >> 1);
|
||||||
|
@ -513,8 +516,9 @@ static bool saQueueFull(void)
|
||||||
|
|
||||||
static void saQueueCmd(uint8_t *buf, int len)
|
static void saQueueCmd(uint8_t *buf, int len)
|
||||||
{
|
{
|
||||||
if (saQueueFull())
|
if (saQueueFull()) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
sa_queue[sa_qhead].buf = buf;
|
sa_queue[sa_qhead].buf = buf;
|
||||||
sa_queue[sa_qhead].len = len;
|
sa_queue[sa_qhead].len = len;
|
||||||
|
@ -523,8 +527,9 @@ static void saQueueCmd(uint8_t *buf, int len)
|
||||||
|
|
||||||
static void saSendQueue(void)
|
static void saSendQueue(void)
|
||||||
{
|
{
|
||||||
if (saQueueEmpty())
|
if (saQueueEmpty()) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
|
saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
|
||||||
sa_qtail = (sa_qtail + 1) % SA_QSIZE;
|
sa_qtail = (sa_qtail + 1) % SA_QSIZE;
|
||||||
|
@ -601,8 +606,9 @@ void saSetPowerByIndex(uint8_t index)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (index > 3)
|
if (index > 3) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
|
buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2;
|
||||||
buf[5] = CRC8(buf, 5);
|
buf[5] = CRC8(buf, 5);
|
||||||
|
@ -646,8 +652,9 @@ void vtxSAProcess(uint32_t now)
|
||||||
{
|
{
|
||||||
static char initPhase = 0;
|
static char initPhase = 0;
|
||||||
|
|
||||||
if (smartAudioSerialPort == NULL)
|
if (smartAudioSerialPort == NULL) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
|
while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
|
||||||
uint8_t c = serialRead(smartAudioSerialPort);
|
uint8_t c = serialRead(smartAudioSerialPort);
|
||||||
|
@ -673,8 +680,7 @@ void vtxSAProcess(uint32_t now)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((sa_outstanding != SA_CMD_NONE)
|
if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
|
||||||
&& (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
|
|
||||||
// Last command timed out
|
// Last command timed out
|
||||||
// dprintf(("process: resending 0x%x\r\n", sa_outstanding));
|
// dprintf(("process: resending 0x%x\r\n", sa_outstanding));
|
||||||
// XXX Todo: Resend termination and possible offline transition
|
// XXX Todo: Resend termination and possible offline transition
|
||||||
|
@ -694,8 +700,9 @@ void vtxSAProcess(uint32_t now)
|
||||||
// Testing VTX_COMMON API
|
// Testing VTX_COMMON API
|
||||||
{
|
{
|
||||||
static uint32_t lastMonitorUs = 0;
|
static uint32_t lastMonitorUs = 0;
|
||||||
if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000)
|
if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
static uint8_t monBand;
|
static uint8_t monBand;
|
||||||
static uint8_t monChan;
|
static uint8_t monChan;
|
||||||
|
@ -725,8 +732,9 @@ bool vtxSAIsReady(void)
|
||||||
|
|
||||||
void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
|
void vtxSASetBandAndChannel(uint8_t band, uint8_t channel)
|
||||||
{
|
{
|
||||||
if (band && channel)
|
if (band && channel) {
|
||||||
saSetBandAndChannel(band - 1, channel - 1);
|
saSetBandAndChannel(band - 1, channel - 1);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void vtxSASetPowerByIndex(uint8_t index)
|
void vtxSASetPowerByIndex(uint8_t index)
|
||||||
|
@ -741,8 +749,9 @@ void vtxSASetPowerByIndex(uint8_t index)
|
||||||
|
|
||||||
void vtxSASetPitMode(uint8_t onoff)
|
void vtxSASetPitMode(uint8_t onoff)
|
||||||
{
|
{
|
||||||
if (!(vtxSAIsReady() && (saDevice.version == 2)))
|
if (!(vtxSAIsReady() && (saDevice.version == 2))) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (onoff) {
|
if (onoff) {
|
||||||
// SmartAudio can not turn pit mode on by software.
|
// SmartAudio can not turn pit mode on by software.
|
||||||
|
@ -751,11 +760,13 @@ void vtxSASetPitMode(uint8_t onoff)
|
||||||
|
|
||||||
uint8_t newmode = SA_MODE_CLR_PITMODE;
|
uint8_t newmode = SA_MODE_CLR_PITMODE;
|
||||||
|
|
||||||
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE)
|
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
|
||||||
newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
|
newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
|
||||||
|
}
|
||||||
|
|
||||||
if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
|
if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
|
||||||
newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
|
newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
|
||||||
|
}
|
||||||
|
|
||||||
saSetMode(newmode);
|
saSetMode(newmode);
|
||||||
|
|
||||||
|
@ -764,8 +775,9 @@ void vtxSASetPitMode(uint8_t onoff)
|
||||||
|
|
||||||
bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
||||||
{
|
{
|
||||||
if (!vtxSAIsReady())
|
if (!vtxSAIsReady()) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
*pBand = (saDevice.channel / 8) + 1;
|
*pBand = (saDevice.channel / 8) + 1;
|
||||||
*pChannel = (saDevice.channel % 8) + 1;
|
*pChannel = (saDevice.channel % 8) + 1;
|
||||||
|
@ -774,8 +786,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
||||||
|
|
||||||
bool vtxSAGetPowerIndex(uint8_t *pIndex)
|
bool vtxSAGetPowerIndex(uint8_t *pIndex)
|
||||||
{
|
{
|
||||||
if (!vtxSAIsReady())
|
if (!vtxSAIsReady()) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
*pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
|
*pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
|
||||||
return true;
|
return true;
|
||||||
|
@ -783,8 +796,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex)
|
||||||
|
|
||||||
bool vtxSAGetPitMode(uint8_t *pOnOff)
|
bool vtxSAGetPitMode(uint8_t *pOnOff)
|
||||||
{
|
{
|
||||||
if (!(vtxSAIsReady() && (saDevice.version == 2)))
|
if (!(vtxSAIsReady() && (saDevice.version == 2))) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
*pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
|
*pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0;
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -1,5 +1,10 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
// opmode flags, GET side
|
// opmode flags, GET side
|
||||||
#define SA_MODE_GET_FREQ_BY_FREQ 1
|
#define SA_MODE_GET_FREQ_BY_FREQ 1
|
||||||
#define SA_MODE_GET_PITMODE 2
|
#define SA_MODE_GET_PITMODE 2
|
||||||
|
|
|
@ -19,10 +19,9 @@
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <ctype.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
#if defined(VTX_COMMON)
|
#if defined(VTX_COMMON)
|
||||||
|
@ -53,13 +52,10 @@ const char * const vtx58ChannelNames[] = {
|
||||||
|
|
||||||
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
|
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
|
||||||
{
|
{
|
||||||
int8_t band;
|
|
||||||
uint8_t channel;
|
|
||||||
|
|
||||||
// Use reverse lookup order so that 5880Mhz
|
// Use reverse lookup order so that 5880Mhz
|
||||||
// get Raceband 7 instead of Fatshark 8.
|
// get Raceband 7 instead of Fatshark 8.
|
||||||
for (band = 4 ; band >= 0 ; band--) {
|
for (int band = 4 ; band >= 0 ; band--) {
|
||||||
for (channel = 0 ; channel < 8 ; channel++) {
|
for (int channel = 0 ; channel < 8 ; channel++) {
|
||||||
if (vtx58frequencyTable[band][channel] == freq) {
|
if (vtx58frequencyTable[band][channel] == freq) {
|
||||||
*pBand = band + 1;
|
*pBand = band + 1;
|
||||||
*pChannel = channel + 1;
|
*pChannel = channel + 1;
|
||||||
|
|
|
@ -2,6 +2,8 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
#if defined(VTX_COMMON)
|
#if defined(VTX_COMMON)
|
||||||
|
|
||||||
extern const uint16_t vtx58frequencyTable[5][8];
|
extern const uint16_t vtx58frequencyTable[5][8];
|
||||||
|
|
|
@ -107,16 +107,18 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
|
||||||
{
|
{
|
||||||
uint8_t cksum = 0;
|
uint8_t cksum = 0;
|
||||||
|
|
||||||
for (int i = 1 ; i < 14 ; i++)
|
for (int i = 1 ; i < 14 ; i++) {
|
||||||
cksum += trampBuf[i];
|
cksum += trampBuf[i];
|
||||||
|
}
|
||||||
|
|
||||||
return cksum;
|
return cksum;
|
||||||
}
|
}
|
||||||
|
|
||||||
void trampCmdU16(uint8_t cmd, uint16_t param)
|
void trampCmdU16(uint8_t cmd, uint16_t param)
|
||||||
{
|
{
|
||||||
if (!trampSerialPort)
|
if (!trampSerialPort) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
|
||||||
trampReqBuffer[0] = 15;
|
trampReqBuffer[0] = 15;
|
||||||
|
@ -130,8 +132,9 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
|
||||||
void trampSetFreq(uint16_t freq)
|
void trampSetFreq(uint16_t freq)
|
||||||
{
|
{
|
||||||
trampConfFreq = freq;
|
trampConfFreq = freq;
|
||||||
if (trampConfFreq != trampCurFreq)
|
if (trampConfFreq != trampCurFreq) {
|
||||||
trampFreqRetries = TRAMP_MAX_RETRIES;
|
trampFreqRetries = TRAMP_MAX_RETRIES;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void trampSendFreq(uint16_t freq)
|
void trampSendFreq(uint16_t freq)
|
||||||
|
@ -147,8 +150,9 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel)
|
||||||
void trampSetRFPower(uint16_t level)
|
void trampSetRFPower(uint16_t level)
|
||||||
{
|
{
|
||||||
trampConfPower = level;
|
trampConfPower = level;
|
||||||
if (trampConfPower != trampPower)
|
if (trampConfPower != trampPower) {
|
||||||
trampPowerRetries = TRAMP_MAX_RETRIES;
|
trampPowerRetries = TRAMP_MAX_RETRIES;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void trampSendRFPower(uint16_t level)
|
void trampSendRFPower(uint16_t level)
|
||||||
|
@ -159,8 +163,9 @@ void trampSendRFPower(uint16_t level)
|
||||||
// return false if error
|
// return false if error
|
||||||
bool trampCommitChanges(void)
|
bool trampCommitChanges(void)
|
||||||
{
|
{
|
||||||
if (trampStatus != TRAMP_STATUS_ONLINE)
|
if (trampStatus != TRAMP_STATUS_ONLINE) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
||||||
return true;
|
return true;
|
||||||
|
@ -174,12 +179,12 @@ void trampSetPitMode(uint8_t onoff)
|
||||||
// returns completed response code
|
// returns completed response code
|
||||||
char trampHandleResponse(void)
|
char trampHandleResponse(void)
|
||||||
{
|
{
|
||||||
uint8_t respCode = trampRespBuffer[1];
|
const uint8_t respCode = trampRespBuffer[1];
|
||||||
|
|
||||||
switch (respCode) {
|
switch (respCode) {
|
||||||
case 'r':
|
case 'r':
|
||||||
{
|
{
|
||||||
uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
||||||
if (min_freq != 0) {
|
if (min_freq != 0) {
|
||||||
trampRFFreqMin = min_freq;
|
trampRFFreqMin = min_freq;
|
||||||
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||||
|
@ -193,7 +198,7 @@ char trampHandleResponse(void)
|
||||||
|
|
||||||
case 'v':
|
case 'v':
|
||||||
{
|
{
|
||||||
uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
|
||||||
if (freq != 0) {
|
if (freq != 0) {
|
||||||
trampCurFreq = freq;
|
trampCurFreq = freq;
|
||||||
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
|
||||||
|
@ -212,7 +217,7 @@ char trampHandleResponse(void)
|
||||||
|
|
||||||
case 's':
|
case 's':
|
||||||
{
|
{
|
||||||
uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
|
const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
|
||||||
if (temp != 0) {
|
if (temp != 0) {
|
||||||
trampTemperature = temp;
|
trampTemperature = temp;
|
||||||
return 's';
|
return 's';
|
||||||
|
@ -241,10 +246,11 @@ static void trampResetReceiver(void)
|
||||||
|
|
||||||
static bool trampIsValidResponseCode(uint8_t code)
|
static bool trampIsValidResponseCode(uint8_t code)
|
||||||
{
|
{
|
||||||
if (code == 'r' || code == 'v' || code == 's')
|
if (code == 'r' || code == 'v' || code == 's') {
|
||||||
return true;
|
return true;
|
||||||
else
|
} else {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// returns completed response code or 0
|
// returns completed response code or 0
|
||||||
|
@ -252,11 +258,12 @@ static char trampReceive(uint32_t currentTimeUs)
|
||||||
{
|
{
|
||||||
UNUSED(currentTimeUs);
|
UNUSED(currentTimeUs);
|
||||||
|
|
||||||
if (!trampSerialPort)
|
if (!trampSerialPort) {
|
||||||
return 0;
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
while (serialRxBytesWaiting(trampSerialPort)) {
|
while (serialRxBytesWaiting(trampSerialPort)) {
|
||||||
uint8_t c = serialRead(trampSerialPort);
|
const uint8_t c = serialRead(trampSerialPort);
|
||||||
trampRespBuffer[trampReceivePos++] = c;
|
trampRespBuffer[trampReceivePos++] = c;
|
||||||
|
|
||||||
switch (trampReceiveState) {
|
switch (trampReceiveState) {
|
||||||
|
@ -290,6 +297,7 @@ static char trampReceive(uint32_t currentTimeUs)
|
||||||
|
|
||||||
default:
|
default:
|
||||||
trampResetReceiver();
|
trampResetReceiver();
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -326,10 +334,11 @@ void vtxTrampProcess(uint32_t currentTimeUs)
|
||||||
static uint16_t debugPowReqCounter = 0;
|
static uint16_t debugPowReqCounter = 0;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (trampStatus == TRAMP_STATUS_BAD_DEVICE)
|
if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
char replyCode = trampReceive(currentTimeUs);
|
const char replyCode = trampReceive(currentTimeUs);
|
||||||
|
|
||||||
#ifdef TRAMP_DEBUG
|
#ifdef TRAMP_DEBUG
|
||||||
debug[0] = trampStatus;
|
debug[0] = trampStatus;
|
||||||
|
@ -337,13 +346,15 @@ void vtxTrampProcess(uint32_t currentTimeUs)
|
||||||
|
|
||||||
switch (replyCode) {
|
switch (replyCode) {
|
||||||
case 'r':
|
case 'r':
|
||||||
if (trampStatus <= TRAMP_STATUS_OFFLINE)
|
if (trampStatus <= TRAMP_STATUS_OFFLINE) {
|
||||||
trampStatus = TRAMP_STATUS_ONLINE;
|
trampStatus = TRAMP_STATUS_ONLINE;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'v':
|
case 'v':
|
||||||
if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW)
|
if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
|
||||||
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
trampStatus = TRAMP_STATUS_SET_FREQ_PW;
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -353,14 +364,15 @@ void vtxTrampProcess(uint32_t currentTimeUs)
|
||||||
case TRAMP_STATUS_ONLINE:
|
case TRAMP_STATUS_ONLINE:
|
||||||
if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
|
if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
|
||||||
|
|
||||||
if (trampStatus == TRAMP_STATUS_OFFLINE)
|
if (trampStatus == TRAMP_STATUS_OFFLINE) {
|
||||||
trampQueryR();
|
trampQueryR();
|
||||||
else {
|
} else {
|
||||||
static unsigned int cnt = 0;
|
static unsigned int cnt = 0;
|
||||||
if (((cnt++) & 1) == 0)
|
if (((cnt++) & 1) == 0) {
|
||||||
trampQueryV();
|
trampQueryV();
|
||||||
else
|
} else {
|
||||||
trampQueryS();
|
trampQueryS();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
lastQueryTimeUs = currentTimeUs;
|
lastQueryTimeUs = currentTimeUs;
|
||||||
|
@ -377,8 +389,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
|
||||||
debugFreqReqCounter++;
|
debugFreqReqCounter++;
|
||||||
#endif
|
#endif
|
||||||
done = false;
|
done = false;
|
||||||
}
|
} else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
|
||||||
else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
|
|
||||||
trampSendRFPower(trampConfPower);
|
trampSendRFPower(trampConfPower);
|
||||||
trampPowerRetries--;
|
trampPowerRetries--;
|
||||||
#ifdef TRAMP_DEBUG
|
#ifdef TRAMP_DEBUG
|
||||||
|
@ -392,8 +403,7 @@ void vtxTrampProcess(uint32_t currentTimeUs)
|
||||||
|
|
||||||
// delay next status query by 300ms
|
// delay next status query by 300ms
|
||||||
lastQueryTimeUs = currentTimeUs + 300 * 1000;
|
lastQueryTimeUs = currentTimeUs + 300 * 1000;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
// everything has been done, let's return to original state
|
// everything has been done, let's return to original state
|
||||||
trampStatus = TRAMP_STATUS_ONLINE;
|
trampStatus = TRAMP_STATUS_ONLINE;
|
||||||
// reset configuration value in case it failed (no more retries)
|
// reset configuration value in case it failed (no more retries)
|
||||||
|
@ -464,8 +474,9 @@ void vtxTrampSetPitMode(uint8_t onoff)
|
||||||
|
|
||||||
bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
||||||
{
|
{
|
||||||
if (!vtxTrampIsReady())
|
if (!vtxTrampIsReady()) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
*pBand = trampBand;
|
*pBand = trampBand;
|
||||||
*pChannel = trampChannel;
|
*pChannel = trampChannel;
|
||||||
|
@ -474,8 +485,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
|
||||||
|
|
||||||
bool vtxTrampGetPowerIndex(uint8_t *pIndex)
|
bool vtxTrampGetPowerIndex(uint8_t *pIndex)
|
||||||
{
|
{
|
||||||
if (!vtxTrampIsReady())
|
if (!vtxTrampIsReady()) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (trampConfiguredPower > 0) {
|
if (trampConfiguredPower > 0) {
|
||||||
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
|
for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
|
||||||
|
@ -491,8 +503,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
|
||||||
|
|
||||||
bool vtxTrampGetPitMode(uint8_t *pOnOff)
|
bool vtxTrampGetPitMode(uint8_t *pOnOff)
|
||||||
{
|
{
|
||||||
if (!vtxTrampIsReady())
|
if (!vtxTrampIsReady()) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
*pOnOff = trampPitMode;
|
*pOnOff = trampPitMode;
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -1,5 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
#define VTX_TRAMP_POWER_COUNT 5
|
#define VTX_TRAMP_POWER_COUNT 5
|
||||||
extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
|
extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
|
||||||
extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];
|
extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue