mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
SmartAudio Support
This commit is contained in:
jflyper
parent
81623d4ac7
commit
8334cd35ff
14 changed files with 858 additions and 22 deletions
|
|
@ -170,7 +170,7 @@ typedef struct master_s {
|
|||
uint8_t transponderData[6];
|
||||
#endif
|
||||
|
||||
#ifdef USE_RTC6705
|
||||
#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
uint8_t vtx_channel;
|
||||
uint8_t vtx_power;
|
||||
#endif
|
||||
|
|
|
|||
780
src/main/drivers/vtx_smartaudio.c
Normal file
780
src/main/drivers/vtx_smartaudio.c
Normal file
|
|
@ -0,0 +1,780 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef VTX_SMARTAUDIO
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/adc.h"
|
||||
#include "drivers/light_led.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/escservo.h"
|
||||
//#include "fc/rc_controls.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/osd.h"
|
||||
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "flight/altitudehold.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/smartport.h"
|
||||
|
||||
//#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/config_profile.h"
|
||||
//#include "config/feature.h"
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
#include "drivers/vtx_smartaudio.h"
|
||||
|
||||
//#include "build/debug.h"
|
||||
#include "debug.h"
|
||||
|
||||
//#define SMARTAUDIO_DPRINTF
|
||||
//#define SMARTAUDIO_DEBUG_MONITOR
|
||||
|
||||
#ifdef SMARTAUDIO_DPRINTF
|
||||
#include "common/printf.h"
|
||||
serialPort_t *debugSerialPort = NULL;
|
||||
#define dprintf(x) if (debugSerialPort) printf x;
|
||||
#else
|
||||
#define dprintf(x)
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
extern profile_t *currentProfile;
|
||||
extern controlRateConfig_t *currentControlRateProfile;
|
||||
#endif
|
||||
|
||||
static serialPort_t *smartAudioSerialPort = NULL;
|
||||
|
||||
// SmartAudio command and response codes
|
||||
enum {
|
||||
SA_CMD_NONE = 0x00,
|
||||
SA_CMD_GET_SETTINGS = 0x01,
|
||||
SA_CMD_SET_POWER,
|
||||
SA_CMD_SET_CHAN,
|
||||
SA_CMD_SET_FREQ,
|
||||
SA_CMD_SET_MODE,
|
||||
SA_CMD_GET_SETTINGS_V2 = 0x09 // Response only
|
||||
} smartAudioCommand_e;
|
||||
|
||||
#define SACMD(cmd) (((cmd) << 1) | 1)
|
||||
|
||||
// opmode flags, GET side
|
||||
#define SA_MODE_GET_FREQ_BY_FREQ 1
|
||||
#define SA_MODE_GET_PITMODE 2
|
||||
#define SA_MODE_GET_IN_RANGE_PITMODE 4
|
||||
#define SA_MODE_GET_OUT_RANGE_PITMODE 8
|
||||
#define SA_MODE_GET_UNLOCK 16
|
||||
|
||||
// opmode flags, SET side
|
||||
#define SA_MODE_SET_IN_RANGE_PITMODE 1
|
||||
#define SA_MODE_SET_OUT_RANGE_PITMODE 2
|
||||
#define SA_MODE_SET_PITMODE 4
|
||||
#define SA_MODE_CLR_PITMODE 4
|
||||
#define SA_MODE_SET_UNLOCK 8
|
||||
#define SA_MODE_SET_LOCK 0 // ~UNLOCK
|
||||
|
||||
// SetFrequency flags, for pit mode frequency manipulation
|
||||
#define SA_FREQ_GETPIT (1 << 14)
|
||||
#define SA_FREQ_SETPIT (1 << 15)
|
||||
|
||||
// Error counters, may be good for post production debugging.
|
||||
static uint16_t saerr_badlen = 0;
|
||||
static uint16_t saerr_crc = 0;
|
||||
static uint16_t saerr_oooresp = 0;
|
||||
|
||||
// Receive frame reassembly buffer
|
||||
#define SMARTAUDIO_MAXLEN 32
|
||||
static uint8_t sa_rbuf[SMARTAUDIO_MAXLEN];
|
||||
|
||||
// CRC8 computations
|
||||
|
||||
#define POLYGEN 0xd5
|
||||
|
||||
static uint8_t CRC8(uint8_t *data, int8_t len)
|
||||
{
|
||||
uint8_t crc = 0; /* start with 0 so first byte can be 'xored' in */
|
||||
uint8_t currByte;
|
||||
|
||||
for (int i = 0 ; i < len ; i++) {
|
||||
currByte = data[i];
|
||||
|
||||
crc ^= currByte; /* XOR-in the next input byte */
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if ((crc & 0x80) != 0) {
|
||||
crc = (uint8_t)((crc << 1) ^ POLYGEN);
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
// Last received device states
|
||||
|
||||
static int8_t sa_vers = 0; // Will be set to 1 or 2
|
||||
static int8_t sa_chan = -1;
|
||||
static int8_t sa_power = -1;
|
||||
static int8_t sa_opmode = -1;
|
||||
static uint16_t sa_freq = 0;
|
||||
static uint16_t sa_pitfreq = 0;
|
||||
static bool sa_pitfreqpending = false;
|
||||
|
||||
static void smartAudioPrintSettings(void)
|
||||
{
|
||||
#ifdef SMARTAUDIO_DPRINTF
|
||||
static int osa_vers;
|
||||
static int osa_chan;
|
||||
static int osa_power;
|
||||
static int osa_opmode;
|
||||
static uint32_t osa_freq;
|
||||
|
||||
if ((osa_vers == sa_vers)
|
||||
&& (osa_chan == sa_chan)
|
||||
&& (osa_power == sa_power)
|
||||
&& (osa_opmode == sa_opmode)
|
||||
&& (osa_freq == sa_freq))
|
||||
return;
|
||||
|
||||
dprintf(("Settings:\r\n"));
|
||||
dprintf((" version: %d\r\n", sa_vers));
|
||||
dprintf((" mode(0x%x): vtx=%s", sa_opmode, (sa_opmode & 1) ? "freq" : "chan"));
|
||||
dprintf((" pit=%s ", (sa_opmode & 2) ? "on " : "off"));
|
||||
dprintf((" inb=%s", (sa_opmode & 4) ? "on " : "off"));
|
||||
dprintf((" outb=%s", (sa_opmode & 8) ? "on " : "off"));
|
||||
dprintf((" lock=%s\r\n", (sa_opmode & 16) ? "unlocked" : "locked"));
|
||||
dprintf((" chan: %d\r\n", sa_chan));
|
||||
dprintf((" freq: %d\r\n", sa_freq));
|
||||
dprintf((" power: %d\r\n", sa_power));
|
||||
dprintf(("\r\n"));
|
||||
|
||||
osa_vers = sa_vers;
|
||||
osa_chan = sa_chan;
|
||||
osa_power = sa_power;
|
||||
osa_opmode = sa_opmode;
|
||||
osa_freq = sa_freq;
|
||||
#endif
|
||||
}
|
||||
|
||||
// Autobauding
|
||||
|
||||
#define SMARTBAUD_MIN 4800
|
||||
#define SMARTBAUD_MAX 4950
|
||||
static int smartbaud = SMARTBAUD_MIN;
|
||||
static int adjdir = 1; // -1=going down, 1=going up
|
||||
static int baudstep = 50;
|
||||
|
||||
#define SMARTAUDIO_CMD_TIMEOUT 120
|
||||
|
||||
// Statistics for autobauding
|
||||
|
||||
static int sa_pktsent = 0;
|
||||
static int sa_pktrcvd = 0;
|
||||
|
||||
static void saAutobaud(void)
|
||||
{
|
||||
if (sa_pktsent < 10)
|
||||
// Not enough samples collected
|
||||
return;
|
||||
|
||||
dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n",
|
||||
smartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent)));
|
||||
|
||||
if (((sa_pktrcvd * 100) / sa_pktsent) >= 70) {
|
||||
// This is okay
|
||||
sa_pktsent = 0; // Should be more moderate?
|
||||
sa_pktrcvd = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
dprintf(("autobaud: adjusting\r\n"));
|
||||
|
||||
if ((adjdir == 1) && (smartbaud == SMARTBAUD_MAX)) {
|
||||
adjdir = -1;
|
||||
dprintf(("autobaud: now going down\r\n"));
|
||||
} else if ((adjdir == -1 && smartbaud == SMARTBAUD_MIN)) {
|
||||
adjdir = 1;
|
||||
dprintf(("autobaud: now going up\r\n"));
|
||||
}
|
||||
|
||||
smartbaud += baudstep * adjdir;
|
||||
|
||||
dprintf(("autobaud: %d\r\n", smartbaud));
|
||||
|
||||
smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, smartbaud);
|
||||
|
||||
sa_pktsent = 0;
|
||||
sa_pktrcvd = 0;
|
||||
}
|
||||
|
||||
// Transport level protocol variables
|
||||
|
||||
static uint32_t sa_lastTransmission = 0;
|
||||
static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command
|
||||
static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission
|
||||
static int sa_oslen; // And associate length
|
||||
|
||||
static void saProcessResponse(uint8_t *buf, int len)
|
||||
{
|
||||
uint8_t resp = buf[0];
|
||||
|
||||
if (resp == sa_outstanding) {
|
||||
sa_outstanding = SA_CMD_NONE;
|
||||
} else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) {
|
||||
sa_outstanding = SA_CMD_NONE;
|
||||
} else {
|
||||
saerr_oooresp++;
|
||||
dprintf(("processResponse: outstanding %d got %d\r\n", outstanding, resp));
|
||||
}
|
||||
|
||||
switch(resp) {
|
||||
case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings
|
||||
case SA_CMD_GET_SETTINGS: // Version 1 Get Settings
|
||||
if (len < 7)
|
||||
break;
|
||||
|
||||
sa_vers = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
|
||||
sa_chan = buf[2];
|
||||
sa_power = buf[3];
|
||||
sa_opmode = buf[4];
|
||||
sa_freq = (buf[5] << 8)|buf[6];
|
||||
|
||||
smartAudioPrintSettings();
|
||||
|
||||
#ifdef SMARTAUDIO_DEBUG_MONITOR
|
||||
debug[0] = sa_vers * 100 + sa_opmode;
|
||||
debug[1] = sa_chan;
|
||||
debug[2] = sa_freq;
|
||||
debug[3] = sa_power;
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
||||
case SA_CMD_SET_POWER: // Set Power
|
||||
break;
|
||||
|
||||
case SA_CMD_SET_CHAN: // Set Channel
|
||||
break;
|
||||
|
||||
case SA_CMD_SET_FREQ: // Set Frequency
|
||||
if (len < 5)
|
||||
break;
|
||||
|
||||
if (sa_pitfreqpending)
|
||||
sa_pitfreq = ((buf[2] << 8)|buf[3]) & ~SA_FREQ_GETPIT;
|
||||
break;
|
||||
|
||||
case SA_CMD_SET_MODE: // Set Mode
|
||||
dprintf(("resp SET_MODE 0x%x\r\n", buf[2]));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void saReceiveFramer(uint8_t c)
|
||||
{
|
||||
|
||||
static enum saFramerState_e {
|
||||
S_WAITPRE1, // Waiting for preamble 1 (0xAA)
|
||||
S_WAITPRE2, // Waiting for preamble 2 (0x55)
|
||||
S_WAITRESP, // Waiting for response code
|
||||
S_WAITLEN, // Waiting for length
|
||||
S_DATA, // Receiving data
|
||||
S_WAITCRC, // Waiting for CRC
|
||||
} state = S_WAITPRE1;
|
||||
|
||||
static int len;
|
||||
static int dlen;
|
||||
|
||||
switch(state) {
|
||||
case S_WAITPRE1:
|
||||
if (c == 0xAA) {
|
||||
state = S_WAITPRE2;
|
||||
} else {
|
||||
state = S_WAITPRE1; // Bogus
|
||||
}
|
||||
break;
|
||||
|
||||
case S_WAITPRE2:
|
||||
if (c == 0x55) {
|
||||
state = S_WAITRESP;
|
||||
} else {
|
||||
state = S_WAITPRE1;
|
||||
}
|
||||
break;
|
||||
|
||||
case S_WAITRESP:
|
||||
sa_rbuf[0] = c;
|
||||
state = S_WAITLEN;
|
||||
break;
|
||||
|
||||
case S_WAITLEN:
|
||||
sa_rbuf[1] = c;
|
||||
len = c;
|
||||
|
||||
if (len > SMARTAUDIO_MAXLEN - 2) {
|
||||
saerr_badlen++;
|
||||
state = S_WAITPRE1;
|
||||
} else if (len == 0) {
|
||||
state = S_WAITCRC;
|
||||
} else {
|
||||
dlen = 0;
|
||||
state = S_DATA;
|
||||
}
|
||||
break;
|
||||
|
||||
case S_DATA:
|
||||
sa_rbuf[2 + dlen] = c;
|
||||
if (++dlen == len) {
|
||||
state = S_WAITCRC;
|
||||
}
|
||||
break;
|
||||
|
||||
case S_WAITCRC:
|
||||
if (CRC8(sa_rbuf, 2 + len) == c) {
|
||||
// Response
|
||||
saProcessResponse(sa_rbuf, len + 2);
|
||||
sa_pktrcvd++;
|
||||
} else if (sa_rbuf[0] & 1) {
|
||||
// Command echo (may be)
|
||||
} else {
|
||||
saerr_crc++;
|
||||
}
|
||||
state = S_WAITPRE1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Output framer
|
||||
|
||||
static void saSendFrame(uint8_t *buf, int len)
|
||||
{
|
||||
int i;
|
||||
|
||||
serialWrite(smartAudioSerialPort, 0x00);
|
||||
|
||||
for (i = 0 ; i < len ; i++)
|
||||
serialWrite(smartAudioSerialPort, buf[i]);
|
||||
|
||||
serialWrite(smartAudioSerialPort, 0x00);
|
||||
|
||||
sa_lastTransmission = millis();
|
||||
sa_pktsent++;
|
||||
}
|
||||
|
||||
static void saResendCmd(void)
|
||||
{
|
||||
saSendFrame(sa_osbuf, sa_oslen);
|
||||
}
|
||||
|
||||
static void saSendCmd(uint8_t *buf, int len)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0 ; i < len ; i++)
|
||||
sa_osbuf[i] = buf[i];
|
||||
|
||||
sa_oslen = len;
|
||||
sa_outstanding = (buf[2] >> 1);
|
||||
|
||||
saSendFrame(sa_osbuf, sa_oslen);
|
||||
}
|
||||
|
||||
// Command transmission queue and management
|
||||
|
||||
typedef struct saCmdQueue_s {
|
||||
uint8_t *buf;
|
||||
int len;
|
||||
} saCmdQueue_t;
|
||||
|
||||
#define SA_QSIZE 4 // 2 should be enough
|
||||
static saCmdQueue_t sa_queue[SA_QSIZE];
|
||||
static uint8_t sa_qhead = 0;
|
||||
static uint8_t sa_qtail = 0;
|
||||
|
||||
#ifdef DPRINTF_SMARTAUDIO
|
||||
static int saQueueLength()
|
||||
{
|
||||
if (sa_qhead >= sa_qtail) {
|
||||
return sa_qhead - sa_qtail;
|
||||
} else {
|
||||
return SA_QSIZE + sa_qhead - sa_qtail;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static bool saQueueEmpty()
|
||||
{
|
||||
return sa_qhead == sa_qtail;
|
||||
}
|
||||
|
||||
static bool saQueueFull()
|
||||
{
|
||||
return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail;
|
||||
}
|
||||
|
||||
static void saQueueCmd(uint8_t *buf, int len)
|
||||
{
|
||||
if (saQueueFull())
|
||||
return;
|
||||
|
||||
sa_queue[sa_qhead].buf = buf;
|
||||
sa_queue[sa_qhead].len = len;
|
||||
sa_qhead = (sa_qhead + 1) % SA_QSIZE;
|
||||
}
|
||||
|
||||
static void saSendQueue(void)
|
||||
{
|
||||
if (saQueueEmpty())
|
||||
return;
|
||||
|
||||
saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
|
||||
sa_qtail = (sa_qtail + 1) % SA_QSIZE;
|
||||
}
|
||||
|
||||
// Individual commands
|
||||
|
||||
vtxPowerTable_t saPowerTableV1[] = {
|
||||
{ " 25", 25, 7 },
|
||||
{ "200", 200, 16 },
|
||||
{ "500", 500, 25 },
|
||||
{ "800", 800, 40 },
|
||||
{ NULL }
|
||||
};
|
||||
|
||||
vtxPowerTable_t saPowerTableV2[] = {
|
||||
{ "PIT", 0, -1 },
|
||||
{ " 25", 25, 0 },
|
||||
{ "200", 200, 1 },
|
||||
{ "500", 500, 2 },
|
||||
{ "800", 800, 3 },
|
||||
{ NULL }
|
||||
};
|
||||
|
||||
void smartAudioGetSettings(void)
|
||||
{
|
||||
static uint8_t bufGetSettings[5] = {0xAA, 0x55, 0x03, 0x00, 0x9F};
|
||||
saQueueCmd(bufGetSettings, 5);
|
||||
}
|
||||
|
||||
void smartAudioSetFreq(uint16_t freq)
|
||||
{
|
||||
static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
|
||||
|
||||
dprintf(("smartAudioSetFreq: freq %d\r\n", freq));
|
||||
|
||||
buf[4] = (freq >> 8) & 0xff;
|
||||
buf[5] = freq & 0xff;
|
||||
buf[6] = CRC8(buf, 6);
|
||||
|
||||
saQueueCmd(buf, 7);
|
||||
|
||||
sa_freq = 0; // Will be read by a following heartbeat
|
||||
}
|
||||
|
||||
#ifdef SMARTAUDIO_EXTENDED_API
|
||||
void smartAudioSetFreqSetPit(uint16_t freq)
|
||||
{
|
||||
smartAudioSetFreq(freq | SA_FREQ_SETPIT);
|
||||
|
||||
sa_pitfreq = 0; // Will be read by a following heartbeat
|
||||
}
|
||||
|
||||
void smartAudioSetFreqGetPit(void)
|
||||
{
|
||||
smartAudioSetFreq(SA_FREQ_GETPIT);
|
||||
|
||||
sa_pitfreqpending = true;
|
||||
}
|
||||
|
||||
uint16_t smartAudioGetFreq(void)
|
||||
{
|
||||
return sa_freq;
|
||||
}
|
||||
|
||||
uint16_t smartAudioGetPitFreq(void)
|
||||
{
|
||||
return sa_pitfreq;
|
||||
}
|
||||
#endif
|
||||
|
||||
void smartAudioSetBandChan(int band, int chan)
|
||||
{
|
||||
static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
|
||||
|
||||
buf[4] = band * 8 + chan;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
|
||||
saQueueCmd(buf, 6);
|
||||
}
|
||||
|
||||
void smartAudioSetPowerByIndex(uint8_t index)
|
||||
{
|
||||
static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
|
||||
|
||||
dprintf(("smartAudioSetPowerByIndex: index %d\r\n", index));
|
||||
|
||||
if (sa_vers != 1 && sa_vers != 2) {
|
||||
// Unknown or yet unknown version.
|
||||
return;
|
||||
}
|
||||
|
||||
if (index > 4)
|
||||
return;
|
||||
|
||||
if (sa_vers == 1) {
|
||||
dprintf(("smartAudioSetPowerByIndex: V1 value %d\r\n",
|
||||
saPowerTableV1[index].value));
|
||||
buf[4] = saPowerTableV1[index].value;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
saQueueCmd(buf, 6);
|
||||
} else {
|
||||
index++; // XXX Skip pit mode for v3.0.0 BFOSD API
|
||||
|
||||
if (index > 0) {
|
||||
if (sa_opmode & SA_MODE_GET_PITMODE) {
|
||||
// Currently in pit mode; have to deactivate and set power.
|
||||
// XXX Have to implement chained request...
|
||||
} else {
|
||||
dprintf(("smartAudioSetPowerByIndex: V2 value %d\r\n",
|
||||
saPowerTableV2[index].value));
|
||||
buf[4] = saPowerTableV2[index].value;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
saQueueCmd(buf, 6);
|
||||
}
|
||||
} else {
|
||||
// Pit mode requested.
|
||||
// Not implemented yet.
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef SMARTAUDIO_EXTENDED_API
|
||||
void smartAudioSetMode(int mode)
|
||||
{
|
||||
static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
|
||||
|
||||
buf[4] = mode & 0x0f;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
|
||||
saQueueCmd(buf, 6);
|
||||
}
|
||||
#endif
|
||||
|
||||
void smartAudioInit(void)
|
||||
{
|
||||
portOptions_t portOptions;
|
||||
|
||||
#ifdef SMARTPORT_DPRINF
|
||||
// Setup debugSerialPort
|
||||
|
||||
debugSerialPort = openSerialPort(SERIAL_PORT_USART3, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
|
||||
if (!debugSerialPort)
|
||||
return;
|
||||
setPrintfSerialPort(debugSerialPort);
|
||||
dprintf(("smartAudioInit: OK\r\n"));
|
||||
#endif
|
||||
|
||||
portOptions = SERIAL_BIDIR|SERIAL_BIDIR_PP;
|
||||
|
||||
// XXX Fixed at UART2 fow the first cut
|
||||
smartAudioSerialPort = openSerialPort(SERIAL_PORT_USART2, FUNCTION_NONE, NULL, 4800, MODE_RXTX, portOptions);
|
||||
|
||||
if (!smartAudioSerialPort) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef SMARTAUDIO_EXTENDED_API
|
||||
bool smartAudioIsReady(void)
|
||||
{
|
||||
return (sa_vers != 0);
|
||||
}
|
||||
|
||||
int smartAudioGetPowerTable(int *pTableSize, vtxPowerTable_t **pTable)
|
||||
{
|
||||
switch (sa_vers) {
|
||||
case 0:
|
||||
return -1;
|
||||
|
||||
case 1:
|
||||
if (pTableSize)
|
||||
*pTableSize = 4;
|
||||
|
||||
if (pTable)
|
||||
*pTable = saPowerTableV1;
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (pTableSize)
|
||||
*pTableSize = 5;
|
||||
|
||||
if (pTable)
|
||||
*pTable = saPowerTableV2;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SMARTAUDIO_PITMODE_DEBUG
|
||||
void smartAudioPitMode(void)
|
||||
{
|
||||
static int turn = 0;
|
||||
|
||||
if ((turn++ % 2) == 0) {
|
||||
smartAudioSetMode(SA_MODE_SET_UNLOCK|SA_MODE_SET_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
|
||||
} else {
|
||||
smartAudioSetMode(SA_MODE_SET_UNLOCK|SA_MODE_CLR_PITMODE);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#define SCHED_PERIOD_DISARMED (200*1000) // 200msec
|
||||
#define SCHED_PERIOD_ARMED (1000*1000) // 1sec, really want to make it none.
|
||||
|
||||
void smartAudioProcess()
|
||||
{
|
||||
static bool previousArmedState = false;
|
||||
|
||||
if (smartAudioSerialPort == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Try reducing scheduling frequency (overkill?),
|
||||
// and conserve as much cycles as we can.
|
||||
|
||||
bool armedState = ARMING_FLAG(ARMED) ? true : false;
|
||||
bool armedStateChanged = armedState != previousArmedState;
|
||||
previousArmedState = armedState;
|
||||
|
||||
if (armedStateChanged) {
|
||||
if (armedState) {
|
||||
// Reduce scheduling frequency
|
||||
|
||||
rescheduleTask(TASK_VTX_CONTROL, SCHED_PERIOD_ARMED);
|
||||
} else {
|
||||
// Restore scheduling frequency
|
||||
|
||||
rescheduleTask(TASK_VTX_CONTROL, SCHED_PERIOD_DISARMED);
|
||||
|
||||
// Cleanup possible garbage; last command-response cycle
|
||||
// may have been terminated by arming.
|
||||
|
||||
while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
|
||||
(void)serialRead(smartAudioSerialPort);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (armedState) {
|
||||
dprintf(("smartAudioProcess: silence\r\n"));
|
||||
// We keep silence while armed.
|
||||
return;
|
||||
}
|
||||
|
||||
// Disarmed, full processing
|
||||
|
||||
while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
|
||||
uint8_t c = serialRead(smartAudioSerialPort);
|
||||
saReceiveFramer((uint16_t)c);
|
||||
}
|
||||
|
||||
// Evaluate baudrate after each frame reception
|
||||
|
||||
saAutobaud();
|
||||
|
||||
uint32_t now = millis();
|
||||
|
||||
// If we haven't talked to the device, keep trying.
|
||||
|
||||
if (sa_vers == 0) {
|
||||
smartAudioGetSettings();
|
||||
saSendQueue();
|
||||
return;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
if ((sa_outstanding != SA_CMD_NONE)
|
||||
&& (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) {
|
||||
// Last command timed out
|
||||
saResendCmd();
|
||||
} else if (!saQueueEmpty()) {
|
||||
// Command pending. Send it.
|
||||
saSendQueue();
|
||||
} else if (now - sa_lastTransmission >= 1000) {
|
||||
// Heart beat for autobauding
|
||||
|
||||
#ifdef SMARTAUDIO_PITMODE_DEBUG
|
||||
static int turn = 0;
|
||||
if ((turn++ % 2) == 0) {
|
||||
smartAudioGetSettings();
|
||||
} else {
|
||||
smartAudioPitMode();
|
||||
}
|
||||
#else
|
||||
smartAudioGetSettings();
|
||||
#endif
|
||||
saSendQueue();
|
||||
}
|
||||
}
|
||||
|
||||
// Things to make it work for the first cut on v3.0.0
|
||||
|
||||
// This doesn't belong here, really.
|
||||
uint16_t current_vtx_channel;
|
||||
|
||||
// A table that's repeated over and over in every vtx code.
|
||||
const uint16_t vtx_freq[] =
|
||||
{
|
||||
5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // Boacam A
|
||||
5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // Boscam B
|
||||
5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // Boscam E
|
||||
5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // FatShark
|
||||
5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // RaceBand
|
||||
};
|
||||
|
||||
#endif
|
||||
14
src/main/drivers/vtx_smartaudio.h
Normal file
14
src/main/drivers/vtx_smartaudio.h
Normal file
|
|
@ -0,0 +1,14 @@
|
|||
|
||||
// For generic API use, but here for now
|
||||
|
||||
typedef struct vtxPowerTable_s {
|
||||
char *name;
|
||||
int16_t power;
|
||||
int16_t value;
|
||||
} vtxPowerTable_t;
|
||||
|
||||
void smartAudioInit(void);
|
||||
void smartAudioProcess(void);
|
||||
void smartAudioSetPowerByIndex(uint8_t);
|
||||
void smartAudioSetFreq(uint16_t);
|
||||
void smartAudioSetBandChan(int, int);
|
||||
|
|
@ -41,6 +41,7 @@
|
|||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/vtx_smartaudio.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
|
|
@ -1064,3 +1065,13 @@ void taskUpdateOsd(uint32_t currentTime)
|
|||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef VTX_CONTROL
|
||||
// Everything that listens to VTX devices
|
||||
void taskVtxControl(void)
|
||||
{
|
||||
#ifdef VTX_SMARTAUDIO
|
||||
smartAudioProcess();
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -132,9 +132,9 @@ uint8_t armState;
|
|||
uint8_t featureBlackbox = 0;
|
||||
uint8_t featureLedstrip = 0;
|
||||
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
uint8_t featureVtx = 0, vtxBand, vtxChannel;
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
|
||||
// We are in menu flag
|
||||
bool inMenu = false;
|
||||
|
|
@ -230,9 +230,9 @@ OSD_Entry menuBlackbox[];
|
|||
#ifdef LED_STRIP
|
||||
OSD_Entry menuLedstrip[];
|
||||
#endif // LED_STRIP
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
OSD_Entry menu_vtx[];
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
OSD_Entry menuImu[];
|
||||
OSD_Entry menuPid[];
|
||||
OSD_Entry menuRc[];
|
||||
|
|
@ -258,9 +258,9 @@ OSD_Entry menuFeatures[] =
|
|||
#ifdef LED_STRIP
|
||||
{"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]},
|
||||
#endif // LED_STRIP
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
{"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]},
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
{"BACK", OME_Back, NULL, NULL},
|
||||
{NULL, OME_END, NULL, NULL}
|
||||
};
|
||||
|
|
@ -339,7 +339,7 @@ OSD_Entry menuLedstrip[] =
|
|||
};
|
||||
#endif // LED_STRIP
|
||||
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
static const char * const vtxBandNames[] = {
|
||||
"BOSCAM A",
|
||||
"BOSCAM B",
|
||||
|
|
@ -366,13 +366,13 @@ OSD_Entry menu_vtx[] =
|
|||
#endif // VTX
|
||||
{"BAND", OME_TAB, NULL, &entryVtxBand},
|
||||
{"CHANNEL", OME_UINT8, NULL, &entryVtxChannel},
|
||||
#ifdef USE_RTC6705
|
||||
#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
{"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power},
|
||||
#endif // USE_RTC6705
|
||||
#endif // USE_RTC6705 || VTX_SMARTAUDIO
|
||||
{"BACK", OME_Back, NULL, NULL},
|
||||
{NULL, OME_END, NULL, NULL}
|
||||
};
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
|
||||
OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10};
|
||||
OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1};
|
||||
|
|
@ -1365,21 +1365,21 @@ void osdExitMenu(void *ptr)
|
|||
featureSet(FEATURE_LED_STRIP);
|
||||
else
|
||||
featureClear(FEATURE_LED_STRIP);
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
if (featureVtx)
|
||||
featureSet(FEATURE_VTX);
|
||||
else
|
||||
featureClear(FEATURE_VTX);
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
|
||||
#ifdef VTX
|
||||
masterConfig.vtxBand = vtxBand;
|
||||
masterConfig.vtx_channel = vtxChannel - 1;
|
||||
#endif // VTX
|
||||
|
||||
#ifdef USE_RTC6705
|
||||
#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1;
|
||||
#endif // USE_RTC6705
|
||||
#endif // USE_RTC6705 || VTX_SMARTAUDIO
|
||||
|
||||
saveConfigAndNotify();
|
||||
}
|
||||
|
|
@ -1398,17 +1398,17 @@ void osdOpenMenu(void)
|
|||
if (feature(FEATURE_BLACKBOX))
|
||||
featureBlackbox = 1;
|
||||
|
||||
#if defined(VTX) || defined(USE_RTC6705)
|
||||
#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
if (feature(FEATURE_VTX))
|
||||
featureVtx = 1;
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO
|
||||
|
||||
#ifdef VTX
|
||||
vtxBand = masterConfig.vtxBand;
|
||||
vtxChannel = masterConfig.vtx_channel + 1;
|
||||
#endif // VTX
|
||||
|
||||
#ifdef USE_RTC6705
|
||||
#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
vtxBand = masterConfig.vtx_channel / 8;
|
||||
vtxChannel = masterConfig.vtx_channel % 8 + 1;
|
||||
#endif // USE_RTC6705
|
||||
|
|
|
|||
|
|
@ -261,6 +261,8 @@ bool doesConfigurationUsePort(serialPortIdentifier_e identifier)
|
|||
return candidate != NULL && candidate->functionMask;
|
||||
}
|
||||
|
||||
#include "debug.h"
|
||||
|
||||
serialPort_t *openSerialPort(
|
||||
serialPortIdentifier_e identifier,
|
||||
serialPortFunction_e function,
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ typedef enum {
|
|||
FUNCTION_BLACKBOX = (1 << 7), // 128
|
||||
FUNCTION_PASSTHROUGH = (1 << 8), // 256
|
||||
FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512
|
||||
FUNCTION_VTX_CONTROL = (1 << 10),// 1024
|
||||
} serialPortFunction_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -933,7 +933,7 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef LED_STRIP
|
||||
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
|
||||
#endif
|
||||
#ifdef USE_RTC6705
|
||||
#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO)
|
||||
{ "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } },
|
||||
{ "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } },
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -57,6 +57,7 @@
|
|||
#include "drivers/io.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/vtx_soft_spi_rtc6705.h"
|
||||
#include "drivers/vtx_smartaudio.h"
|
||||
|
||||
#ifdef USE_BST
|
||||
#include "bus_bst.h"
|
||||
|
|
@ -618,6 +619,10 @@ void init(void)
|
|||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||
#endif
|
||||
|
||||
#ifdef VTX_SMARTAUDIO
|
||||
smartAudioInit();
|
||||
#endif
|
||||
|
||||
// start all timers
|
||||
// TODO - not implemented yet
|
||||
timerStart();
|
||||
|
|
@ -735,6 +740,9 @@ void main_init(void)
|
|||
#ifdef USE_BST
|
||||
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
|
||||
#endif
|
||||
#ifdef VTX_CONTROL
|
||||
setTaskEnabled(TASK_VTX_CONTROL, true);
|
||||
#endif
|
||||
}
|
||||
|
||||
void main_step(void)
|
||||
|
|
|
|||
|
|
@ -85,6 +85,9 @@ typedef enum {
|
|||
#ifdef USE_BST
|
||||
TASK_BST_MASTER_PROCESS,
|
||||
#endif
|
||||
#ifdef VTX_CONTROL
|
||||
TASK_VTX_CONTROL,
|
||||
#endif
|
||||
|
||||
/* Count of real tasks */
|
||||
TASK_COUNT,
|
||||
|
|
|
|||
|
|
@ -181,4 +181,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
.staticPriority = TASK_PRIORITY_IDLE,
|
||||
},
|
||||
#endif
|
||||
|
||||
#ifdef VTX_CONTROL
|
||||
[TASK_VTX_CONTROL] = {
|
||||
.taskName = "VTX",
|
||||
.taskFunc = taskVtxControl,
|
||||
.desiredPeriod = 1000000 / 5, // 5 Hz, 200ms: TX 12B=25ms@4800 , SmartAudio response in 60ms, RX 12B=25ms@4800
|
||||
.staticPriority = TASK_PRIORITY_IDLE,
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
|
|
|||
|
|
@ -42,4 +42,6 @@ void taskUpdateOsd(uint32_t currentTime);
|
|||
void taskBstReadWrite(uint32_t currentTime);
|
||||
void taskBstMasterProcess(uint32_t currentTime);
|
||||
#endif
|
||||
|
||||
#ifdef VTX_CONTROL
|
||||
void taskVtxControl(void);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -103,6 +103,11 @@
|
|||
//#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2
|
||||
//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER
|
||||
|
||||
// VTX monitor task
|
||||
#define VTX_CONTROL
|
||||
// VTX device type
|
||||
#define VTX_SMARTAUDIO
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
|
||||
|
||||
|
|
@ -176,7 +181,7 @@
|
|||
#define BUTTON_B_PORT GPIOB
|
||||
#define BUTTON_B_PIN Pin_0
|
||||
|
||||
#define AVOID_UART3_FOR_PWM_PPM
|
||||
//#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3,
|
||||
|
|
|
|||
|
|
@ -13,4 +13,5 @@ TARGET_SRC = \
|
|||
drivers/transponder_ir_stm32f30x.c \
|
||||
io/transponder_ir.c \
|
||||
drivers/max7456.c \
|
||||
io/osd.c
|
||||
io/osd.c \
|
||||
drivers/vtx_smartaudio.c
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue