From 8334cd35ff5f0d0347482cef67da47d83e3ed30d Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 8 Oct 2016 23:23:45 +0900 Subject: [PATCH 01/40] SmartAudio Support --- src/main/config/config_master.h | 2 +- src/main/drivers/vtx_smartaudio.c | 780 +++++++++++++++++++++++++++ src/main/drivers/vtx_smartaudio.h | 14 + src/main/fc/mw.c | 11 + src/main/io/osd.c | 34 +- src/main/io/serial.c | 2 + src/main/io/serial.h | 1 + src/main/io/serial_cli.c | 2 +- src/main/main.c | 8 + src/main/scheduler/scheduler.h | 3 + src/main/scheduler/scheduler_tasks.c | 9 + src/main/scheduler/scheduler_tasks.h | 4 +- src/main/target/OMNIBUS/target.h | 7 +- src/main/target/OMNIBUS/target.mk | 3 +- 14 files changed, 858 insertions(+), 22 deletions(-) create mode 100644 src/main/drivers/vtx_smartaudio.c create mode 100644 src/main/drivers/vtx_smartaudio.h diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 87a527e417..a7154a60e8 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/drivers/vtx_smartaudio.c new file mode 100644 index 0000000000..8f539bc038 --- /dev/null +++ b/src/main/drivers/vtx_smartaudio.c @@ -0,0 +1,780 @@ +#include +#include +#include +#include + +#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 diff --git a/src/main/drivers/vtx_smartaudio.h b/src/main/drivers/vtx_smartaudio.h new file mode 100644 index 0000000000..25ecd13543 --- /dev/null +++ b/src/main/drivers/vtx_smartaudio.h @@ -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); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index d8d0398b58..e335f75511 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 065397f828..ce0215b6df 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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 diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 99f7cfcf3f..558b951a54 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -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, diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 0448d3e592..8f0b1f7670 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -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 { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ed27fa4723..3a0f4973e5 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 diff --git a/src/main/main.c b/src/main/main.c index 9ae7801192..e103b13c45 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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) diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f44cd497f9..f90891c4da 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -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, diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 30920724a8..71db38afbe 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -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 }; diff --git a/src/main/scheduler/scheduler_tasks.h b/src/main/scheduler/scheduler_tasks.h index 38fb9d8c7b..0f3f1e82c3 100644 --- a/src/main/scheduler/scheduler_tasks.h +++ b/src/main/scheduler/scheduler_tasks.h @@ -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 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 86c9d1699b..36646ae090 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -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, diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 82cde62266..a398f12dcd 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -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 From f5dd171c8c7931d2b9301a8b17d53cff096131f4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 5 Oct 2016 01:40:36 +0900 Subject: [PATCH 02/40] SERIAL_BIDIR_PP def in drivers/serial.h --- src/main/drivers/serial.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index bb42cf709e..104c1c1ae4 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -31,7 +31,9 @@ typedef enum portOptions_t { SERIAL_PARITY_NO = 0 << 2, SERIAL_PARITY_EVEN = 1 << 2, SERIAL_UNIDIR = 0 << 3, - SERIAL_BIDIR = 1 << 3 + SERIAL_BIDIR = 1 << 3, + SERIAL_BIDIR_OD = 0 << 4, + SERIAL_BIDIR_PP = 1 << 4 } portOptions_t; typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app From c724b7c4ee00d9ed871a317574c15562e3180a69 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 5 Oct 2016 01:11:10 +0900 Subject: [PATCH 03/40] SERIAL_BIDIR_PP option MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Required for SmartAudio Support with single wire connection and without external pull-up. Tested with F3 only. F1 and F4 has untested mods, and Softserial haven’t been touched. --- src/main/drivers/serial_uart_stm32f10x.c | 15 ++++++++++++--- src/main/drivers/serial_uart_stm32f30x.c | 6 ++++-- src/main/drivers/serial_uart_stm32f4xx.c | 5 ++++- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 4774ab33e6..b4f467cafd 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -124,7 +124,10 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option // UART1_RX PA10 if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TXRX, 1); - IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_OD); + if (options & SERIAL_BIDIR_PP) + IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_PP); + else + IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TX, 1); @@ -195,7 +198,10 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option // UART2_RX PA3 if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TXRX, 2); - IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_OD); + if (options & SERIAL_BIDIR_PP) + IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_PP); + else + IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TX, 2); @@ -257,7 +263,10 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TXRX, 3); - IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_OD); + if (options & SERIAL_BIDIR_PP) + IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_PP); + else + IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TX, 3); diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 29bebfb954..e573debbfd 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -115,8 +115,10 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui { if (options & SERIAL_BIDIR) { ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, - (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD, - (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) + ? GPIO_OType_PP : GPIO_OType_OD, + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) + ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, index); diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index d6bd58024e..1aedae381e 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -336,7 +336,10 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po if (options & SERIAL_BIDIR) { IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(tx, IOCFG_AF_OD, uart->af); + if (options & SERIAL_BIDIR_PP) + IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + else + IOConfigGPIOAF(tx, IOCFG_AF_OD, uart->af); } else { if (mode & MODE_TX) { From d26a1873d001e4abf4ffc3fb51e758d9047d0fd0 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 8 Oct 2016 15:00:07 +0900 Subject: [PATCH 04/40] Removed rescheduling code and superfluous includes --- src/main/drivers/serial_uart_stm32f10x.c | 15 +--- src/main/drivers/serial_uart_stm32f30x.c | 6 +- src/main/drivers/vtx_smartaudio.c | 97 ++---------------------- 3 files changed, 13 insertions(+), 105 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index b4f467cafd..81eb763469 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -124,10 +124,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option // UART1_RX PA10 if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TXRX, 1); - if (options & SERIAL_BIDIR_PP) - IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_PP); - else - IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_OD); + IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TX, 1); @@ -198,10 +195,7 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option // UART2_RX PA3 if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TXRX, 2); - if (options & SERIAL_BIDIR_PP) - IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_PP); - else - IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), IOCFG_AF_OD); + IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TX, 2); @@ -263,10 +257,7 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TXRX, 3); - if (options & SERIAL_BIDIR_PP) - IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_PP); - else - IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOCFG_AF_OD); + IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TX, 3); diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index e573debbfd..3ae94f383b 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -115,10 +115,8 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui { if (options & SERIAL_BIDIR) { ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, - ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) - ? GPIO_OType_PP : GPIO_OType_OD, - ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) - ? GPIO_PuPd_DOWN : GPIO_PuPd_UP + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_OType_PP : GPIO_OType_OD, + ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, index); diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/drivers/vtx_smartaudio.c index 8f539bc038..ab62dea83d 100644 --- a/src/main/drivers/vtx_smartaudio.c +++ b/src/main/drivers/vtx_smartaudio.c @@ -1,18 +1,10 @@ #include #include -#include -#include #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" @@ -24,42 +16,7 @@ #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" @@ -72,17 +29,13 @@ #ifdef SMARTAUDIO_DPRINTF #include "common/printf.h" +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 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 @@ -118,6 +71,7 @@ enum { #define SA_FREQ_SETPIT (1 << 15) // Error counters, may be good for post production debugging. +static uint16_t saerr_badpre = 0; static uint16_t saerr_badlen = 0; static uint16_t saerr_crc = 0; static uint16_t saerr_oooresp = 0; @@ -331,7 +285,7 @@ static void saReceiveFramer(uint8_t c) if (c == 0xAA) { state = S_WAITPRE2; } else { - state = S_WAITPRE1; // Bogus + state = S_WAITPRE1; // Don't need this } break; @@ -339,6 +293,7 @@ static void saReceiveFramer(uint8_t c) if (c == 0x55) { state = S_WAITRESP; } else { + saerr_badpre++; state = S_WAITPRE1; } break; @@ -608,7 +563,7 @@ void smartAudioInit(void) #ifdef SMARTPORT_DPRINF // Setup debugSerialPort - debugSerialPort = openSerialPort(SERIAL_PORT_USART3, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); + debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); if (!debugSerialPort) return; setPrintfSerialPort(debugSerialPort); @@ -671,51 +626,15 @@ void smartAudioPitMode(void) } #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. + if (armedState) return; - } - // Disarmed, full processing + if (smartAudioSerialPort == NULL) + return; while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); From a04d67c2627c4d21df9d9ff0461de40f3f058602 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 8 Oct 2016 23:09:05 +0900 Subject: [PATCH 05/40] Rebasing onto development --- src/main/drivers/vtx_smartaudio.c | 47 ++++++++++++++++------------ src/main/drivers/vtx_smartaudio.h | 2 +- src/main/fc/mw.c | 4 +-- src/main/io/serial.c | 2 -- src/main/scheduler/scheduler_tasks.h | 2 +- 5 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/drivers/vtx_smartaudio.c index ab62dea83d..11552065bc 100644 --- a/src/main/drivers/vtx_smartaudio.c +++ b/src/main/drivers/vtx_smartaudio.c @@ -1,28 +1,18 @@ #include #include +#include #include "platform.h" #ifdef VTX_SMARTAUDIO #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 "io/serial.h" -#include "config/runtime_config.h" - #include "drivers/vtx_smartaudio.h" +#include "io/serial.h" +#include "fc/runtime_config.h" +#include "config/config_master.h" -//#include "build/debug.h" -#include "debug.h" //#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR @@ -36,6 +26,10 @@ serialPort_t *debugSerialPort = NULL; #define dprintf(x) #endif +#ifdef SMARTAUDIO_DEBUG_MONITOR +#include "build/debug.h" +#endif + static serialPort_t *smartAudioSerialPort = NULL; // SmartAudio command and response codes @@ -115,6 +109,13 @@ static uint16_t sa_freq = 0; static uint16_t sa_pitfreq = 0; static bool sa_pitfreqpending = false; +// A measure for osd.c that resets on exit: +// masterConfig.{vtx_channel,vtx_power} can not be set at boot time, +// but after a communication with the smartaudio device is established. +// We remember here if channel and power is in sync with masterConfig. + +static bool sa_configSynced = false; + static void smartAudioPrintSettings(void) { #ifdef SMARTAUDIO_DPRINTF @@ -219,7 +220,7 @@ static void saProcessResponse(uint8_t *buf, int len) sa_outstanding = SA_CMD_NONE; } else { saerr_oooresp++; - dprintf(("processResponse: outstanding %d got %d\r\n", outstanding, resp)); + dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); } switch(resp) { @@ -560,12 +561,13 @@ void smartAudioInit(void) { portOptions_t portOptions; -#ifdef SMARTPORT_DPRINF +#ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); - if (!debugSerialPort) + if (!debugSerialPort) { return; + } setPrintfSerialPort(debugSerialPort); dprintf(("smartAudioInit: OK\r\n")); #endif @@ -626,7 +628,7 @@ void smartAudioPitMode(void) } #endif -void smartAudioProcess() +void smartAudioProcess(uint32_t now) { bool armedState = ARMING_FLAG(ARMED) ? true : false; @@ -645,14 +647,19 @@ void smartAudioProcess() saAutobaud(); - uint32_t now = millis(); - // If we haven't talked to the device, keep trying. if (sa_vers == 0) { smartAudioGetSettings(); saSendQueue(); return; + } else if (!sa_configSynced) { + // XXX Should take care of pit mode on boot case. + // Note that vtx_power = 1 means LOW POWER (25mW). + smartAudioSetPowerByIndex(masterConfig.vtx_power ? 0 : 1); + smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); + saSendQueue(); + sa_configSynced = true; } // diff --git a/src/main/drivers/vtx_smartaudio.h b/src/main/drivers/vtx_smartaudio.h index 25ecd13543..4ba54f291a 100644 --- a/src/main/drivers/vtx_smartaudio.h +++ b/src/main/drivers/vtx_smartaudio.h @@ -8,7 +8,7 @@ typedef struct vtxPowerTable_s { } vtxPowerTable_t; void smartAudioInit(void); -void smartAudioProcess(void); +void smartAudioProcess(uint32_t); void smartAudioSetPowerByIndex(uint8_t); void smartAudioSetFreq(uint16_t); void smartAudioSetBandChan(int, int); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index e335f75511..311c4e222a 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -1068,10 +1068,10 @@ void taskUpdateOsd(uint32_t currentTime) #ifdef VTX_CONTROL // Everything that listens to VTX devices -void taskVtxControl(void) +void taskVtxControl(uint32_t currentTime) { #ifdef VTX_SMARTAUDIO - smartAudioProcess(); + smartAudioProcess(currentTime); #endif } #endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 558b951a54..99f7cfcf3f 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -261,8 +261,6 @@ bool doesConfigurationUsePort(serialPortIdentifier_e identifier) return candidate != NULL && candidate->functionMask; } -#include "debug.h" - serialPort_t *openSerialPort( serialPortIdentifier_e identifier, serialPortFunction_e function, diff --git a/src/main/scheduler/scheduler_tasks.h b/src/main/scheduler/scheduler_tasks.h index 0f3f1e82c3..c052db61e4 100644 --- a/src/main/scheduler/scheduler_tasks.h +++ b/src/main/scheduler/scheduler_tasks.h @@ -43,5 +43,5 @@ void taskBstReadWrite(uint32_t currentTime); void taskBstMasterProcess(uint32_t currentTime); #endif #ifdef VTX_CONTROL -void taskVtxControl(void); +void taskVtxControl(uint32_t currentTime); #endif From 3a58bbb5553a24ecafacf0e2d3bf229a68ff252c Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 9 Oct 2016 00:37:03 +0900 Subject: [PATCH 06/40] Make UART configurable --- src/main/drivers/vtx_smartaudio.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/drivers/vtx_smartaudio.c index 11552065bc..c075e4cf40 100644 --- a/src/main/drivers/vtx_smartaudio.c +++ b/src/main/drivers/vtx_smartaudio.c @@ -26,9 +26,7 @@ serialPort_t *debugSerialPort = NULL; #define dprintf(x) #endif -#ifdef SMARTAUDIO_DEBUG_MONITOR #include "build/debug.h" -#endif static serialPort_t *smartAudioSerialPort = NULL; @@ -559,8 +557,6 @@ void smartAudioSetMode(int mode) void smartAudioInit(void) { - portOptions_t portOptions; - #ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort @@ -572,10 +568,14 @@ void smartAudioInit(void) dprintf(("smartAudioInit: OK\r\n")); #endif - portOptions = SERIAL_BIDIR|SERIAL_BIDIR_PP; + // Get an UART port assigned to SmartAudio. - // XXX Fixed at UART2 fow the first cut - smartAudioSerialPort = openSerialPort(SERIAL_PORT_USART2, FUNCTION_NONE, NULL, 4800, MODE_RXTX, portOptions); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); + + if (!portConfig) + return; + + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); if (!smartAudioSerialPort) { return; From a3f9c3dde7a9b1203f3e2508685e3e11209464c1 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 9 Oct 2016 14:28:29 +0900 Subject: [PATCH 07/40] Make osd.c aware of SmartAudio power table --- src/main/drivers/vtx_smartaudio.c | 26 ++++++++++++++++---------- src/main/io/osd.c | 20 ++++++++++++++++++-- 2 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/drivers/vtx_smartaudio.c index c075e4cf40..02c47ab112 100644 --- a/src/main/drivers/vtx_smartaudio.c +++ b/src/main/drivers/vtx_smartaudio.c @@ -13,8 +13,9 @@ #include "fc/runtime_config.h" #include "config/config_master.h" +#define SMARTAUDIO_EXTENDED_API -//#define SMARTAUDIO_DPRINTF +#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR #ifdef SMARTAUDIO_DPRINTF @@ -437,6 +438,7 @@ vtxPowerTable_t saPowerTableV1[] = { }; vtxPowerTable_t saPowerTableV2[] = { + { "OFF", 0, -2 }, { "PIT", 0, -1 }, { " 25", 25, 0 }, { "200", 200, 1 }, @@ -513,7 +515,7 @@ void smartAudioSetPowerByIndex(uint8_t index) return; } - if (index > 4) + if (index > 5) return; if (sa_vers == 1) { @@ -523,21 +525,26 @@ void smartAudioSetPowerByIndex(uint8_t index) buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } else { - index++; // XXX Skip pit mode for v3.0.0 BFOSD API + int pwrval = saPowerTableV2[index].value; - if (index > 0) { + dprintf(("smartAudioSetPowerByIndex: pwrval %d\n", pwrval)); + + if (pwrval >= 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); + smartAudioSetMode(0x00); // Reset power off } - } else { - // Pit mode requested. + } else if (pwrval == -1) { + // Pit mode + // Not implemented yet. + } else if (pwrval == -2) { + // Power off // Not implemented yet. } } @@ -548,7 +555,7 @@ void smartAudioSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; - buf[4] = mode & 0x0f; + buf[4] = mode & 0x1f; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); @@ -655,8 +662,7 @@ void smartAudioProcess(uint32_t now) return; } else if (!sa_configSynced) { // XXX Should take care of pit mode on boot case. - // Note that vtx_power = 1 means LOW POWER (25mW). - smartAudioSetPowerByIndex(masterConfig.vtx_power ? 0 : 1); + smartAudioSetPowerByIndex(masterConfig.vtx_power); smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); saSendQueue(); sa_configSynced = true; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce0215b6df..d6a6f95f07 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -356,6 +356,19 @@ OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; #endif // VTX +#ifdef VTX_SMARTAUDIO +static const char * const vtxSmartAudioPower[] = { + "OFF", + "PIT", + "25", + "200", + "500", + "800", +}; + +OSD_TAB_t entrySmartAudioPower = { &masterConfig.vtx_power, 5, &vtxSmartAudioPower[0] }; +#endif + OSD_Entry menu_vtx[] = { {"--- VTX ---", OME_Label, NULL, NULL}, @@ -366,9 +379,12 @@ OSD_Entry menu_vtx[] = #endif // VTX {"BAND", OME_TAB, NULL, &entryVtxBand}, {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, -#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +#ifdef USE_RTC6705 {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, -#endif // USE_RTC6705 || VTX_SMARTAUDIO +#endif // USE_RTC6705 +#ifdef VTX_SMARTAUDIO + {"POWER", OME_TAB, NULL, &entrySmartAudioPower}, +#endif {"BACK", OME_Back, NULL, NULL}, {NULL, OME_END, NULL, NULL} }; From 8c19d9f79e58a42617494b465b0ce6a9e1568714 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 10 Oct 2016 21:29:38 +0900 Subject: [PATCH 08/40] Move everything to io --- src/main/fc/mw.c | 5 ++- src/main/{drivers => io}/vtx_smartaudio.c | 55 ++++++++++------------- src/main/{drivers => io}/vtx_smartaudio.h | 2 +- src/main/main.c | 7 ++- src/main/target/OMNIBUS/target.mk | 2 +- 5 files changed, 35 insertions(+), 36 deletions(-) rename src/main/{drivers => io}/vtx_smartaudio.c (96%) rename src/main/{drivers => io}/vtx_smartaudio.h (91%) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 311c4e222a..5f997ee061 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -41,7 +41,6 @@ #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" @@ -68,6 +67,7 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/transponder_ir.h" #include "io/osd.h" +#include "io/vtx_smartaudio.h" #include "io/vtx.h" @@ -1070,6 +1070,9 @@ void taskUpdateOsd(uint32_t currentTime) // Everything that listens to VTX devices void taskVtxControl(uint32_t currentTime) { + if (ARMING_FLAG(ARMED)) + return; + #ifdef VTX_SMARTAUDIO smartAudioProcess(currentTime); #endif diff --git a/src/main/drivers/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c similarity index 96% rename from src/main/drivers/vtx_smartaudio.c rename to src/main/io/vtx_smartaudio.c index 02c47ab112..47d639122e 100644 --- a/src/main/drivers/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -8,21 +8,19 @@ #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/vtx_smartaudio.h" #include "io/serial.h" -#include "fc/runtime_config.h" -#include "config/config_master.h" +#include "io/vtx_smartaudio.h" #define SMARTAUDIO_EXTENDED_API -#define SMARTAUDIO_DPRINTF +//#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR #ifdef SMARTAUDIO_DPRINTF #include "common/printf.h" #define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 serialPort_t *debugSerialPort = NULL; -#define dprintf(x) if (debugSerialPort) printf x; +#define dprintf(x) if (debugSerialPort) printf x #else #define dprintf(x) #endif @@ -504,6 +502,18 @@ void smartAudioSetBandChan(int band, int chan) saQueueCmd(buf, 6); } +#ifdef SMARTAUDIO_EXTENDED_API +void smartAudioSetMode(int mode) +{ + static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; + + buf[4] = mode & 0x1f; + buf[5] = CRC8(buf, 5); + + saQueueCmd(buf, 6); +} +#endif + void smartAudioSetPowerByIndex(uint8_t index) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; @@ -550,19 +560,7 @@ void smartAudioSetPowerByIndex(uint8_t index) } } -#ifdef SMARTAUDIO_EXTENDED_API -void smartAudioSetMode(int mode) -{ - static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; - - buf[4] = mode & 0x1f; - buf[5] = CRC8(buf, 5); - - saQueueCmd(buf, 6); -} -#endif - -void smartAudioInit(void) +bool smartAudioInit() { #ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort @@ -575,18 +573,16 @@ void smartAudioInit(void) dprintf(("smartAudioInit: OK\r\n")); #endif - // Get an UART port assigned to SmartAudio. - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); - - if (!portConfig) - return; - - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + if (portConfig) { + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + } if (!smartAudioSerialPort) { - return; + return false; } + + return true; } #ifdef SMARTAUDIO_EXTENDED_API @@ -637,11 +633,6 @@ void smartAudioPitMode(void) void smartAudioProcess(uint32_t now) { - bool armedState = ARMING_FLAG(ARMED) ? true : false; - - if (armedState) - return; - if (smartAudioSerialPort == NULL) return; @@ -661,11 +652,13 @@ void smartAudioProcess(uint32_t now) saSendQueue(); return; } else if (!sa_configSynced) { +#if 0 // XXX Should take care of pit mode on boot case. smartAudioSetPowerByIndex(masterConfig.vtx_power); smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); saSendQueue(); sa_configSynced = true; +#endif } // diff --git a/src/main/drivers/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h similarity index 91% rename from src/main/drivers/vtx_smartaudio.h rename to src/main/io/vtx_smartaudio.h index 4ba54f291a..e9755e997e 100644 --- a/src/main/drivers/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -7,7 +7,7 @@ typedef struct vtxPowerTable_s { int16_t value; } vtxPowerTable_t; -void smartAudioInit(void); +bool smartAudioInit(); void smartAudioProcess(uint32_t); void smartAudioSetPowerByIndex(uint8_t); void smartAudioSetFreq(uint16_t); diff --git a/src/main/main.c b/src/main/main.c index e103b13c45..389ba7ed64 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -57,7 +57,6 @@ #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" @@ -82,6 +81,7 @@ #include "io/transponder_ir.h" #include "io/osd.h" #include "io/vtx.h" +#include "io/vtx_smartaudio.h" #include "scheduler/scheduler.h" @@ -620,7 +620,10 @@ void init(void) #endif #ifdef VTX_SMARTAUDIO - smartAudioInit(); + if (smartAudioInit()) { + smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); + smartAudioSetPowerByIndex(masterConfig.vtx_power); + } #endif // start all timers diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index a398f12dcd..feabe04451 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -14,4 +14,4 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ - drivers/vtx_smartaudio.c + io/vtx_smartaudio.c From 2813e5a199a28097b8f47d2741e8c35eec151c6f Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 14 Oct 2016 01:36:17 +0900 Subject: [PATCH 09/40] wip --- src/main/drivers/serial.h | 8 + src/main/io/osd.c | 74 +++++-- src/main/io/vtx_smartaudio.c | 370 ++++++++++++++++++++++------------- src/main/io/vtx_smartaudio.h | 26 ++- src/main/main.c | 5 +- 5 files changed, 317 insertions(+), 166 deletions(-) diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 104c1c1ae4..bb99dab680 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -32,6 +32,14 @@ typedef enum portOptions_t { SERIAL_PARITY_EVEN = 1 << 2, SERIAL_UNIDIR = 0 << 3, SERIAL_BIDIR = 1 << 3, + + /* + * Note on SERIAL_BIDIR_PP + * With SERIAL_BIDIR_PP, the very first start bit of back-to-back bytes + * is lost and the first data byte will be lost by a framing error. + * To ensure the first start bit to be sent, prepend a zero byte (0x00) + * to actual data bytes. + */ SERIAL_BIDIR_OD = 0 << 4, SERIAL_BIDIR_PP = 1 << 4 } portOptions_t; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d6a6f95f07..cbc2cdfc2b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -59,6 +59,10 @@ #include "drivers/vtx_soft_spi_rtc6705.h" #endif +#ifdef VTX_SMARTAUDIO +#include "io/vtx_smartaudio.h" +#endif + #include "common/printf.h" #define IS_HI(X) (rcData[X] > 1750) @@ -339,7 +343,7 @@ OSD_Entry menuLedstrip[] = }; #endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +#if defined(VTX) || defined(USE_RTC6705) static const char * const vtxBandNames[] = { "BOSCAM A", "BOSCAM B", @@ -356,19 +360,6 @@ OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; #endif // VTX -#ifdef VTX_SMARTAUDIO -static const char * const vtxSmartAudioPower[] = { - "OFF", - "PIT", - "25", - "200", - "500", - "800", -}; - -OSD_TAB_t entrySmartAudioPower = { &masterConfig.vtx_power, 5, &vtxSmartAudioPower[0] }; -#endif - OSD_Entry menu_vtx[] = { {"--- VTX ---", OME_Label, NULL, NULL}, @@ -382,13 +373,60 @@ OSD_Entry menu_vtx[] = #ifdef USE_RTC6705 {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, #endif // USE_RTC6705 -#ifdef VTX_SMARTAUDIO - {"POWER", OME_TAB, NULL, &entrySmartAudioPower}, -#endif {"BACK", OME_Back, NULL, NULL}, {NULL, OME_END, NULL, NULL} }; -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO +#endif // VTX || USE_RTC6705 + +#ifdef VTX_SMARTAUDIO +static const char * const smartAudioBandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; +OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; + +static const char * const smartAudioChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; + +static const char * const smartAudioPowerNames[] = { + "---", + " 25", + "200", + "500", + "800", +}; + +OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; + +static const char * const smartAudioModeNames[] = { + // Sync with SA_RFMODE_* + "------", + "ACTIVE", + "PIT-OR", + "PIT-IR", +}; + +OSD_TAB_t entrySmartAudioMode = { &smartAudioMode, 3, &smartAudioModeNames[0]}; + +OSD_Entry menu_vtx[] = +{ + { "--- VTX ---", OME_Label, NULL, NULL }, + { smartAudioStatusString, OME_Label, NULL, NULL }, + { "RFMODE", OME_TAB, smartAudioSetModeByGvar, &entrySmartAudioMode }, + { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, + { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan }, + { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower }, + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; +#endif // SMARTAUDIO OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 47d639122e..b2c4425137 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -6,18 +6,20 @@ #ifdef VTX_SMARTAUDIO +#include "common/printf.h" #include "drivers/system.h" #include "drivers/serial.h" #include "io/serial.h" #include "io/vtx_smartaudio.h" +#include "build/build_config.h" + #define SMARTAUDIO_EXTENDED_API -//#define SMARTAUDIO_DPRINTF +#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR #ifdef SMARTAUDIO_DPRINTF -#include "common/printf.h" #define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 serialPort_t *debugSerialPort = NULL; #define dprintf(x) if (debugSerialPort) printf x @@ -27,6 +29,10 @@ serialPort_t *debugSerialPort = NULL; #include "build/debug.h" +#ifdef OSD +static void smartAudioUpdateStatusString(void); // Forward +#endif + static serialPort_t *smartAudioSerialPort = NULL; // SmartAudio command and response codes @@ -96,7 +102,38 @@ static uint8_t CRC8(uint8_t *data, int8_t len) return crc; } -// Last received device states +// The band/chan to frequency table +// XXX Should really be consolidated among different vtx drivers +static const uint16_t saFreqTable[5][8] = +{ + { 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 +}; + +typedef struct saPowerTable_s { + int rfpower; + int16_t valueV1; + int16_t valueV2; +} saPowerTable_t; + +static saPowerTable_t saPowerTable[] = { + { 25, 7, 0 }, + { 200, 16, 1 }, + { 500, 25, 2 }, + { 800, 40, 3 }, +}; + +// Driver defined modes +#define SA_RFMODE_NODEF 0 +#define SA_RFMODE_ACTIVE 1 +#define SA_RFMODE_PIT_INRANGE 2 +#define SA_RFMODE_PIT_OUTRANGE 3 +#define SA_RFMODE_OFF 4 // Should not be used with osd + +// Last received device ('hard') states static int8_t sa_vers = 0; // Will be set to 1 or 2 static int8_t sa_chan = -1; @@ -106,13 +143,6 @@ static uint16_t sa_freq = 0; static uint16_t sa_pitfreq = 0; static bool sa_pitfreqpending = false; -// A measure for osd.c that resets on exit: -// masterConfig.{vtx_channel,vtx_power} can not be set at boot time, -// but after a communication with the smartaudio device is established. -// We remember here if channel and power is in sync with masterConfig. - -static bool sa_configSynced = false; - static void smartAudioPrintSettings(void) { #ifdef SMARTAUDIO_DPRINTF @@ -149,6 +179,17 @@ static void smartAudioPrintSettings(void) #endif } +static int saDacToPowerIndex(int dac) +{ + int idx; + + for (idx = 0 ; idx < 4 ; idx++) { + if (saPowerTable[idx].valueV1 <= dac) + return(idx); + } + return(3); +} + // Autobauding #define SMARTBAUD_MIN 4800 @@ -234,6 +275,18 @@ static void saProcessResponse(uint8_t *buf, int len) smartAudioPrintSettings(); + smartAudioUpdateStatusString(); + + // Export current settings for BFOSD 3.0.0 + + smartAudioBand = (sa_chan / 8) + 1; + smartAudioChan = (sa_chan % 8) + 1; + if (sa_vers == 2) { + smartAudioPower = sa_power + 1; // XXX Take care V1 + } else { + smartAudioPower = saDacToPowerIndex(sa_power) + 1; + } + #ifdef SMARTAUDIO_DEBUG_MONITOR debug[0] = sa_vers * 100 + sa_opmode; debug[1] = sa_chan; @@ -344,17 +397,39 @@ static void saSendFrame(uint8_t *buf, int len) { int i; - serialWrite(smartAudioSerialPort, 0x00); + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit for (i = 0 ; i < len ; i++) serialWrite(smartAudioSerialPort, buf[i]); - serialWrite(smartAudioSerialPort, 0x00); + serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this sa_lastTransmission = millis(); sa_pktsent++; } + +/* + * Retransmission and command queuing + * + * The transport level support includes retransmission on response timeout + * and command queueing. + * + * Resend buffer: + * The smartaudio returns response for valid command frames in no less + * than 60msec, which we can't wait. So there's a need for a resend buffer. + * + * Command queueing: + * The driver autonomously sends GetSettings command for auto-bauding, + * asynchronous to user initiated commands; commands issued while another + * command is outstanding must be queued for later processing. + * The queueing also handles the case in which multiple commands are + * required to implement a user level command, e.g., "Change RF power + * from PIT to 200mW" which requires (1) SetPower and (2) SetMode. + */ + +// Retransmission + static void saResendCmd(void) { saSendFrame(sa_osbuf, sa_oslen); @@ -373,14 +448,14 @@ static void saSendCmd(uint8_t *buf, int len) saSendFrame(sa_osbuf, sa_oslen); } -// Command transmission queue and management +// Command queue management typedef struct saCmdQueue_s { uint8_t *buf; int len; } saCmdQueue_t; -#define SA_QSIZE 4 // 2 should be enough +#define SA_QSIZE 4 // 1 heartbeat (GetSettings) + 2 commands + 1 slack static saCmdQueue_t sa_queue[SA_QSIZE]; static uint8_t sa_qhead = 0; static uint8_t sa_qtail = 0; @@ -427,24 +502,6 @@ static void saSendQueue(void) // Individual commands -vtxPowerTable_t saPowerTableV1[] = { - { " 25", 25, 7 }, - { "200", 200, 16 }, - { "500", 500, 25 }, - { "800", 800, 40 }, - { NULL } -}; - -vtxPowerTable_t saPowerTableV2[] = { - { "OFF", 0, -2 }, - { "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}; @@ -481,9 +538,24 @@ void smartAudioSetFreqGetPit(void) sa_pitfreqpending = true; } -uint16_t smartAudioGetFreq(void) +bool smartAudioGetFreq(uint16_t *pFreq) { - return sa_freq; + if (sa_vers == 0) + return false; + + *pFreq = sa_freq; + return true; +} + +bool smartAudioGetBandChan(uint8_t *pBand, uint8_t *pChan) +{ + if (sa_vers == 0) + return false; + + *pBand = sa_chan / 8; + *pChan = sa_chan % 8; + + return true; } uint16_t smartAudioGetPitFreq(void) @@ -492,7 +564,7 @@ uint16_t smartAudioGetPitFreq(void) } #endif -void smartAudioSetBandChan(int band, int chan) +void smartAudioSetBandChan(uint8_t band, uint8_t chan) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 }; @@ -502,8 +574,7 @@ void smartAudioSetBandChan(int band, int chan) saQueueCmd(buf, 6); } -#ifdef SMARTAUDIO_EXTENDED_API -void smartAudioSetMode(int mode) +static void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; @@ -512,7 +583,6 @@ void smartAudioSetMode(int mode) saQueueCmd(buf, 6); } -#endif void smartAudioSetPowerByIndex(uint8_t index) { @@ -520,57 +590,42 @@ void smartAudioSetPowerByIndex(uint8_t index) dprintf(("smartAudioSetPowerByIndex: index %d\r\n", index)); - if (sa_vers != 1 && sa_vers != 2) { + if (sa_vers == 0) { // Unknown or yet unknown version. return; } - if (index > 5) + if (index > 3) 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 { - int pwrval = saPowerTableV2[index].value; - - dprintf(("smartAudioSetPowerByIndex: pwrval %d\n", pwrval)); - - if (pwrval >= 0) { - if (sa_opmode & SA_MODE_GET_PITMODE) { - // Currently in pit mode; have to deactivate and set power. - } else { - dprintf(("smartAudioSetPowerByIndex: V2 value %d\r\n", - saPowerTableV2[index].value)); - buf[4] = saPowerTableV2[index].value; - buf[5] = CRC8(buf, 5); - saQueueCmd(buf, 6); - smartAudioSetMode(0x00); // Reset power off - } - } else if (pwrval == -1) { - // Pit mode - // Not implemented yet. - } else if (pwrval == -2) { - // Power off - // Not implemented yet. - } - } + buf[4] = (sa_vers == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; + buf[5] = CRC8(buf, 5); + saQueueCmd(buf, 6); } +#if 0 +dup? +void smartAudioConfigurePowerByGvar(void *opaque) +{ + UNUSED(opaque); + + if (smartAudioPower == 0) + return; + + smartAudioSetPowerByIndex(smartAudioPower - 1); +} +#endif + bool smartAudioInit() { #ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); - if (!debugSerialPort) { - return; + if (debugSerialPort) { + setPrintfSerialPort(debugSerialPort); + dprintf(("smartAudioInit: OK\r\n")); } - setPrintfSerialPort(debugSerialPort); - dprintf(("smartAudioInit: OK\r\n")); #endif serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); @@ -585,48 +640,15 @@ bool smartAudioInit() return true; } -#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); + saSetMode(SA_MODE_SET_UNLOCK|SA_MODE_SET_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); } else { - smartAudioSetMode(SA_MODE_SET_UNLOCK|SA_MODE_CLR_PITMODE); + saSetMode(SA_MODE_SET_UNLOCK|SA_MODE_CLR_PITMODE); } } #endif @@ -641,28 +663,16 @@ void smartAudioProcess(uint32_t now) saReceiveFramer((uint16_t)c); } - // Evaluate baudrate after each frame reception - + // Re-evaluate baudrate after each frame reception saAutobaud(); - // If we haven't talked to the device, keep trying. - if (sa_vers == 0) { + // If we haven't talked to the device, keep trying. smartAudioGetSettings(); saSendQueue(); return; - } else if (!sa_configSynced) { -#if 0 - // XXX Should take care of pit mode on boot case. - smartAudioSetPowerByIndex(masterConfig.vtx_power); - smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); - saSendQueue(); - sa_configSynced = true; -#endif } - // - if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out @@ -687,19 +697,109 @@ void smartAudioProcess(uint32_t now) } } -// Things to make it work for the first cut on v3.0.0 +// API for BFOSD3.0 -// This doesn't belong here, really. -uint16_t current_vtx_channel; +char smartAudioStatusString[31] = "- - ---- --- ----"; -// A table that's repeated over and over in every vtx code. -const uint16_t vtx_freq[] = +uint8_t smartAudioBand = 0; +uint8_t smartAudioChan = 0; +uint8_t smartAudioPower = 0; + +static void smartAudioUpdateStatusString(void) { - 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 -}; + if (sa_vers == 0) + return; -#endif + tfp_sprintf(smartAudioStatusString, "%c %d %4d %3d %3s", + "ABEFR"[sa_chan / 8], + (sa_chan % 8) + 1, + saFreqTable[sa_chan / 8][sa_chan % 8], + (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower, + "---"); +} + +void smartAudioConfigureBandByGvar(void *opaque) +{ + UNUSED(opaque); + + if (sa_vers == 0) { + // Bounce back + smartAudioBand = 0; + return; + } + + if (smartAudioBand == 0) { + // Bounce back + smartAudioBand = 1; + return; + } + + smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); +} + +void smartAudioConfigureChanByGvar(void *opaque) +{ + UNUSED(opaque); + + if (sa_vers == 0) { + // Bounce back + smartAudioChan = 0; + return; + } + + if (smartAudioChan == 0) { + // Bounce back + smartAudioChan = 1; + return; + } + + smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); +} + +void smartAudioConfigurePowerByGvar(void *opaque) +{ + UNUSED(opaque); + + if (sa_vers == 0) { + // Bounce back + smartAudioPower = 0; + return; + } + + if (smartAudioPower == 0) { + // Bounce back + smartAudioPower = 1; + return; + } + + smartAudioSetPowerByIndex(smartAudioPower - 1); +} + +void smartAudioSetModeByGvar(void *opaque) +{ + UNUSED(opaque); + + if (sa_vers != 2) { + // Bounce back + smartAudioMode = SA_RFMODE_NODEF; + return; + } + + if (smartAudioMode == 0) { + ++smartAudioMode; + return; + } + + switch (smartAudioMode) { + case SA_RFMODE_ACTIVE: + saSetMode(SA_MODE_CLR_PITMODE); + break; + + case SA_RFMODE_PIT_OUTRANGE: + break; + + case SA_RFMODE_PIT_INRANGE: + break; + } +} +#endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index e9755e997e..36f9aba696 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -1,14 +1,22 @@ // For generic API use, but here for now -typedef struct vtxPowerTable_s { - char *name; - int16_t power; - int16_t value; -} vtxPowerTable_t; - bool smartAudioInit(); void smartAudioProcess(uint32_t); -void smartAudioSetPowerByIndex(uint8_t); -void smartAudioSetFreq(uint16_t); -void smartAudioSetBandChan(int, int); + +#ifdef OSD + +// API for BFOSD3.0 + +char smartAudioStatusString[31]; + +uint8_t smartAudioBand; +uint8_t smartAudioChan; +uint8_t smartAudioPower; +uint8_t smartAudioMode; + +void smartAudioConfigureBandByGvar(void *); +void smartAudioConfigureChanByGvar(void *); +void smartAudioConfigurePowerByGvar(void *); +void smartAudioSetModeByGvar(void *); +#endif diff --git a/src/main/main.c b/src/main/main.c index 389ba7ed64..c5b35a9460 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -620,10 +620,7 @@ void init(void) #endif #ifdef VTX_SMARTAUDIO - if (smartAudioInit()) { - smartAudioSetBandChan(masterConfig.vtx_channel / 8, masterConfig.vtx_channel % 8); - smartAudioSetPowerByIndex(masterConfig.vtx_power); - } + smartAudioInit(); #endif // start all timers From d5eee5965d6df7cf3705f23e0acec43ca7c4affc Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 15 Oct 2016 01:24:48 +0900 Subject: [PATCH 10/40] wip --- src/main/io/osd.c | 23 ++++- src/main/io/vtx_smartaudio.c | 192 ++++++++++++++++------------------- src/main/io/vtx_smartaudio.h | 8 ++ 3 files changed, 117 insertions(+), 106 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index cbc2cdfc2b..129d35eb77 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -415,6 +415,25 @@ static const char * const smartAudioModeNames[] = { OSD_TAB_t entrySmartAudioMode = { &smartAudioMode, 3, &smartAudioModeNames[0]}; +OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; + +OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; + +OSD_Entry menu_vtxstat[] = { + { "--- VTX STATS ---", OME_Label, NULL, NULL }, + { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate }, + { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre }, + { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen }, + { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr }, + { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr }, + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + OSD_Entry menu_vtx[] = { { "--- VTX ---", OME_Label, NULL, NULL }, @@ -422,11 +441,13 @@ OSD_Entry menu_vtx[] = { "RFMODE", OME_TAB, smartAudioSetModeByGvar, &entrySmartAudioMode }, { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan }, + { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq }, { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower }, + { "STAT", OME_Submenu, osdChangeScreen, &menu_vtxstat[0]}, { "BACK", OME_Back, NULL, NULL }, { NULL, OME_END, NULL, NULL } }; -#endif // SMARTAUDIO +#endif // VTX_SMARTAUDIO OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index b2c4425137..8919aedacf 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -68,14 +68,14 @@ enum { #define SA_FREQ_SETPIT (1 << 15) // Error counters, may be good for post production debugging. -static uint16_t saerr_badpre = 0; -static uint16_t saerr_badlen = 0; -static uint16_t saerr_crc = 0; -static uint16_t saerr_oooresp = 0; +uint16_t saerr_badpre = 0; +uint16_t saerr_badlen = 0; +uint16_t saerr_crc = 0; +uint16_t saerr_oooresp = 0; // Receive frame reassembly buffer -#define SMARTAUDIO_MAXLEN 32 -static uint8_t sa_rbuf[SMARTAUDIO_MAXLEN]; +#define SA_MAX_RCVLEN 11 +static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard // CRC8 computations @@ -140,24 +140,19 @@ 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 int8_t sa_overs = 0; +static int8_t sa_ochan; +static int8_t sa_opower; +static int8_t sa_oopmode; +static uint16_t sa_ofreq; + 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)); @@ -171,11 +166,6 @@ static void smartAudioPrintSettings(void) 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 } @@ -194,7 +184,7 @@ static int saDacToPowerIndex(int dac) #define SMARTBAUD_MIN 4800 #define SMARTBAUD_MAX 4950 -static int smartbaud = SMARTBAUD_MIN; +uint16_t smartAudioSmartbaud = SMARTBAUD_MIN; static int adjdir = 1; // -1=going down, 1=going up static int baudstep = 50; @@ -211,8 +201,10 @@ static void saAutobaud(void) // Not enough samples collected return; +#if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", - smartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent))); + smartAudioSmartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent))); +#endif if (((sa_pktrcvd * 100) / sa_pktsent) >= 70) { // This is okay @@ -223,19 +215,19 @@ static void saAutobaud(void) dprintf(("autobaud: adjusting\r\n")); - if ((adjdir == 1) && (smartbaud == SMARTBAUD_MAX)) { + if ((adjdir == 1) && (smartAudioSmartbaud == SMARTBAUD_MAX)) { adjdir = -1; dprintf(("autobaud: now going down\r\n")); - } else if ((adjdir == -1 && smartbaud == SMARTBAUD_MIN)) { + } else if ((adjdir == -1 && smartAudioSmartbaud == SMARTBAUD_MIN)) { adjdir = 1; dprintf(("autobaud: now going up\r\n")); } - smartbaud += baudstep * adjdir; + smartAudioSmartbaud += baudstep * adjdir; - dprintf(("autobaud: %d\r\n", smartbaud)); + dprintf(("autobaud: %d\r\n", smartAudioSmartbaud)); - smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, smartbaud); + smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, smartAudioSmartbaud); sa_pktsent = 0; sa_pktrcvd = 0; @@ -273,14 +265,23 @@ static void saProcessResponse(uint8_t *buf, int len) sa_opmode = buf[4]; sa_freq = (buf[5] << 8)|buf[6]; - smartAudioPrintSettings(); + if ((sa_overs == sa_vers) + && (sa_ochan == sa_chan) + && (sa_opower == sa_power) + && (sa_oopmode == sa_opmode) + && (sa_ofreq == sa_freq)) + break; - smartAudioUpdateStatusString(); + // Debug + smartAudioPrintSettings(); // Export current settings for BFOSD 3.0.0 smartAudioBand = (sa_chan / 8) + 1; smartAudioChan = (sa_chan % 8) + 1; + smartAudioFreq = saFreqTable[sa_chan / 8][sa_chan % 8]; + smartAudioUpdateStatusString(); + if (sa_vers == 2) { smartAudioPower = sa_power + 1; // XXX Take care V1 } else { @@ -293,6 +294,11 @@ static void saProcessResponse(uint8_t *buf, int len) debug[2] = sa_freq; debug[3] = sa_power; #endif + sa_overs = sa_vers; + sa_ochan = sa_chan; + sa_opower = sa_power; + sa_oopmode = sa_opmode; + sa_ofreq = sa_freq; break; @@ -306,8 +312,17 @@ static void saProcessResponse(uint8_t *buf, int len) if (len < 5) break; - if (sa_pitfreqpending) - sa_pitfreq = ((buf[2] << 8)|buf[3]) & ~SA_FREQ_GETPIT; + uint16_t freq = (buf[2] << 8)|buf[3]; + + if (freq & SA_FREQ_GETPIT) { + sa_pitfreq = freq & ~SA_FREQ_GETPIT; + dprintf(("saProcessResponse: GETPIT freq %d\r\n", sa_pitfreq)); + smartAudioUpdateStatusString(); + } else if (freq & SA_FREQ_SETPIT) { + dprintf(("saProcessResponse: SETPIT freq %d\r\n", freq)); + } else { + dprintf(("saProcessResponse: GETFREQ freq %d\r\n", freq)); + } break; case SA_CMD_SET_MODE: // Set Mode @@ -358,7 +373,7 @@ static void saReceiveFramer(uint8_t c) sa_rbuf[1] = c; len = c; - if (len > SMARTAUDIO_MAXLEN - 2) { + if (len > SA_MAX_RCVLEN - 2) { saerr_badlen++; state = S_WAITPRE1; } else if (len == 0) { @@ -378,7 +393,7 @@ static void saReceiveFramer(uint8_t c) case S_WAITCRC: if (CRC8(sa_rbuf, 2 + len) == c) { - // Response + // Got a response saProcessResponse(sa_rbuf, len + 2); sa_pktrcvd++; } else if (sa_rbuf[0] & 1) { @@ -424,8 +439,7 @@ static void saSendFrame(uint8_t *buf, int len) * asynchronous to user initiated commands; commands issued while another * command is outstanding must be queued for later processing. * The queueing also handles the case in which multiple commands are - * required to implement a user level command, e.g., "Change RF power - * from PIT to 200mW" which requires (1) SetPower and (2) SetMode. + * required to implement a user level command. */ // Retransmission @@ -502,13 +516,14 @@ static void saSendQueue(void) // Individual commands -void smartAudioGetSettings(void) +static void saGetSettings(void) { - static uint8_t bufGetSettings[5] = {0xAA, 0x55, 0x03, 0x00, 0x9F}; + static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F}; + saQueueCmd(bufGetSettings, 5); } -void smartAudioSetFreq(uint16_t freq) +static void saSetFreq(uint16_t freq) { static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 }; @@ -519,48 +534,17 @@ void smartAudioSetFreq(uint16_t freq) 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) +static void saSetPitFreq(uint16_t freq) { - smartAudioSetFreq(freq | SA_FREQ_SETPIT); - - sa_pitfreq = 0; // Will be read by a following heartbeat + saSetFreq(freq | SA_FREQ_SETPIT); } -void smartAudioSetFreqGetPit(void) +static void saGetPitFreq(void) { - smartAudioSetFreq(SA_FREQ_GETPIT); - - sa_pitfreqpending = true; -} - -bool smartAudioGetFreq(uint16_t *pFreq) -{ - if (sa_vers == 0) - return false; - - *pFreq = sa_freq; - return true; -} - -bool smartAudioGetBandChan(uint8_t *pBand, uint8_t *pChan) -{ - if (sa_vers == 0) - return false; - - *pBand = sa_chan / 8; - *pChan = sa_chan % 8; - - return true; -} - -uint16_t smartAudioGetPitFreq(void) -{ - return sa_pitfreq; + saSetFreq(SA_FREQ_GETPIT); } #endif @@ -603,19 +587,6 @@ void smartAudioSetPowerByIndex(uint8_t index) saQueueCmd(buf, 6); } -#if 0 -dup? -void smartAudioConfigurePowerByGvar(void *opaque) -{ - UNUSED(opaque); - - if (smartAudioPower == 0) - return; - - smartAudioSetPowerByIndex(smartAudioPower - 1); -} -#endif - bool smartAudioInit() { #ifdef SMARTAUDIO_DPRINTF @@ -655,6 +626,8 @@ void smartAudioPitMode(void) void smartAudioProcess(uint32_t now) { + static bool initialSent = false; + if (smartAudioSerialPort == NULL) return; @@ -666,10 +639,11 @@ void smartAudioProcess(uint32_t now) // Re-evaluate baudrate after each frame reception saAutobaud(); - if (sa_vers == 0) { - // If we haven't talked to the device, keep trying. - smartAudioGetSettings(); + if (!initialSent) { + saGetSettings(); + saGetPitFreq(); saSendQueue(); + initialSent = true; return; } @@ -686,36 +660,42 @@ void smartAudioProcess(uint32_t now) #ifdef SMARTAUDIO_PITMODE_DEBUG static int turn = 0; if ((turn++ % 2) == 0) { - smartAudioGetSettings(); + saGetSettings(); } else { smartAudioPitMode(); } #else - smartAudioGetSettings(); + saGetSettings(); #endif saSendQueue(); } } +#ifdef OSD // API for BFOSD3.0 -char smartAudioStatusString[31] = "- - ---- --- ----"; +char smartAudioStatusString[31] = "- - ---- --- ---- -"; uint8_t smartAudioBand = 0; uint8_t smartAudioChan = 0; uint8_t smartAudioPower = 0; +uint16_t smartAudioFreq = 0; static void smartAudioUpdateStatusString(void) { if (sa_vers == 0) return; - tfp_sprintf(smartAudioStatusString, "%c %d %4d %3d %3s", + tfp_sprintf(smartAudioStatusString, "%c %d %4d %3d ", "ABEFR"[sa_chan / 8], (sa_chan % 8) + 1, saFreqTable[sa_chan / 8][sa_chan % 8], - (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower, - "---"); + (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower); + + if (sa_vers == 2) + tfp_sprintf(&smartAudioStatusString[13], "%4d", sa_pitfreq); + else + tfp_sprintf(&smartAudioStatusString[13], "%4s", "----"); } void smartAudioConfigureBandByGvar(void *opaque) @@ -723,13 +703,13 @@ void smartAudioConfigureBandByGvar(void *opaque) UNUSED(opaque); if (sa_vers == 0) { - // Bounce back + // Bounce back; not online yet smartAudioBand = 0; return; } if (smartAudioBand == 0) { - // Bounce back + // Bouce back, no going back to undef state smartAudioBand = 1; return; } @@ -742,13 +722,13 @@ void smartAudioConfigureChanByGvar(void *opaque) UNUSED(opaque); if (sa_vers == 0) { - // Bounce back + // Bounce back; not online yet smartAudioChan = 0; return; } if (smartAudioChan == 0) { - // Bounce back + // Bounce back; no going back to undef state smartAudioChan = 1; return; } @@ -761,13 +741,13 @@ void smartAudioConfigurePowerByGvar(void *opaque) UNUSED(opaque); if (sa_vers == 0) { - // Bounce back + // Bounce back; not online yet smartAudioPower = 0; return; } if (smartAudioPower == 0) { - // Bounce back + // Bouce back; no going back to undef state smartAudioPower = 1; return; } @@ -780,12 +760,13 @@ void smartAudioSetModeByGvar(void *opaque) UNUSED(opaque); if (sa_vers != 2) { - // Bounce back + // Bounce back; not online yet or can't handle mode (V1) smartAudioMode = SA_RFMODE_NODEF; return; } if (smartAudioMode == 0) { + // Bouce back; no going back to undef state ++smartAudioMode; return; } @@ -802,4 +783,5 @@ void smartAudioSetModeByGvar(void *opaque) break; } } +#endif // OSD #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 36f9aba696..962f4ee813 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -8,10 +8,18 @@ void smartAudioProcess(uint32_t); // API for BFOSD3.0 +uint16_t smartAudioSmartbaud; + +uint16_t saerr_badpre; +uint16_t saerr_badlen; +uint16_t saerr_crcerr; +uint16_t saerr_oooresp; + char smartAudioStatusString[31]; uint8_t smartAudioBand; uint8_t smartAudioChan; +uint16_t smartAudioFreq; uint8_t smartAudioPower; uint8_t smartAudioMode; From 7cc1010cb5376570e40b40ca4a4d209e3c3b2697 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 16 Oct 2016 23:56:10 +0900 Subject: [PATCH 11/40] wip --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 129d35eb77..f8f7e2030e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -436,7 +436,7 @@ OSD_Entry menu_vtxstat[] = { OSD_Entry menu_vtx[] = { - { "--- VTX ---", OME_Label, NULL, NULL }, + { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL }, { smartAudioStatusString, OME_Label, NULL, NULL }, { "RFMODE", OME_TAB, smartAudioSetModeByGvar, &entrySmartAudioMode }, { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, From de74dbd7769cdba98cd62d20d814619260d81c4d Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 23 Oct 2016 00:43:08 +0900 Subject: [PATCH 12/40] Pit mode first implementation --- src/main/config/config.c | 4 ++ src/main/config/config_master.h | 4 ++ src/main/io/osd.c | 49 ++++++++++++++--- src/main/io/serial_cli.c | 4 ++ src/main/io/vtx_smartaudio.c | 97 ++++++++++++++++++++++++++------- src/main/io/vtx_smartaudio.h | 10 +++- 6 files changed, 137 insertions(+), 31 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 9462910668..2245fca00d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -602,6 +602,10 @@ void createDefaultConfig(master_t *config) config->vtx_mhz = 5740; //F0 #endif +#ifdef VTX_SMARTAUDIO + config->vtx_smartaudio_opmodel = 1; // PIT +#endif + #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index a7154a60e8..3ef8a88b5a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -194,6 +194,10 @@ typedef struct master_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; #endif +#ifdef VTX_SMARTAUDIO + uint8_t vtx_smartaudio_opmodel; // 0=FREE, 1=PIT +#endif + #ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f8f7e2030e..f20e32f992 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -405,26 +405,58 @@ static const char * const smartAudioPowerNames[] = { OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; -static const char * const smartAudioModeNames[] = { - // Sync with SA_RFMODE_* +static const char * const smartAudioTxModeNames[] = { "------", - "ACTIVE", "PIT-OR", "PIT-IR", + "ACTIVE", }; -OSD_TAB_t entrySmartAudioMode = { &smartAudioMode, 3, &smartAudioModeNames[0]}; +OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; +static const char * const smartAudioOpModelNames[] = { + "FREE", + "PIT", +}; + +OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; + +static const char * const smartAudioPitFModeNames[] = { + "IN-RANGE", + "OUT-RANGE", +}; + +OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; + +OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; + +OSD_Entry menu_smartAudioConfig[] = { + { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL }, + { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel }, + { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode }, + { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq }, // OME_Poll_UINT16 + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + +static const char * const smartAudioStatusNames[] = { + "OFFLINE", + "ONLINE V1", + "ONLINE V2", +}; + +OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; -OSD_Entry menu_vtxstat[] = { - { "--- VTX STATS ---", OME_Label, NULL, NULL }, +OSD_Entry menu_smartAudioStats[] = { + { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL }, + { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline }, { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate }, { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre }, { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen }, @@ -438,12 +470,13 @@ OSD_Entry menu_vtx[] = { { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL }, { smartAudioStatusString, OME_Label, NULL, NULL }, - { "RFMODE", OME_TAB, smartAudioSetModeByGvar, &entrySmartAudioMode }, + { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode }, { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan }, { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq }, { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower }, - { "STAT", OME_Submenu, osdChangeScreen, &menu_vtxstat[0]}, + { "CONFIG", OME_Submenu, osdChangeScreen, &menu_smartAudioConfig[0] }, + { "STAT", OME_Submenu, osdChangeScreen, &menu_smartAudioStats[0] }, { "BACK", OME_Back, NULL, NULL }, { NULL, OME_END, NULL, NULL } }; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3a0f4973e5..e112d06eb3 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -925,6 +925,10 @@ const clivalue_t valueTable[] = { { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } }, #endif +#ifdef VTX_SMARTAUDIO + { "vtx_smartaudio_opmodel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_smartaudio_opmodel, .config.minmax = { 0, 1 } }, +#endif + #ifdef MAG { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 8919aedacf..99c14b663b 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -12,6 +12,15 @@ #include "io/serial.h" #include "io/vtx_smartaudio.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" +#include "config/config.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" + #include "build/build_config.h" #define SMARTAUDIO_EXTENDED_API @@ -127,11 +136,14 @@ static saPowerTable_t saPowerTable[] = { }; // Driver defined modes -#define SA_RFMODE_NODEF 0 -#define SA_RFMODE_ACTIVE 1 -#define SA_RFMODE_PIT_INRANGE 2 -#define SA_RFMODE_PIT_OUTRANGE 3 -#define SA_RFMODE_OFF 4 // Should not be used with osd + +#define SA_OPMODEL_FREE 0 // Power up transmitting +#define SA_OPMODEL_PIT 1 // Power up in pit mode + +#define SA_TXMODE_NODEF 0 +#define SA_TXMODE_PIT_OUTRANGE 1 +#define SA_TXMODE_PIT_INRANGE 2 +#define SA_TXMODE_ACTIVE 3 // Last received device ('hard') states @@ -280,6 +292,15 @@ static void saProcessResponse(uint8_t *buf, int len) smartAudioBand = (sa_chan / 8) + 1; smartAudioChan = (sa_chan % 8) + 1; smartAudioFreq = saFreqTable[sa_chan / 8][sa_chan % 8]; + + if ((sa_opmode & SA_MODE_GET_PITMODE) == 0) { + smartAudioTxMode = SA_TXMODE_ACTIVE; + } else if (sa_opmode & SA_MODE_GET_IN_RANGE_PITMODE) { + smartAudioTxMode = SA_TXMODE_PIT_INRANGE; + } else { + smartAudioTxMode = SA_TXMODE_PIT_OUTRANGE; + } + smartAudioUpdateStatusString(); if (sa_vers == 2) { @@ -608,11 +629,41 @@ bool smartAudioInit() return false; } + smartAudioOpModel = masterConfig.vtx_smartaudio_opmodel; + return true; } -#ifdef SMARTAUDIO_PITMODE_DEBUG -void smartAudioPitMode(void) +void smartAudioConfigurePitFModeByGvar(void *opaque) +{ + UNUSED(opaque); + + if (smartAudioPitFMode == 0) { + saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); + } else { + saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); + } +} + +void smartAudioConfigureOpModelByGvar(void *opaque) +{ + UNUSED(opaque); + + masterConfig.vtx_smartaudio_opmodel = smartAudioOpModel; + + if (smartAudioOpModel == SA_OPMODEL_FREE) { + // VTX should power up transmitting. + // Turn off In-Range and Out-Range bits + saSetMode(0); + } else { + // VTX should power up in pit mode. + // Setup In-Range or Out-Range bits + smartAudioConfigurePitFModeByGvar(opaque); + } +} + +#if 0 +void { static int turn = 0; @@ -755,32 +806,36 @@ void smartAudioConfigurePowerByGvar(void *opaque) smartAudioSetPowerByIndex(smartAudioPower - 1); } -void smartAudioSetModeByGvar(void *opaque) +void smartAudioSetTxModeByGvar(void *opaque) { UNUSED(opaque); if (sa_vers != 2) { // Bounce back; not online yet or can't handle mode (V1) - smartAudioMode = SA_RFMODE_NODEF; + smartAudioTxMode = SA_TXMODE_NODEF; return; } - if (smartAudioMode == 0) { + if (smartAudioTxMode == 0) { // Bouce back; no going back to undef state - ++smartAudioMode; + ++smartAudioTxMode; return; } - switch (smartAudioMode) { - case SA_RFMODE_ACTIVE: - saSetMode(SA_MODE_CLR_PITMODE); - break; - - case SA_RFMODE_PIT_OUTRANGE: - break; - - case SA_RFMODE_PIT_INRANGE: - break; + if (smartAudioTxMode == SA_TXMODE_ACTIVE) { + if (smartAudioOpModel == SA_OPMODEL_FREE) { + saSetMode(SA_MODE_CLR_PITMODE); + } else { + if (smartAudioPitFMode == 0) + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); + else + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); + } + } else { + if ((sa_opmode & SA_MODE_GET_PITMODE) == 0) { + // Can't go back to pit mode + smartAudioTxMode = SA_TXMODE_ACTIVE; + } } } #endif // OSD diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 962f4ee813..b146ca1b39 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -17,14 +17,20 @@ uint16_t saerr_oooresp; char smartAudioStatusString[31]; +uint8_t smartAudioOpModel; +uint8_t smartAudioStatus; uint8_t smartAudioBand; uint8_t smartAudioChan; uint16_t smartAudioFreq; uint8_t smartAudioPower; -uint8_t smartAudioMode; +uint8_t smartAudioTxMode; +uint8_t smartAudioPitFMode; +uint16_t smartAudioORFreq; +void smartAudioConfigureOpModelByGvar(void *); +void smartAudioConfigurePitFModeByGvar(void *); void smartAudioConfigureBandByGvar(void *); void smartAudioConfigureChanByGvar(void *); void smartAudioConfigurePowerByGvar(void *); -void smartAudioSetModeByGvar(void *); +void smartAudioSetTxModeByGvar(void *); #endif From 9264a9a30241679795e9a9b32c9aa7cb96a29760 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 1 Nov 2016 12:23:12 +0900 Subject: [PATCH 13/40] Working version after rebase (onto master) --- src/main/fc/fc_tasks.c | 25 +++++++++++++++++++++++++ src/main/io/vtx_smartaudio.c | 2 ++ src/main/scheduler/scheduler.h | 2 +- 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b81d36ce01..43b308ab23 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -269,6 +269,19 @@ static void taskUpdateOsd(uint32_t currentTime) } #endif +#ifdef VTX_CONTROL +// Everything that listens to VTX devices +void taskVtxControl(uint32_t currentTime) +{ + if (ARMING_FLAG(ARMED)) + return; + +#ifdef VTX_SMARTAUDIO + smartAudioProcess(currentTime); +#endif +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -329,6 +342,9 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef VTX_CONTROL + setTaskEnabled(TASK_VTXCTRL, true); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -488,4 +504,13 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + +#ifdef VTX_CONTROL + [TASK_VTXCTRL] = { + .taskName = "VTXCTRL", + .taskFunc = taskVtxControl, + .desiredPeriod = 1000000 / 5, // 5Hz @200msec + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif }; diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 99c14b663b..786a411d4a 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -16,9 +16,11 @@ #include "fc/runtime_config.h" #include "flight/pid.h" +#if 0 #include "config/config.h" #include "config/config_eeprom.h" #include "config/config_profile.h" +#endif #include "config/config_master.h" #include "build/build_config.h" diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index f90891c4da..7bb6543a02 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -86,7 +86,7 @@ typedef enum { TASK_BST_MASTER_PROCESS, #endif #ifdef VTX_CONTROL - TASK_VTX_CONTROL, + TASK_VTXCTRL, #endif /* Count of real tasks */ From 278cf811a70a7355fa210a53bbca0d1bbcc9f1e9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 01:59:02 +0900 Subject: [PATCH 14/40] Rebased onto CMS (WIP, nasty OLEDCMS / cli hang bug) When OLEDCMS is enabled, enter cli hangs the FC. --- Makefile | 6 +- src/main/common/filter.c | 2 +- src/main/drivers/exti.c | 6 + src/main/drivers/pwm_output.c | 6 +- src/main/drivers/pwm_output_stm32f3xx.c | 39 +- src/main/drivers/pwm_output_stm32f4xx.c | 41 +- src/main/drivers/pwm_rx.c | 8 +- src/main/drivers/timer.c | 17 +- src/main/drivers/timer.h | 11 +- src/main/drivers/timer_hal.c | 19 +- src/main/fc/config.c | 24 +- src/main/fc/config.h | 2 +- src/main/fc/fc_msp.c | 14 - src/main/fc/fc_msp.h | 2 - src/main/fc/fc_tasks.c | 23 +- src/main/fc/rc_controls.c | 8 +- src/main/io/canvas.c | 26 +- src/main/io/canvas.h | 2 +- src/main/io/cms.c | 643 ++++----------------- src/main/io/cms.h | 32 +- src/main/io/cms_blackbox.c | 77 +++ src/main/io/cms_blackbox.h | 4 + src/main/io/cms_imu.c | 206 +++++++ src/main/io/cms_imu.h | 13 + src/main/io/cms_ledstrip.c | 107 ++++ src/main/io/cms_ledstrip.h | 6 + src/main/io/cms_osd.h | 2 + src/main/io/cms_types.h | 9 + src/main/io/cms_vtx.c | 96 +++ src/main/io/cms_vtx.h | 7 + src/main/io/{display.c => dashboard.c} | 113 ++-- src/main/io/{display.h => dashboard.h} | 16 +- src/main/io/gps.c | 14 +- src/main/io/osd.c | 148 +++-- src/main/io/serial_cli.c | 118 ++-- src/main/io/vtx_smartaudio.c | 107 ++++ src/main/io/vtx_smartaudio.h | 2 + src/main/main.c | 19 +- src/main/main.c.orig | 24 +- src/main/msp/msp_serial.c | 28 +- src/main/msp/msp_serial.h | 4 +- src/main/scheduler/scheduler.h | 4 +- src/main/target/AIORACERF3/target.c | 24 +- src/main/target/AIR32/target.c | 18 +- src/main/target/AIR32/target.h | 1 - src/main/target/AIRHEROF3/target.c | 29 +- src/main/target/ALIENFLIGHTF1/target.c | 28 +- src/main/target/ALIENFLIGHTF3/target.c | 24 +- src/main/target/ALIENFLIGHTF4/target.c | 27 +- src/main/target/ANYFCF7/target.c | 64 +- src/main/target/BETAFLIGHTF3/target.c | 20 +- src/main/target/BETAFLIGHTF3/target.h | 14 +- src/main/target/BLUEJAYF4/target.c | 14 +- src/main/target/CC3D/target.c | 24 +- src/main/target/CHEBUZZF3/target.c | 36 +- src/main/target/CJMCU/target.c | 28 +- src/main/target/COLIBRI/target.c | 33 +- src/main/target/COLIBRI_RACE/target.c | 22 +- src/main/target/DOGE/target.c | 21 +- src/main/target/F4BY/target.c | 36 +- src/main/target/FURYF3/target.c | 19 +- src/main/target/FURYF4/target.c | 11 +- src/main/target/IMPULSERCF3/target.c | 16 +- src/main/target/IRCFUSIONF3/target.c | 34 +- src/main/target/ISHAPEDF3/target.c | 35 +- src/main/target/KISSFC/target.c | 24 +- src/main/target/KISSFC/target.h | 2 +- src/main/target/LUX_RACE/target.c | 22 +- src/main/target/MICROSCISKY/target.c | 28 +- src/main/target/MICROSCISKY/target.h | 1 - src/main/target/MOTOLAB/target.c | 18 +- src/main/target/MOTOLAB/target.h | 1 - src/main/target/NAZE/target.c | 28 +- src/main/target/NAZE/target.h | 2 - src/main/target/OMNIBUS/target.c | 20 +- src/main/target/OMNIBUS/target.h | 4 +- src/main/target/OMNIBUS/target.mk | 8 +- src/main/target/OMNIBUS/target.mk.orig | 9 +- src/main/target/OMNIBUSF4/target.c | 24 +- src/main/target/OMNIBUSF4/target.mk | 7 +- src/main/target/PIKOBLX/target.c | 18 +- src/main/target/RACEBASE/target.c | 11 +- src/main/target/RCEXPLORERF3/target.c | 12 +- src/main/target/RCEXPLORERF3/target.h | 1 - src/main/target/REVO/target.c | 24 +- src/main/target/REVO/target.h | 3 - src/main/target/REVONANO/target.c | 24 +- src/main/target/RMDO/target.c | 34 +- src/main/target/SINGULARITY/target.c | 20 +- src/main/target/SIRINFPV/target.c | 16 +- src/main/target/SIRINFPV/target.h | 2 + src/main/target/SIRINFPV/target.mk | 6 +- src/main/target/SOULF4/target.c | 24 +- src/main/target/SPARKY/target.c | 22 +- src/main/target/SPARKY2/target.c | 23 +- src/main/target/SPRACINGF3/target.c | 36 +- src/main/target/SPRACINGF3/target.h | 2 + src/main/target/SPRACINGF3/target.mk | 6 +- src/main/target/SPRACINGF3EVO/target.c | 24 +- src/main/target/SPRACINGF3MINI/target.c | 26 +- src/main/target/STM32F3DISCOVERY/target.c | 28 +- src/main/target/STM32F3DISCOVERY/target.mk | 8 +- src/main/target/VRRACE/target.c | 25 +- src/main/target/X_RACERSPI/target.c | 32 +- src/main/target/YUPIF4/target.c | 14 +- src/main/target/ZCOREF3/target.c | 36 +- src/main/target/common.h | 2 +- src/main/telemetry/smartport.c | 364 +++++++++++- src/main/vcpf4/usbd_cdc_vcp.c | 9 +- src/main/vcpf4/usbd_conf.h | 18 +- src/test/SpMsp.lua | 207 +++++++ 111 files changed, 2415 insertions(+), 1493 deletions(-) create mode 100644 src/main/io/cms_blackbox.c create mode 100644 src/main/io/cms_blackbox.h create mode 100644 src/main/io/cms_imu.c create mode 100644 src/main/io/cms_imu.h create mode 100644 src/main/io/cms_ledstrip.c create mode 100644 src/main/io/cms_ledstrip.h create mode 100644 src/main/io/cms_osd.h create mode 100644 src/main/io/cms_vtx.c create mode 100644 src/main/io/cms_vtx.h rename src/main/io/{display.c => dashboard.c} (91%) rename src/main/io/{display.h => dashboard.h} (72%) create mode 100644 src/test/SpMsp.lua diff --git a/Makefile b/Makefile index 983c51c904..c6ccb2b20f 100644 --- a/Makefile +++ b/Makefile @@ -561,7 +561,7 @@ HIGHEND_SRC = \ flight/gps_conversion.c \ io/gps.c \ io/ledstrip.c \ - io/display.c \ + io/dashboard.c \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ @@ -734,7 +734,11 @@ OPTIMIZE = -O0 LTO_FLAGS = $(OPTIMIZE) else OPTIMIZE = -Os +ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) +else +LTO_FLAGS = -fuse-linker-plugin $(OPTIMIZE) +endif endif DEBUG_FLAGS = -ggdb3 -DDEBUG diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 98d82919e4..6fdf6e5c59 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -75,7 +75,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh const float cs = cosf(omega); const float alpha = sn / (2 * Q); - float b0, b1, b2, a0, a1, a2; + float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0; switch (filterType) { case FILTER_LPF: diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index b4d4cfaa49..db41a01c7a 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -56,6 +56,12 @@ void EXTIInit(void) #if defined(STM32F3) || defined(STM32F4) /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); +#ifdef REMAP_TIM16_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM16, ENABLE); +#endif +#ifdef REMAP_TIM17_DMA + SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_TIM17, ENABLE); +#endif #endif memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 46f082a1dc..ea28c3e48e 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -156,7 +156,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { - uint32_t timerMhzCounter; + uint32_t timerMhzCounter = 0; pwmWriteFuncPtr pwmWritePtr; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool isDigital = false; @@ -208,7 +208,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot break; } - const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_MOTOR); if (timerHardware == NULL) { /* flag failure and disable ability to arm */ @@ -271,7 +271,7 @@ void servoInit(const servoConfig_t *servoConfig) IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_OUTPUT, RESOURCE_INDEX(servoIndex)); IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); - const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_SERVO); if (timer == NULL) { /* flag failure and disable ability to arm */ diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 80903ce19f..e72e03234c 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -84,7 +84,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } @@ -123,18 +123,18 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; @@ -146,20 +146,15 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_Low; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } + TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 7e60c62f1d..d2bf798187 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -85,7 +85,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { + for (int i = 0; i < dmaMotorTimerCount; i++) { TIM_SetCounter(dmaMotorTimers[i].timer, 0); TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); } @@ -124,18 +124,18 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); - uint32_t hz; - switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): - hz = MOTOR_DSHOT600_MHZ * 1000000; - break; - case(PWM_TYPE_DSHOT300): - hz = MOTOR_DSHOT300_MHZ * 1000000; - break; - default: - case(PWM_TYPE_DSHOT150): - hz = MOTOR_DSHOT150_MHZ * 1000000; - } + uint32_t hz; + switch (pwmProtocolType) { + case(PWM_TYPE_DSHOT600): + hz = MOTOR_DSHOT600_MHZ * 1000000; + break; + case(PWM_TYPE_DSHOT300): + hz = MOTOR_DSHOT300_MHZ * 1000000; + break; + default: + case(PWM_TYPE_DSHOT150): + hz = MOTOR_DSHOT150_MHZ * 1000000; + } TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1; TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; @@ -145,12 +145,15 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + } TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index f055683976..13f074fad1 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -393,7 +393,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) pwmInputPort_t *port = &pwmInputPorts[channel]; - const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM); if (!timer) { /* TODO: maybe fail here if not enough channels? */ @@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IO_t io = IOGetByTag(pwmConfig->ioTags[channel]); IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); @@ -459,7 +459,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT]; - const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIMER_INPUT_ENABLED); + const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_PPM); if (!timer) { /* TODO: fail here? */ return; @@ -472,7 +472,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) IO_t io = IOGetByTag(ppmConfig->ioTag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); - IOConfigGPIO(io, timer->ioMode); + IOConfigGPIO(io, IOCFG_IPD); #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 7dfef2033f..77096a1cb7 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -199,7 +199,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -686,14 +686,14 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } #if defined(STM32F3) || defined(STM32F4) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -755,14 +755,11 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].output & flag) == flag) { - return &timerHardware[i]; - } else if (!flag && timerHardware[i].output == flag) { - // TODO: shift flag by one so not to be 0 + if (timerHardware[i].usageFlags & flag) { return &timerHardware[i]; } } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index cf4d12b220..9a86a92101 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -54,6 +54,13 @@ typedef uint32_t timCNT_t; #error "Unknown CPU defined" #endif +typedef enum { + TIM_USE_PPM = 0x1, + TIM_USE_PWM = 0x2, + TIM_USE_MOTOR = 0x4, + TIM_USE_SERVO = 0x8, + TIM_USE_LED = 0x10 +} timerUsageFlag_e; // use different types from capture and overflow - multiple overflow handlers are implemented as linked list struct timerCCHandlerRec_s; @@ -80,8 +87,8 @@ typedef struct timerHardware_s { ioTag_t tag; uint8_t channel; uint8_t irq; + timerUsageFlag_e usageFlags; uint8_t output; - ioConfig_t ioMode; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint8_t alternateFunction; #endif @@ -171,7 +178,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - rccPeriphTag_t timerRCC(TIM_TypeDef *tim); -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag); +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim); diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 05c01a5210..8268bd3497 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -208,7 +208,7 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { - for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { if (timerDefinitions[i].TIMx == tim) { return timerDefinitions[i].rcc; } @@ -787,14 +787,14 @@ void timerInit(void) #endif /* enable the timer peripherals */ - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { RCC_ClockCmd(timerRCC(timerHardware[i].tim), ENABLE); } #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) - for (uint8_t timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { + for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); + IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif @@ -856,16 +856,13 @@ void timerForceOverflow(TIM_TypeDef *tim) } } -const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { - for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { - if (flag && (timerHardware[i].output & flag) == flag) { + if (timerHardware[i].output & flag) { return &timerHardware[i]; - } else if (!flag && timerHardware[i].output == flag) { - // TODO: shift flag by one so not to be 0 - return &timerHardware[i]; - } + } } } return NULL; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 7d5272733c..cbe715a4af 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -244,6 +244,14 @@ void resetServoConfig(servoConfig_t *servoConfig) { servoConfig->servoCenterPulse = 1500; servoConfig->servoPwmRate = 50; + + int servoIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_SERVO) { + servoConfig->ioTags[servoIndex] = timerHardware[i].tag; + servoIndex++; + } + } } #endif @@ -263,9 +271,9 @@ void resetMotorConfig(motorConfig_t *motorConfig) motorConfig->mincommand = 1000; motorConfig->digitalIdleOffset = 40; - uint8_t motorIndex = 0; - for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { - if ((timerHardware[i].output & TIMER_OUTPUT_ENABLED) == TIMER_OUTPUT_ENABLED) { + int motorIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { motorConfig->ioTags[motorIndex] = timerHardware[i].tag; motorIndex++; } @@ -305,7 +313,7 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) ppmConfig->ioTag = IO_TAG(PPM_PIN); #else for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if (timerHardware[i].usageFlags & TIM_USE_PPM) { ppmConfig->ioTag = timerHardware[i].tag; return; } @@ -317,9 +325,9 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) void resetPwmConfig(pwmConfig_t *pwmConfig) { - uint8_t inputIndex = 0; + int inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { - if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + if (timerHardware[i].usageFlags & TIM_USE_PWM) { pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; inputIndex++; } @@ -900,8 +908,8 @@ void validateAndFixConfig(void) #endif #if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3) - if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) { - featureClear(FEATURE_DISPLAY); + if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DASHBOARD)) { + featureClear(FEATURE_DASHBOARD); } #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 60c2f517c6..5da0c5b53b 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -45,7 +45,7 @@ typedef enum { FEATURE_RX_MSP = 1 << 14, FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, - FEATURE_DISPLAY = 1 << 17, + FEATURE_DASHBOARD = 1 << 17, FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3119e27bc9..b92507b1af 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1778,17 +1778,3 @@ void mspFcInit(void) { initActiveBoxIds(); } - -void mspServerPush(mspPacket_t *push, uint8_t *data, int len) -{ - sbuf_t *dst = &push->buf; - - while (len--) { - sbufWriteU8(dst, *data++); - } -} - -mspPushCommandFnPtr mspFcPushInit(void) -{ - return mspServerPush; -} diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index df317ab3a1..53edbd8ed1 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -21,5 +21,3 @@ void mspFcInit(void); mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); - -mspPushCommandFnPtr mspFcPushInit(void); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b5cc16f7e5..9be44fe891 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -41,9 +41,10 @@ #include "flight/altitudehold.h" #include "io/cms.h" +#include "io/cms_types.h" #include "io/beeper.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" @@ -225,11 +226,11 @@ static void taskCalculateAltitude(uint32_t currentTime) }} #endif -#ifdef DISPLAY -static void taskUpdateDisplay(uint32_t currentTime) +#ifdef USE_DASHBOARD +static void taskUpdateDashboard(uint32_t currentTime) { - if (feature(FEATURE_DISPLAY)) { - displayUpdate(currentTime); + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(currentTime); } } #endif @@ -323,8 +324,8 @@ void fcTasksInit(void) #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif -#ifdef DISPLAY - setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); +#ifdef USE_DASHBOARD + setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); @@ -469,10 +470,10 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef DISPLAY - [TASK_DISPLAY] = { - .taskName = "DISPLAY", - .taskFunc = taskUpdateDisplay, +#ifdef USE_DASHBOARD + [TASK_DASHBOARD] = { + .taskName = "DASHBOARD", + .taskFunc = taskUpdateDashboard, .desiredPeriod = 1000000 / 10, .staticPriority = TASK_PRIORITY_LOW, }, diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 1d410a9435..016c4287ec 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -46,7 +46,7 @@ #include "io/beeper.h" #include "io/motors.h" #include "io/vtx.h" -#include "io/display.h" +#include "io/dashboard.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -293,13 +293,13 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } -#ifdef DISPLAY +#ifdef USE_DASHBOARD if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { - displayDisablePageCycling(); + dashboardDisablePageCycling(); } if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { - displayEnablePageCycling(); + dashboardEnablePageCycling(); } #endif diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c index 97b12f03d9..f2d0eeed15 100644 --- a/src/main/io/canvas.c +++ b/src/main/io/canvas.c @@ -9,6 +9,8 @@ #ifdef CANVAS +#include "common/utils.h" + #include "drivers/system.h" #include "io/cms.h" @@ -67,28 +69,36 @@ int canvasWrite(uint8_t col, uint8_t row, char *string) return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); } -screenFnVTable_t canvasVTable = { +void canvasResync(displayPort_t *pPort) +{ + pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future + pPort->rows = 30; +} + +uint32_t canvasTxBytesFree(void) +{ + return mspSerialTxBytesFree(); +} + +displayPortVTable_t canvasVTable = { canvasBegin, canvasEnd, canvasClear, canvasWrite, canvasHeartBeat, - NULL, + canvasResync, + canvasTxBytesFree, }; void canvasCmsInit(displayPort_t *pPort) { - pPort->rows = 13; + pPort->rows = 13; // XXX Will reflect NTSC/PAL in the future pPort->cols = 30; - pPort->buftime = 23; // = 256/(115200/10) - pPort->bufsize = 192; // 256 * 3/4 (Be conservative) - pPort->VTable = &canvasVTable; + pPort->vTable = &canvasVTable; } void canvasInit() { - mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp - cmsDeviceRegister(canvasCmsInit); } #endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h index c7f96207ca..d288656f6a 100644 --- a/src/main/io/canvas.h +++ b/src/main/io/canvas.h @@ -1,3 +1,3 @@ #pragma once void canvasInit(void); -void canvasCmsInit(displayPort_t *); +void canvasCmsInit(displayPort_t *dPort); diff --git a/src/main/io/cms.c b/src/main/io/cms.c index 76e01a76b4..d4504f5d9d 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -37,13 +37,22 @@ #include "io/cms.h" #include "io/cms_types.h" + #ifdef CANVAS #include "io/canvas.h" #endif +#ifdef USE_FLASHFS #include "io/flashfs.h" +#endif + +#ifdef OSD #include "io/osd.h" -#include "io/display.h" +#endif + +#ifdef USE_DASHBOARD +#include "io/dashboard.h" +#endif #include "fc/config.h" #include "fc/rc_controls.h" @@ -55,10 +64,26 @@ #include "config/config_master.h" #include "config/feature.h" -#include "io/cms.h" - #include "build/debug.h" +// External menu contents +#include "io/cms_imu.h" +#include "io/cms_blackbox.h" +#include "io/cms_vtx.h" +#ifdef OSD +#include "io/cms_osd.h" +#endif // OSD +#include "io/cms_ledstrip.h" +#ifdef VTX_SMARTAUDIO +#include "io/vtx_smartaudio.h" +#endif + + +// Forwards +void cmsx_InfoInit(void); +void cmsx_FeatureRead(void); +void cmsx_FeatureWriteback(void); + // Device management #define CMS_MAX_DEVICE 4 @@ -107,37 +132,40 @@ int8_t lastCursorPos; void cmsScreenClear(displayPort_t *instance) { - instance->VTable->clear(); + instance->vTable->clear(); instance->cleared = true; lastCursorPos = -1; // XXX Here } void cmsScreenBegin(displayPort_t *instance) { - instance->VTable->begin(); - instance->VTable->clear(); + instance->vTable->begin(); + instance->vTable->clear(); } void cmsScreenEnd(displayPort_t *instance) { - instance->VTable->end(); + instance->vTable->end(); } int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s) { - return instance->VTable->write(x, y, s); + return instance->vTable->write(x, y, s); } void cmsScreenHeartBeat(displayPort_t *instance) { - if (instance->VTable->heartbeat) - instance->VTable->heartbeat(); + instance->vTable->heartbeat(); } void cmsScreenResync(displayPort_t *instance) { - if (instance->VTable->resync) - instance->VTable->resync(); + instance->vTable->resync(instance); +} + +uint16_t cmsScreenTxBytesFree(displayPort_t *instance) +{ + return instance->vTable->txBytesFree(); } void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) @@ -163,9 +191,11 @@ void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) // #define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN(p) ((p)->cols - 7) +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) #define MAX_MENU_ITEMS(p) ((p)->rows - 2) +displayPort_t currentDisplay; + bool cmsInMenu = false; OSD_Entry *menuStack[10]; //tab to save menu stack @@ -209,7 +239,7 @@ void cmsUpdateMaxRows(displayPort_t *instance) currentMenuIdx--; } -static void cmsFtoa(int32_t value, char *floatString) +static void cmsFormatFloat(int32_t value, char *floatString) { uint8_t k; // np. 3450 @@ -224,7 +254,8 @@ static void cmsFtoa(int32_t value, char *floatString) // 03.450 // usuwam koncowe zera i kropke - for (k = 5; k > 1; k--) + // Keep the first decimal place + for (k = 5; k > 3; k--) if (floatString[k] == '0' || floatString[k] == '.') floatString[k] = 0; else @@ -257,6 +288,12 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr int cnt = 0; switch (p->type) { + case OME_String: + if (IS_PRINTVALUE(p) && p->data) { + cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + CLR_PRINTVALUE(p); + } + break; case OME_Submenu: if (IS_PRINTVALUE(p)) { cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); @@ -282,8 +319,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr } break; } - case OME_VISIBLE: #ifdef OSD + case OME_VISIBLE: if (IS_PRINTVALUE(p) && p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -297,8 +334,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr } CLR_PRINTVALUE(p); } -#endif break; +#endif case OME_UINT8: if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; @@ -348,7 +385,7 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr case OME_FLOAT: if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; - cmsFtoa(*ptr->val * ptr->multipler, buff); + cmsFormatFloat(*ptr->val * ptr->multipler, buff); cmsPad(buff, 5); cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? CLR_PRINTVALUE(p); @@ -364,10 +401,8 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr return cnt; } -void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) +void cmsDrawMenu(displayPort_t *pDisplay) { - UNUSED(currentTime); - uint8_t i; OSD_Entry *p; uint8_t top = (pDisplay->rows - currentMenuIdx) / 2 - 1; @@ -377,7 +412,7 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) static uint8_t pollDenom = 0; bool drawPolled = (++pollDenom % 8 == 0); - int16_t cnt = 0; + uint32_t room = cmsScreenTxBytesFree(pDisplay); if (!currentMenu) return; @@ -404,20 +439,26 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) currentCursorPos++; if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) { - cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " "); } + if (room < 30) + return; + if (lastCursorPos != currentCursorPos) { - cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >"); lastCursorPos = currentCursorPos; } + if (room < 30) + return; + // Print text labels for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p)) { - cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); + room -= cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text); CLR_PRINTLABEL(p); - if (cnt > pDisplay->batchsize) + if (room < 30) return; } } @@ -425,33 +466,25 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime) // Print values // XXX Polled values at latter positions in the list may not be - // XXX printed if the cnt exceeds batchsize in the middle of the list. + // XXX printed if not enough room in the middle of the list. for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { if (IS_PRINTVALUE(p)) { - cnt += cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); - if (cnt > pDisplay->batchsize) + room -= cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled); + if (room < 30) return; } } } -// XXX Needs separation -OSD_Entry menuPid[]; -void cmsx_PidRead(void); -void cmsx_PidWriteback(void); -OSD_Entry menuRateExpo[]; -void cmsx_RateExpoRead(void); -void cmsx_RateExpoWriteback(void); - long cmsMenuChange(displayPort_t *pDisplay, void *ptr) { if (ptr) { // XXX (jflyper): This can be avoided by adding pre- and post- // XXX (or onEnter and onExit) functions? - if (ptr == &menuPid[0]) + if (ptr == cmsx_menuPid) cmsx_PidRead(); - if (ptr == &menuRateExpo[0]) + if (ptr == cmsx_menuRateExpo) cmsx_RateExpoRead(); if ((OSD_Entry *)ptr != currentMenu) { @@ -474,11 +507,11 @@ long cmsMenuBack(displayPort_t *pDisplay) { // becasue pids and rates may be stored in profiles we need some thicks to manipulate it // hack to save pid profile - if (currentMenu == &menuPid[0]) + if (currentMenu == cmsx_menuPid) cmsx_PidWriteback(); // hack - save rate config for current profile - if (currentMenu == &menuRateExpo[0]) + if (currentMenu == cmsx_menuRateExpo) cmsx_RateExpoWriteback(); if (menuStackIdx) { @@ -494,19 +527,6 @@ long cmsMenuBack(displayPort_t *pDisplay) return 0; } -// XXX This should go to device -void cmsComputeBatchsize(displayPort_t *pDisplay) -{ - pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime; -} - -// XXX Separation -void cmsx_FeatureRead(void); -void cmsx_FeatureWriteback(void); -void cmsx_InfoInit(void); - -displayPort_t currentDisplay; - void cmsMenuOpen(void) { cmsDeviceInitFuncPtr initfunc; @@ -528,7 +548,6 @@ void cmsMenuOpen(void) return; cmsScreenInit(¤tDisplay, initfunc); - cmsComputeBatchsize(¤tDisplay); cmsScreenBegin(¤tDisplay); cmsMenuChange(¤tDisplay, currentMenu); } @@ -640,8 +659,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; - case OME_VISIBLE: #ifdef OSD + case OME_VISIBLE: if (p->data) { uint32_t address = (uint32_t)p->data; uint16_t *val; @@ -654,8 +673,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *val %= ~VISIBLE_FLAG; SET_PRINTVALUE(p); } -#endif break; +#endif case OME_UINT8: case OME_FLOAT: if (p->data) { @@ -730,6 +749,8 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; + case OME_String: + break; case OME_Poll_INT16: case OME_Label: case OME_END: @@ -738,8 +759,6 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -OSD_Entry menuRc[]; - void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) { static int16_t rcDelay = BUTTON_TIME; @@ -779,7 +798,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) key = KEY_RIGHT; rcDelay = BUTTON_TIME; } - else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can't exit using YAW + else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW { key = KEY_ESC; rcDelay = BUTTON_TIME; @@ -792,7 +811,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) return; } - cmsDrawMenu(pDisplay, currentTime); + cmsDrawMenu(pDisplay); if (currentTime > lastCmsHeartBeat + 500) { // Heart beat for external CMS display device @ 500msec @@ -804,15 +823,13 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime) lastCalled = currentTime; } -void cmsHandler(uint32_t unusedTime) +void cmsHandler(uint32_t currentTime) { - UNUSED(unusedTime); - if (cmsDeviceCount < 0) return; static uint32_t lastCalled = 0; - uint32_t now = millis(); + const uint32_t now = currentTime / 1000; if (now - lastCalled >= CMS_UPDATE_INTERVAL) { cmsUpdate(¤tDisplay, now); @@ -826,269 +843,37 @@ void cmsInit(void) } // -// Menu tables, should eventually be all GONE!? +// Menu contents // -// -// IMU -// +// Info -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; +static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; +static char infoTargetName[] = __TARGET__; -uint8_t tempPid[4][3]; +#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere -static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; -static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; -static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; +OSD_Entry menuInfo[] = { + { "--- INFO ---", OME_Label, NULL, NULL, 0 }, + { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 }, + { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 }, + { "GITREV", OME_String, NULL, infoGitRev, 0 }, + { "TARGET", OME_String, NULL, infoTargetName, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; -static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; -static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; -static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; - -static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; -static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; -static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; - -void cmsx_PidRead(void) +void cmsx_InfoInit(void) { - uint8_t i; - - for (i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; + for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { + if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') + infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; + else + infoGitRev[i] = shortGitRevision[i]; } - tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]; } -void cmsx_PidWriteback(void) -{ - uint8_t i; - - for (i = 0; i < 3; i++) { - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; - } - - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; -} - -OSD_Entry menuPid[] = -{ - {"--- PID ---", OME_Label, NULL, NULL, 0}, - {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, - {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, - {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, - - {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, - {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, - {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, - - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Rate & Expo -// -controlRateConfig_t rateProfile; - -static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; -static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; -static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; -static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcYawRate = {&rateProfile.rcYawRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; -static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; -static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; -static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; - -void cmsx_RateExpoRead() -{ - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); -} - -void cmsx_RateExpoWriteback() -{ - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); -} - -OSD_Entry menuRateExpo[] = -{ - {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, - {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, - {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, - {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, - {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, - {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, - {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, - {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, - {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, - {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// RC preview -// -static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; -static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; -static OSD_INT16_t entryRcThr = {&rcData[THROTTLE], 1, 2500, 0}; -static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; -static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; -static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; -static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; -static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; - -OSD_Entry menuRc[] = -{ - {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, - {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, - {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, - {"THR", OME_Poll_INT16, NULL, &entryRcThr, 0}, - {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, - {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, - {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, - {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, - {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Misc -// -OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; -OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; -OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; -OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; -OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; - -OSD_Entry menuMisc[]= -{ - {"--- MISC ---", OME_Label, NULL, NULL, 0}, - {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, - {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, - {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, - {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_Entry menuImu[] = -{ - {"--- CFG. IMU ---", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, - {"PID", OME_Submenu, cmsMenuChange, &menuPid[0], 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, &menuRateExpo[0], 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, &menuRc[0], 0}, - {"MISC", OME_Submenu, cmsMenuChange, &menuMisc[0], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Black box -// - -// -// Should goto flashfs eventually. -// -#ifdef USE_FLASHFS -void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) -{ - UNUSED(ptr); - - cmsScreenClear(pDisplay); - cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH..."); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - - cmsScreenClear(pDisplay); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? -} -#endif // USE_FLASHFS - -uint8_t featureBlackbox = 0; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox, 0}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// VTX -// -#if defined(VTX) || defined(USE_RTC6705) - -uint8_t featureVtx = 0, vtxBand, vtxChannel; - -static const char * const vtxBandNames[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -OSD_TAB_t entryVtxBand = {&vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&vtxChannel, 1, 8, 1}; - -#ifdef VTX -OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; -OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; -#endif // VTX - -OSD_Entry menu_vtx[] = -{ - {"--- VTX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureVtx, 0}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; -#endif // VTX || USE_RTC6705 - +#if 0 #ifdef VTX_SMARTAUDIO #include "io/vtx_smartaudio.h" @@ -1194,191 +979,40 @@ OSD_Entry menu_vtxSmartAudio[] = { NULL, OME_END, NULL, NULL, 0 } }; #endif // VTX_SMARTAUDIO +#endif // 0 -// -// LED_STRIP -// -#ifdef LED_STRIP - -//local variable to keep color value -uint8_t ledColor; - -static const char * const LED_COLOR_NAMES[] = { - "BLACK ", - "WHITE ", - "RED ", - "ORANGE ", - "YELLOW ", - "LIME GRN", - "GREEN ", - "MINT GRN", - "CYAN ", - "LT BLUE ", - "BLUE ", - "DK VIOLT", - "MAGENTA ", - "DEEP PNK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -static void getLedColor(void) -{ - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -static long applyLedColor(displayPort_t *pDisplay, void *ptr) -{ - UNUSED(ptr); - UNUSED(pDisplay); // Arrgh - - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) - *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); - } - - return 0; -} - -static uint8_t featureLedstrip; - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -OSD_Entry menuLedstrip[] = -{ - {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip, 0}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; -#endif // LED_STRIP - -#ifdef OSD -// -// OSD specific menu pages and items -// XXX Should be part of the osd.c, or new osd_cms.c. -// - -OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; -OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; -OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; -OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; - -OSD_Entry menuAlarms[] = -{ - {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, - {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, - {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, - {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, -#ifdef VTX - {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, - {"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; -#endif // OSD - -// -// Info -// -static char infoGitRev[GIT_SHORT_REVISION_LENGTH]; -static char infoTargetName[] = __TARGET__; - -#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere - -OSD_Entry menuInfo[] = { - { "--- INFO ---", OME_Label, NULL, NULL, 0 }, - { BETAFLIGHT_IDENTIFIER, OME_Label, NULL, NULL, 0 }, - { FC_VERSION_STRING, OME_Label, NULL, NULL, 0 }, - { infoGitRev, OME_Label, NULL, NULL, 0 }, - { infoTargetName, OME_Label, NULL, NULL, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -void cmsx_InfoInit(void) -{ - int i; - for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) { - if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') - infoGitRev[i] = shortGitRevision[i] - 'a' + 'A'; - else - infoGitRev[i] = shortGitRevision[i]; - } -} +// Features OSD_Entry menuFeatures[] = { {"--- FEATURES ---", OME_Label, NULL, NULL, 0}, - {"BLACKBOX", OME_Submenu, cmsMenuChange, &menuBlackbox[0], 0}, + {"BLACKBOX", OME_Submenu, cmsMenuChange, cmsx_menuBlackbox, 0}, #if defined(VTX) || defined(USE_RTC6705) - {"VTX", OME_Submenu, cmsMenuChange, &menu_vtx[0], 0}, + {"VTX", OME_Submenu, cmsMenuChange, cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 #if defined(VTX_SMARTAUDIO) - {"VTX", OME_Submenu, cmsMenuChange, &menu_vtxSmartAudio[0], 0}, + {"VTX", OME_Submenu, cmsMenuChange, cmsx_menuVtxSmartAudio, 0}, #endif #ifdef LED_STRIP - {"LED STRIP", OME_Submenu, cmsMenuChange, &menuLedstrip[0], 0}, + {"LED STRIP", OME_Submenu, cmsMenuChange, cmsx_menuLedstrip, 0}, #endif // LED_STRIP {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; +// Main + OSD_Entry menuMain[] = { {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0}, - {"CFG&IMU", OME_Submenu, cmsMenuChange, &menuImu[0], 0}, - {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures[0], 0}, + {"CFG&IMU", OME_Submenu, cmsMenuChange, cmsx_menuImu, 0}, + {"FEATURES", OME_Submenu, cmsMenuChange, menuFeatures, 0}, #ifdef OSD - {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &menuOsdLayout[0], 0}, - {"ALARMS", OME_Submenu, cmsMenuChange, &menuAlarms[0], 0}, + {"SCR LAYOUT", OME_Submenu, cmsMenuChange, cmsx_menuOsdLayout, 0}, + {"ALARMS", OME_Submenu, cmsMenuChange, cmsx_menuAlarms, 0}, #endif - {"INFO", OME_Submenu, cmsMenuChange, &menuInfo[0], 0}, + {"FC&FW INFO", OME_Submenu, cmsMenuChange, menuInfo, 0}, {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0}, {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0}, {NULL,OME_END, NULL, NULL, 0} @@ -1386,60 +1020,33 @@ OSD_Entry menuMain[] = void cmsx_FeatureRead(void) { - featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; + cmsx_Blackbox_FeatureRead(); #ifdef LED_STRIP - featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; - getLedColor(); + cmsx_Ledstrip_FeatureRead(); + cmsx_Ledstrip_ConfigRead(); #endif #if defined(VTX) || defined(USE_RTC6705) - featureVtx = feature(FEATURE_VTX) ? 1 : 0; + cmsx_Vtx_FeatureRead(); + cmsx_Vtx_ConfigRead(); #endif // VTX || USE_RTC6705 - -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX - -#ifdef USE_RTC6705 - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 } void cmsx_FeatureWriteback(void) { - if (featureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); + cmsx_Blackbox_FeatureWriteback(); #ifdef LED_STRIP - if (featureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); + cmsx_Ledstrip_FeatureWriteback(); #endif #if defined(VTX) || defined(USE_RTC6705) - if (featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); + cmsx_Vtx_FeatureWriteback(); + cmsx_Vtx_ConfigWriteback(); #endif // VTX || USE_RTC6705 -#ifdef VTX - masterConfig.vtxBand = vtxBand; - masterConfig.vtx_channel = vtxChannel - 1; -#endif // VTX - -#ifdef USE_RTC6705 - masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; -#endif // USE_RTC6705 - saveConfigAndNotify(); } - #endif // CMS diff --git a/src/main/io/cms.h b/src/main/io/cms.h index a9ab352cd3..a7d0d21e08 100644 --- a/src/main/io/cms.h +++ b/src/main/io/cms.h @@ -1,40 +1,44 @@ #pragma once -typedef struct screenFnVTable_s { +struct displayPort_s; + +typedef struct displayPortVTable_s { int (*begin)(void); int (*end)(void); int (*clear)(void); - int (*write)(uint8_t, uint8_t, char *); + int (*write)(uint8_t col, uint8_t row, char *text); int (*heartbeat)(void); - void (*resync)(void); -} screenFnVTable_t; + void (*resync)(struct displayPort_s *pPort); + uint32_t (*txBytesFree)(void); +} displayPortVTable_t; typedef struct displayPort_s { + displayPortVTable_t *vTable; uint8_t rows; uint8_t cols; uint16_t buftime; uint16_t bufsize; - uint16_t batchsize; // Computed by CMS - screenFnVTable_t *VTable; // CMS state bool cleared; } displayPort_t; // Device management -typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *); +typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *pPort); bool cmsDeviceRegister(cmsDeviceInitFuncPtr); // For main.c and scheduler void cmsInit(void); -void cmsHandler(uint32_t); +void cmsHandler(uint32_t currentTime); // Required for external CMS tables +void cmsScreenClear(displayPort_t *pPort); +void cmsScreenResync(displayPort_t *pPort); +int cmsScreenWrite(displayPort_t *pPort, uint8_t x, uint8_t y, char *s); -long cmsChangeScreen(displayPort_t *, void *); -long cmsExitMenu(displayPort_t *, void *); - -#define STARTUP_HELP_TEXT1 "MENU: THR MID" -#define STARTUP_HELP_TEXT2 "+ YAW LEFT" -#define STARTUP_HELP_TEXT3 "+ PITCH UP" +long cmsMenuChange(displayPort_t *pPort, void *ptr); +long cmsMenuExit(displayPort_t *pPort, void *ptr); +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c new file mode 100644 index 0000000000..3f118f87ee --- /dev/null +++ b/src/main/io/cms_blackbox.c @@ -0,0 +1,77 @@ +// +// CMS things for blackbox and flashfs. +// Should be part of blackbox.c (or new blackbox/blackbox_cms.c) and io/flashfs.c +// +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_blackbox.h" + +#include "io/flashfs.h" + +#ifdef USE_FLASHFS +long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) +{ + UNUSED(ptr); + + cmsScreenClear(pDisplay); + cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH..."); + cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + + cmsScreenClear(pDisplay); + cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? + + return 0; +} +#endif // USE_FLASHFS + +uint8_t cmsx_FeatureBlackbox; + +OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; + +OSD_Entry cmsx_menuBlackbox[] = +{ + {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, +#ifdef USE_FLASHFS + {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, +#endif // USE_FLASHFS + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +void cmsx_Blackbox_FeatureRead(void) +{ + cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; +} + +void cmsx_Blackbox_FeatureWriteback(void) +{ + if (cmsx_FeatureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); +} +#endif diff --git a/src/main/io/cms_blackbox.h b/src/main/io/cms_blackbox.h new file mode 100644 index 0000000000..6e7d02eff2 --- /dev/null +++ b/src/main/io/cms_blackbox.h @@ -0,0 +1,4 @@ +extern OSD_Entry cmsx_menuBlackbox[]; + +void cmsx_Blackbox_FeatureRead(void); +void cmsx_Blackbox_FeatureWriteback(void); diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c new file mode 100644 index 0000000000..9428d993d6 --- /dev/null +++ b/src/main/io/cms_imu.c @@ -0,0 +1,206 @@ + +// Menu contents for PID, RATES, RC preview, misc +// Should be part of the relevant .c file. + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +//#include "common/typeconversion.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; + +uint8_t tempPid[4][3]; + +static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; +static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; +static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; + +static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; +static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; +static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; + +static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; +static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; +static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; + +void cmsx_PidRead(void) +{ + uint8_t i; + + for (i = 0; i < 3; i++) { + tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; + tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; + tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; + } + tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]; +} + +void cmsx_PidWriteback(void) +{ + uint8_t i; + + for (i = 0; i < 3; i++) { + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; + } + + masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; +} + +OSD_Entry cmsx_menuPid[] = +{ + {"--- PID ---", OME_Label, NULL, NULL, 0}, + {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, + {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, + {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, + + {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, + + {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, + {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, + {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, + + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// Rate & Expo +// +controlRateConfig_t rateProfile; + +static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; +static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; +static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; +static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; +static OSD_FLOAT_t entryRcYawRate = {&rateProfile.rcYawRate8, 0, 200, 1, 10}; +static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; +static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; +static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; +static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; +static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; +static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; + +void cmsx_RateExpoRead() +{ + memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); +} + +void cmsx_RateExpoWriteback() +{ + memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); +} + +OSD_Entry cmsx_menuRateExpo[] = +{ + {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, + {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, + {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, + {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, + {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, + {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, + {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, + {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, + {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, + {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// RC preview +// +static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; +static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; +static OSD_INT16_t entryRcThr = {&rcData[THROTTLE], 1, 2500, 0}; +static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; +static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; +static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; +static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; +static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; + +OSD_Entry cmsx_menuRc[] = +{ + {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, + {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, + {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, + {"THR", OME_Poll_INT16, NULL, &entryRcThr, 0}, + {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, + {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, + {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, + {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, + {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +// +// Misc +// +OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; +OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; +OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; +OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; +OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; +OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; +OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; + +OSD_Entry menuImuMisc[]= +{ + {"--- MISC ---", OME_Label, NULL, NULL, 0}, + {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, + {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, + {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, + {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry cmsx_menuImu[] = +{ + {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, + {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, + {"PID", OME_Submenu, cmsMenuChange, cmsx_menuPid, 0}, + {"RATE&RXPO", OME_Submenu, cmsMenuChange, cmsx_menuRateExpo, 0}, + {"RC PREV", OME_Submenu, cmsMenuChange, cmsx_menuRc, 0}, + {"MISC", OME_Submenu, cmsMenuChange, menuImuMisc, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; +#endif // CMS diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h new file mode 100644 index 0000000000..b78c6ddb4b --- /dev/null +++ b/src/main/io/cms_imu.h @@ -0,0 +1,13 @@ +extern OSD_Entry cmsx_menuImu[]; + +// All of below should be gone. + +extern OSD_Entry cmsx_menuPid[]; +extern OSD_Entry cmsx_menuRc[]; +extern OSD_Entry cmsx_menuRateExpo[]; + +void cmsx_PidRead(void); +void cmsx_PidWriteback(void); +void cmsx_RateExpoRead(void); +void cmsx_RateExpoWriteback(void); + diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c new file mode 100644 index 0000000000..63e15d74e6 --- /dev/null +++ b/src/main/io/cms_ledstrip.c @@ -0,0 +1,107 @@ +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CMS + +#include "drivers/system.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_blackbox.h" + +#ifdef LED_STRIP + +//local variable to keep color value +uint8_t ledColor; + +static const char * const LED_COLOR_NAMES[] = { + "BLACK ", + "WHITE ", + "RED ", + "ORANGE ", + "YELLOW ", + "LIME GRN", + "GREEN ", + "MINT GRN", + "CYAN ", + "LT BLUE ", + "BLUE ", + "DK VIOLT", + "MAGENTA ", + "DEEP PNK" +}; + +//find first led with color flag and restore color index +//after saving all leds with flags color will have color set in OSD +void cmsx_GetLedColor(void) +{ + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + int fn = ledGetFunction(ledConfig); + + if (fn == LED_FUNCTION_COLOR) { + ledColor = ledGetColor(ledConfig); + break; + } + } +} + +//udate all leds with flag color +static long applyLedColor(displayPort_t *pDisplay, void *ptr) +{ + UNUSED(ptr); + UNUSED(pDisplay); // Arrgh + + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) + *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); + } + + return 0; +} + +uint8_t cmsx_FeatureLedstrip; + +OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; + +OSD_Entry cmsx_menuLedstrip[] = +{ + {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +void cmsx_Ledstrip_FeatureRead(void) +{ + cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; +} + +void cmsx_Ledstrip_FeatureWriteback(void) +{ + if (cmsx_FeatureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); +} + +void cmsx_Ledstrip_ConfigRead(void) +{ + cmsx_GetLedColor(); +} + +#endif // LED_STRIP +#endif // CMS diff --git a/src/main/io/cms_ledstrip.h b/src/main/io/cms_ledstrip.h new file mode 100644 index 0000000000..1ce1d5009f --- /dev/null +++ b/src/main/io/cms_ledstrip.h @@ -0,0 +1,6 @@ +extern OSD_Entry cmsx_menuLedstrip[]; + +void cmsx_Ledstrip_FeatureRead(void); +void cmsx_Ledstrip_FeatureWriteback(void); + +void cmsx_Ledstrip_ConfigRead(void); diff --git a/src/main/io/cms_osd.h b/src/main/io/cms_osd.h new file mode 100644 index 0000000000..5c93a65215 --- /dev/null +++ b/src/main/io/cms_osd.h @@ -0,0 +1,2 @@ +extern OSD_Entry cmsx_menuAlarms[]; +extern OSD_Entry cmsx_menuOsdLayout[]; diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h index df68379ebf..973b6610b7 100644 --- a/src/main/io/cms_types.h +++ b/src/main/io/cms_types.h @@ -8,6 +8,7 @@ typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *); //type of elements + typedef enum { OME_Label, @@ -20,9 +21,12 @@ typedef enum OME_UINT16, OME_INT16, OME_Poll_INT16, + OME_String, OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's //wlasciwosci elementow +#ifdef OSD OME_VISIBLE, +#endif OME_TAB, OME_END, } OSD_MenuElement; @@ -95,3 +99,8 @@ typedef struct uint8_t max; const char * const *names; } OSD_TAB_t; + +typedef struct +{ + char *val; +} OSD_String_t; diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c new file mode 100644 index 0000000000..f133f4e7f1 --- /dev/null +++ b/src/main/io/cms_vtx.c @@ -0,0 +1,96 @@ +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#include "io/cms.h" +#include "io/cms_types.h" +#include "io/cms_vtx.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + +uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; + +static const char * const vtxBandNames[] = { + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; +OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; + +#ifdef VTX +OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; +OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; +#endif // VTX + +OSD_Entry cmsx_menuVtx[] = +{ + {"--- VTX ---", OME_Label, NULL, NULL, 0}, + {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, +#ifdef USE_RTC6705 + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +void cmsx_Vtx_FeatureRead(void) +{ + cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; +} + +void cmsx_Vtx_FeatureWriteback(void) +{ + if (cmsx_featureVtx) + featureSet(FEATURE_VTX); + else + featureClear(FEATURE_VTX); +} + +void cmsx_Vtx_ConfigRead(void) +{ +#ifdef VTX + cmsx_vtxBand = masterConfig.vtxBand; + cmsx_vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#ifdef USE_RTC6705 + cmsx_vtxBand = masterConfig.vtx_channel / 8; + cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 +} + +void cmsx_Vtx_ConfigWriteback(void) +{ +#ifdef VTX + masterConfig.vtxBand = cmsx_vtxBand; + masterConfig.vtx_channel = cmsx_vtxChannel - 1; +#endif // VTX + +#ifdef USE_RTC6705 + masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; +#endif // USE_RTC6705 +} + +#endif // VTX || USE_RTC6705 + +#endif // CMS diff --git a/src/main/io/cms_vtx.h b/src/main/io/cms_vtx.h new file mode 100644 index 0000000000..412a9e4f73 --- /dev/null +++ b/src/main/io/cms_vtx.h @@ -0,0 +1,7 @@ +extern OSD_Entry cmsx_menuVtx[]; + +void cmsx_Vtx_FeatureRead(void); +void cmsx_Vtx_FeatureWriteback(void); + +void cmsx_Vtx_ConfigRead(void); +void cmsx_Vtx_ConfigWriteback(void); diff --git a/src/main/io/display.c b/src/main/io/dashboard.c similarity index 91% rename from src/main/io/display.c rename to src/main/io/dashboard.c index d8b5a8b82e..3385222791 100644 --- a/src/main/io/display.c +++ b/src/main/io/dashboard.c @@ -21,7 +21,9 @@ #include "platform.h" -#ifdef DISPLAY +#ifdef USE_DASHBOARD + +#include "common/utils.h" #include "build/version.h" #include "build/debug.h" @@ -50,7 +52,10 @@ #include "flight/imu.h" #include "flight/failsafe.h" +#ifdef OLEDCMS #include "io/cms.h" +void dashboardCmsInit(displayPort_t *pPort); // Forward +#endif #ifdef GPS #include "io/gps.h" @@ -60,7 +65,7 @@ #include "config/feature.h" #include "config/config_profile.h" -#include "io/display.h" +#include "io/dashboard.h" #include "rx/rx.h" @@ -76,7 +81,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5) static uint32_t nextDisplayUpdateAt = 0; -static bool displayPresent = false; +static bool dashboardPresent = false; static rxConfig_t *rxConfig; @@ -100,7 +105,7 @@ static const char* const pageTitles[] = { #ifdef GPS ,"GPS" #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,"DEBUG" #endif }; @@ -118,7 +123,7 @@ const pageId_e cyclePageIds[] = { #ifndef SKIP_TASK_STATISTICS ,PAGE_TASKS #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE ,PAGE_DEBUG, #endif }; @@ -146,7 +151,7 @@ typedef struct pageState_s { static pageState_t pageState; void resetDisplay(void) { - displayPresent = ug2864hsweg01InitI2C(); + dashboardPresent = ug2864hsweg01InitI2C(); } void LCDprint(uint8_t i) { @@ -564,7 +569,7 @@ void showTasksPage(void) } #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE void showDebugPage(void) { @@ -580,15 +585,15 @@ void showDebugPage(void) #endif #ifdef OLEDCMS -static bool displayInCMS = false; +static bool dashboardInCMS = false; #endif -void displayUpdate(uint32_t currentTime) +void dashboardUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; #ifdef OLEDCMS - if (displayInCMS) + if (dashboardInCMS) return; #endif @@ -634,13 +639,13 @@ void displayUpdate(uint32_t currentTime) // user to power off/on the display or connect it while powered. resetDisplay(); - if (!displayPresent) { + if (!dashboardPresent) { return; } handlePageChange(); } - if (!displayPresent) { + if (!dashboardPresent) { return; } @@ -677,7 +682,7 @@ void displayUpdate(uint32_t currentTime) } break; #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE case PAGE_DEBUG: showDebugPage(); break; @@ -691,84 +696,82 @@ void displayUpdate(uint32_t currentTime) } -void displaySetPage(pageId_e pageId) +void dashboardSetPage(pageId_e pageId) { pageState.pageId = pageId; pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; } -void displayInit(rxConfig_t *rxConfigToUse) +void dashboardInit(rxConfig_t *rxConfigToUse) { delay(200); resetDisplay(); delay(200); -#if defined(CMS) && defined(OLEDCMS) - cmsDeviceRegister(displayCmsInit); +#ifdef OLEDCMS + cmsDeviceRegister(dashboardCmsInit); #endif rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); - displaySetPage(PAGE_WELCOME); + dashboardSetPage(PAGE_WELCOME); - displayUpdate(micros()); + dashboardUpdate(micros()); - displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5)); + dashboardSetNextPageChangeAt(micros() + (1000 * 1000 * 5)); } -void displayShowFixedPage(pageId_e pageId) +void dashboardShowFixedPage(pageId_e pageId) { - displaySetPage(pageId); - displayDisablePageCycling(); + dashboardSetPage(pageId); + dashboardDisablePageCycling(); } -void displaySetNextPageChangeAt(uint32_t futureMicros) +void dashboardSetNextPageChangeAt(uint32_t futureMicros) { pageState.nextPageAt = futureMicros; } -void displayEnablePageCycling(void) +void dashboardEnablePageCycling(void) { pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED; } -void displayResetPageCycling(void) +void dashboardResetPageCycling(void) { pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page } -void displayDisablePageCycling(void) +void dashboardDisablePageCycling(void) { pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } #ifdef OLEDCMS -#include "io/cms.h" - -int displayCmsBegin(void) +int dashboardCmsBegin(void) { - displayInCMS = true; + dashboardInCMS = true; return 0; } -int displayCmsEnd(void) +int dashboardCmsEnd(void) { - displayInCMS = false; + dashboardInCMS = false; return 0; } -int displayCmsClear(void) +int dashboardCmsClear(void) { i2c_OLED_clear_display_quick(); return 0; } -int displayCmsWrite(uint8_t x, uint8_t y, char *s) +int dashboardCmsWrite(uint8_t x, uint8_t y, char *s) { i2c_OLED_set_xy(x, y); i2c_OLED_send_string(s); @@ -776,22 +779,36 @@ int displayCmsWrite(uint8_t x, uint8_t y, char *s) return 0; } -screenFnVTable_t displayCmsVTable = { - displayCmsBegin, - displayCmsEnd, - displayCmsClear, - displayCmsWrite, - NULL, - NULL, +int dashboardCmsHeartbeat(void) +{ + return 0; +} + +void dashboardCmsResync(displayPort_t *pPort) +{ + UNUSED(pPort); +} + +uint32_t dashboardCmsTxBytesFree(void) +{ + return UINT32_MAX; +} + +displayPortVTable_t dashboardCmsVTable = { + dashboardCmsBegin, + dashboardCmsEnd, + dashboardCmsClear, + dashboardCmsWrite, + dashboardCmsHeartbeat, + dashboardCmsResync, + dashboardCmsTxBytesFree, }; -void displayCmsInit(displayPort_t *pPort) +void dashboardCmsInit(displayPort_t *pPort) { - pPort->rows = 8; - pPort->cols = 21; - pPort->buftime = 1; - pPort->bufsize = 50000; - pPort->VTable = &displayCmsVTable; + pPort->rows = SCREEN_CHARACTER_ROW_COUNT; + pPort->cols = SCREEN_CHARACTER_COLUMN_COUNT; + pPort->vTable = &dashboardCmsVTable; } #endif // OLEDCMS diff --git a/src/main/io/display.h b/src/main/io/dashboard.h similarity index 72% rename from src/main/io/display.h rename to src/main/io/dashboard.h index e2c6c2b967..5b98b4c282 100644 --- a/src/main/io/display.h +++ b/src/main/io/dashboard.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define ENABLE_DEBUG_OLED_PAGE +#define ENABLE_DEBUG_DASHBOARD_PAGE typedef enum { PAGE_WELCOME, @@ -30,22 +30,26 @@ typedef enum { #ifdef GPS PAGE_GPS, #endif -#ifdef ENABLE_DEBUG_OLED_PAGE +#ifdef ENABLE_DEBUG_DASHBOARD_PAGE PAGE_DEBUG, #endif } pageId_e; struct rxConfig_s; -void displayInit(struct rxConfig_s *intialRxConfig); -void displayUpdate(uint32_t currentTime); +void dashboardInit(struct rxConfig_s *intialRxConfig); +void dashboardUpdate(uint32_t currentTime); -void displayShowFixedPage(pageId_e pageId); +void dashboardShowFixedPage(pageId_e pageId); +void dashboardEnablePageCycling(void); +void dashboardDisablePageCycling(void); +void dashboardResetPageCycling(void); +void dashboardSetNextPageChangeAt(uint32_t futureMicros); void displayEnablePageCycling(void); void displayDisablePageCycling(void); void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); #ifdef CMS -void displayCmsInit(displayPort_t *); +//void dashboardCmsInit(displayPort_t *pPort); #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 07d8a33f8b..d4bdc01261 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -41,7 +41,7 @@ #include "io/cms.h" #include "io/serial.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/gps.h" #include "flight/gps_conversion.h" @@ -1074,9 +1074,9 @@ static bool gpsNewFrameUBLOX(uint8_t data) static void gpsHandlePassthrough(uint8_t data) { gpsNewData(data); - #ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayUpdate(micros()); + #ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardUpdate(micros()); } #endif @@ -1090,9 +1090,9 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) if(!(gpsPort->mode & MODE_TX)) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayShowFixedPage(PAGE_GPS); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardShowFixedPage(PAGE_GPS); } #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c67c07ce64..c271d4339f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -266,30 +266,26 @@ void osdDrawSingleElement(uint8_t item) #endif // VTX case OSD_CROSSHAIRS: - { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; - + elemPosX = 14 - 1; // Offset for 1 char to the left + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; - - screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); - screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); - screenBuffer[position] = (SYM_AH_CENTER); - - return; - } + ++elemPosY; + buff[0] = SYM_AH_CENTER_LINE; + buff[1] = SYM_AH_CENTER; + buff[2] = SYM_AH_CENTER_LINE_RIGHT; + buff[3] = 0; + break; case OSD_ARTIFICIAL_HORIZON: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6 - 4; // Top center of the AH area int rollAngle = attitude.values.roll; int pitchAngle = attitude.values.pitch; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; if (pitchAngle > AH_MAX_PITCH) pitchAngle = AH_MAX_PITCH; @@ -300,13 +296,15 @@ void osdDrawSingleElement(uint8_t item) if (rollAngle < -AH_MAX_ROLL) rollAngle = -AH_MAX_ROLL; - for (uint8_t x = 0; x <= 8; x++) { - int y = (rollAngle * (4 - x)) / 64; - y -= pitchAngle / 8; - y += 41; + // Convert pitchAngle to y compensation value + pitchAngle = (pitchAngle / 8) - 41; // 41 = 4 * 9 + 5 + + for (int8_t x = -4; x <= 4; x++) { + int y = (rollAngle * x) / 64; + y -= pitchAngle; + // y += 41; // == 4 * 9 + 5 if (y >= 0 && y <= 81) { - uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; - screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + max7456WriteChar(elemPosX + x, elemPosY + (y / 9), (SYM_AH_BAR9_0 + (y % 9))); } } @@ -317,23 +315,23 @@ void osdDrawSingleElement(uint8_t item) case OSD_HORIZON_SIDEBARS: { - uint8_t *screenBuffer = max7456GetScreenBuffer(); - uint16_t position = 194; + elemPosX = 14; + elemPosY = 6; if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) - position += 30; + ++elemPosY; // Draw AH sides int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; - for (int8_t x = -hudheight; x <= hudheight; x++) { - screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); - screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + for (int8_t y = -hudheight; y <= hudheight; y++) { + max7456WriteChar(elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION); + max7456WriteChar(elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION); } // AH level indicators - screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); - screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + max7456WriteChar(elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); + max7456WriteChar(elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); return; } @@ -392,9 +390,11 @@ void osdInit(void) sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); max7456Write(5, 6, string_buffer); - max7456Write(7, 7, STARTUP_HELP_TEXT1); - max7456Write(11, 8, STARTUP_HELP_TEXT2); - max7456Write(11, 9, STARTUP_HELP_TEXT3); +#ifdef CMS + max7456Write(7, 7, CMS_STARTUP_HELP_TEXT1); + max7456Write(11, 8, CMS_STARTUP_HELP_TEXT2); + max7456Write(11, 9, CMS_STARTUP_HELP_TEXT3); +#endif max7456RefreshAll(); @@ -636,6 +636,7 @@ void osdUpdate(uint32_t currentTime) // // OSD specific CMS functions // +#include "io/cms_osd.h" uint8_t shiftdown; @@ -669,6 +670,23 @@ int osdWrite(uint8_t x, uint8_t y, char *s) return 0; } +void osdResync(displayPort_t *pPort) +{ + max7456RefreshAll(); + pPort->rows = max7456GetRowsCount() - masterConfig.osdProfile.row_shiftdown; + pPort->cols = 30; +} + +int osdHeartbeat(void) +{ + return 0; +} + +uint32_t osdTxBytesFree(void) +{ + return UINT32_MAX; +} + #ifdef EDIT_ELEMENT_SUPPORT void osdEditElement(void *ptr) { @@ -695,22 +713,70 @@ void osdDrawElementPositioningHelp(void) } #endif -screenFnVTable_t osdVTable = { +displayPortVTable_t osdVTable = { osdMenuBegin, osdMenuEnd, osdClearScreen, osdWrite, - NULL, - max7456RefreshAll, + osdHeartbeat, + osdResync, + osdTxBytesFree, }; void osdCmsInit(displayPort_t *pPort) { - shiftdown = masterConfig.osdProfile.row_shiftdown; - pPort->rows = max7456GetRowsCount() - shiftdown; - pPort->cols = 30; - pPort->buftime = 1; // Very fast - pPort->bufsize = 50000; // Very large - pPort->VTable = &osdVTable; + osdResync(pPort); + pPort->vTable = &osdVTable; } + +OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1}; + +OSD_Entry cmsx_menuAlarms[] = +{ + {"--- ALARMS ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0}, + {"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0}, + {"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry menuOsdActiveElems[] = +{ + {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0}, + {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + +OSD_Entry cmsx_menuOsdLayout[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0}, + {"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0}, + {"BACK", OME_Back, NULL, NULL, 0}, + {NULL, OME_END, NULL, NULL, 0} +}; + #endif // OSD diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f6d2f92665..cf4202d3aa 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -148,7 +148,8 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) +static void printResource(uint8_t dumpMask, master_t *defaultConfig); static void cliResource(char *cmdline); #endif #ifdef GPS @@ -203,6 +204,7 @@ typedef enum { DUMP_ALL = (1 << 3), DO_DIFF = (1 << 4), SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6), } dumpFlags_e; static const char* const emptyName = "-"; @@ -237,7 +239,7 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) // sync this with sensors_e static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL @@ -331,7 +333,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), @@ -2744,6 +2746,11 @@ static void printConfig(char *cmdline, bool doDiff) #endif printName(dumpMask); +#ifndef CLI_MINIMAL_VERBOSITY + cliPrint("\r\n# resources\r\n"); +#endif + printResource(dumpMask, &defaultConfig); + #ifndef USE_QUAD_MIXER_ONLY #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mixer\r\n"); @@ -3726,7 +3733,7 @@ void cliProcess(void) } } -#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) +#if (FLASH_SIZE > 64) typedef struct { const uint8_t owner; @@ -3752,13 +3759,72 @@ const cliResourceValue_t resourceTable[] = { #endif }; +static void printResource(uint8_t dumpMask, master_t *defaultConfig) +{ + for (unsigned int i = 0; i < ARRAYLEN(resourceTable); i++) { + const char* owner; + owner = ownerNames[resourceTable[i].owner]; + + if (resourceTable[i].maxIndex > 0) { + for (int index = 0; index < resourceTable[i].maxIndex; index++) { + ioTag_t ioPtr = *(resourceTable[i].ptr + index); + ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %d %c%02d\r\n"; + const char *formatUnassigned = "resource %s %d NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } else { + ioTag_t ioPtr = *resourceTable[i].ptr; + ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + + IO_t io = IOGetByTag(ioPtr); + IO_t ioDefault = IOGetByTag(ioPtrDefault); + bool equalsDefault = io == ioDefault; + const char *format = "resource %s %c%02d\r\n"; + const char *formatUnassigned = "resource %s NONE\r\n"; + if (DEFIO_TAG_ISEMPTY(ioDefault)) { + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } else { + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault)); + } + if (DEFIO_TAG_ISEMPTY(io)) { + if (!(dumpMask & HIDE_UNUSED)) { + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + } + } else { + cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + } +} + static void cliResource(char *cmdline) { - int len; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { - cliPrintf("IO:\r\n----------------------\r\n"); + printResource(DUMP_MASTER | HIDE_UNUSED, NULL); + + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n----------------------\r\n"); +#endif for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { const char* owner; owner = ownerNames[ioRecs[i].owner]; @@ -3772,34 +3838,10 @@ static void cliResource(char *cmdline) cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); } } - cliPrintf("\r\nUse: 'resource list' to see how to change resources.\r\n"); - return; - } else if (strncasecmp(cmdline, "list", len) == 0) { - for (uint8_t i = 0; i < ARRAYLEN(resourceTable); i++) { - const char* owner; - owner = ownerNames[resourceTable[i].owner]; +#ifndef CLI_MINIMAL_VERBOSITY + cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); +#endif - if (resourceTable[i].maxIndex > 0) { - for (int index = 0; index < resourceTable[i].maxIndex; index++) { - - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr + index))) { - continue; - } - - IO_t io = IOGetByTag(*(resourceTable[i].ptr + index)); - if (!io) { - continue; - } - cliPrintf("resource %s %d %c%02d\r\n", owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } - } else { - if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr))) { - continue; - } - IO_t io = IOGetByTag(*resourceTable[i].ptr); - cliPrintf("resource %s %c%02d\r\n", owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); - } - } return; } @@ -3840,7 +3882,11 @@ static void cliResource(char *cmdline) cliPrintf("Resource is freed!"); return; } else { - uint8_t port = (*pch)-'A'; + uint8_t port = (*pch) - 'A'; + if (port >= 8) { + port = (*pch) - 'a'; + } + if (port < 8) { pch++; pin = atoi(pch); @@ -3865,7 +3911,9 @@ static void cliResource(char *cmdline) void cliDfu(char *cmdLine) { UNUSED(cmdLine); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\nRestarting in DFU mode"); +#endif cliRebootEx(true); } diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index b58ed5befe..4b401b384b 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -293,6 +293,7 @@ static void saProcessResponse(uint8_t *buf, int len) // Export current settings for CMS +#ifdef CMS smartAudioBand = (sa_chan / 8) + 1; smartAudioChan = (sa_chan % 8) + 1; smartAudioFreq = saFreqTable[sa_chan / 8][sa_chan % 8]; @@ -312,6 +313,7 @@ static void saProcessResponse(uint8_t *buf, int len) } else { smartAudioPower = saDacToPowerIndex(sa_power) + 1; } +#endif #ifdef SMARTAUDIO_DEBUG_MONITOR debug[0] = sa_vers * 100 + sa_opmode; @@ -342,7 +344,9 @@ static void saProcessResponse(uint8_t *buf, int len) if (freq & SA_FREQ_GETPIT) { sa_pitfreq = freq & ~SA_FREQ_GETPIT; dprintf(("saProcessResponse: GETPIT freq %d\r\n", sa_pitfreq)); +#ifdef CMS smartAudioUpdateStatusString(); +#endif } else if (freq & SA_FREQ_SETPIT) { dprintf(("saProcessResponse: SETPIT freq %d\r\n", freq)); } else { @@ -844,5 +848,108 @@ long smartAudioConfigureOpModelByGvar(displayPort_t *pDisp, void *self) return 0; } + +static const char * const smartAudioBandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; +OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; + +static const char * const smartAudioChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; + +static const char * const smartAudioPowerNames[] = { + "---", + " 25", + "200", + "500", + "800", +}; + +OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; + +static const char * const smartAudioTxModeNames[] = { + "------", + "PIT-OR", + "PIT-IR", + "ACTIVE", +}; + +OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; + +OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; + +static const char * const smartAudioOpModelNames[] = { + "FREE", + "PIT", +}; + +OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; + +static const char * const smartAudioPitFModeNames[] = { + "IN-RANGE", + "OUT-RANGE", +}; + +OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; + +OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; + +OSD_Entry menu_smartAudioConfig[] = { + { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL, 0 }, + { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel, 0 }, + { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode, 0 }, + { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq, 0 }, // OME_Poll_UINT16 + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static const char * const smartAudioStatusNames[] = { + "OFFLINE", + "ONLINE V1", + "ONLINE V2", +}; + +OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; +OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; + +OSD_Entry menu_smartAudioStats[] = { + { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL, 0 }, + { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline, 0 }, + { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate, 0 }, + { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre, 0 }, + { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen, 0 }, + { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr, 0 }, + { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +OSD_Entry cmsx_menuVtxSmartAudio[] = +{ + { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL, 0 }, + { smartAudioStatusString, OME_Label, NULL, NULL, 0 }, + { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode, 0 }, + { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand, 0 }, + { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan, 0 }, + { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq, 0 }, + { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig[0], 0 }, + { "STAT", OME_Submenu, cmsMenuChange, &menu_smartAudioStats[0], 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + #endif // CMS #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 10aa1e303c..0f40c67a04 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -33,4 +33,6 @@ long smartAudioConfigureBandByGvar(displayPort_t *, void *self); long smartAudioConfigureChanByGvar(displayPort_t *, void *self); long smartAudioConfigurePowerByGvar(displayPort_t *, void *self); long smartAudioSetTxModeByGvar(displayPort_t *, void *self); + +extern OSD_Entry cmsx_menuVtxSmartAudio[]; #endif diff --git a/src/main/main.c b/src/main/main.c index cf322a4c20..f44a9cbbf2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -75,6 +75,7 @@ #include "rx/spektrum.h" #include "io/cms.h" +#include "io/cms_types.h" #include "io/beeper.h" #include "io/serial.h" @@ -84,7 +85,7 @@ #include "io/servos.h" #include "io/gimbal.h" #include "io/ledstrip.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" @@ -403,9 +404,9 @@ void init(void) cmsInit(); #endif -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayInit(&masterConfig.rxConfig); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardInit(&masterConfig.rxConfig); } #endif @@ -603,13 +604,13 @@ void init(void) if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY - displayShowFixedPage(PAGE_GPS); + dashboardShowFixedPage(PAGE_GPS); #else - displayResetPageCycling(); - displayEnablePageCycling(); + dashboardResetPageCycling(); + dashboardEnablePageCycling(); #endif } #endif diff --git a/src/main/main.c.orig b/src/main/main.c.orig index 37dda56ec0..3841ad3561 100644 --- a/src/main/main.c.orig +++ b/src/main/main.c.orig @@ -84,18 +84,18 @@ #include "io/servos.h" #include "io/gimbal.h" #include "io/ledstrip.h" -#include "io/display.h" +#include "io/dashboard.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" -#include "io/cms.h" #include "io/osd.h" -#include "io/vtx.h" <<<<<<< HEAD +#include "io/vtx.h" #include "io/vtx_smartaudio.h" ======= -#include "io/canvas.h" >>>>>>> bfdev-osd-cms-separation-poc +#include "io/canvas.h" +#include "io/vtx.h" #include "scheduler/scheduler.h" @@ -407,9 +407,9 @@ void init(void) cmsInit(); #endif -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { - displayInit(&masterConfig.rxConfig); +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { + dashboardInit(&masterConfig.rxConfig); } #endif @@ -607,13 +607,13 @@ void init(void) if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); -#ifdef DISPLAY - if (feature(FEATURE_DISPLAY)) { +#ifdef USE_DASHBOARD + if (feature(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY - displayShowFixedPage(PAGE_GPS); + dashboardShowFixedPage(PAGE_GPS); #else - displayResetPageCycling(); - displayEnablePageCycling(); + dashboardResetPageCycling(); + dashboardEnablePageCycling(); #endif } #endif diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index ef39598c96..409e60afc8 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -211,8 +211,6 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } -mspPushCommandFnPtr mspPushCommandFn; - void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) { static uint8_t pushBuf[30]; @@ -234,7 +232,7 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) continue; } - mspPushCommandFn(&push, data, datalen); + sbufWriteData(&push.buf, data, datalen); sbufSwitchToReader(&push.buf, pushBuf); @@ -242,7 +240,27 @@ void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) } } -void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse) +uint32_t mspSerialTxBytesFree() { - mspPushCommandFn = mspPushCommandFnToUse; + uint32_t minroom = UINT16_MAX; + + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + uint32_t room = serialTxBytesFree(mspPort->port); + + if (room < minroom) { + minroom = room; + } + } + + return minroom; } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 5d92114736..0f1c8a2fff 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,5 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); -void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); -void mspSerialPush(uint8_t, uint8_t *, int); +void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen); +uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5ec6cb9330..89b6a708dc 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -67,8 +67,8 @@ typedef enum { #if defined(BARO) || defined(SONAR) TASK_ALTITUDE, #endif -#ifdef DISPLAY - TASK_DISPLAY, +#ifdef USE_DASHBOARD + TASK_DASHBOARD, #endif #ifdef TELEMETRY TASK_TELEMETRY, diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index acb1586cfe..591219e747 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -24,16 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, //LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, //LED_STRIP }; diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 6ead0d6fee..1709455c7d 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -23,13 +23,13 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 1b8f6c2111..0cff2b6aab 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -89,7 +89,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index 242a1945f9..e7ca5a7261 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -23,19 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - RC8 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_11}, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2} // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF_1}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2}, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_11}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2} // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index e838c785ba..baa453c020 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index fc7861550b..4673d05a46 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -24,18 +24,16 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // up to 10 Motor Outputs - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - - // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index dcdce3e43c..1fddc56b96 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,19 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 - { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 - { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 - { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PWM1 - PA8 RC1 + { TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM2 - PB0 RC2 + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // PWM3 - PB1 RC3 + { TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM4 - PA14 RC4 + { TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // PWM5 - PA15 RC5 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM6 - PB8 OUT1 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // PWM7 - PB9 OUT2 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM8 - PA0 OUT3 + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // PWM9 - PA1 OUT4 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM10 - PC6 OUT5 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM11 - PC7 OUT6 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC8 OUT7 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // PWM13 - PC9 OUT8 }; diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 978118d2eb..e2f55aa290 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -26,44 +26,44 @@ #if defined(USE_DSHOT) // DSHOT TEST const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8, NULL, 0, 0 }, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, DMA1_Stream7, DMA_CHANNEL_2, DMA1_ST7_HANDLER }, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream1, DMA_CHANNEL_3, DMA1_ST1_HANDLER }, // S6_OUT 2 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, DMA1_Stream6, DMA_CHANNEL_3, DMA1_ST6_HANDLER }, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream4, DMA_CHANNEL_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, DMA1_Stream5, DMA_CHANNEL_5, DMA1_ST5_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5, DMA1_Stream2, DMA_CHANNEL_6, DMA1_ST2_HANDLER }, // S7_OUT + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4, NULL, 0, 0 }, // S5_OUT 3 + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9, NULL, 0, 0 }, // S3_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2, NULL, 0, 0 }, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3, NULL, 0, 0 }, // S9_OUT }; #else // STANDARD LAYOUT const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S1_IN - { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP , GPIO_AF9_TIM12}, // S2_IN - { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S3_IN - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S4_IN - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S5_IN - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP , GPIO_AF3_TIM8}, // S6_IN + { TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF9_TIM12}, // S1_IN + { TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF9_TIM12}, // S2_IN + { TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S3_IN + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S4_IN + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S5_IN + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF3_TIM8}, // S6_IN - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S10_OUT 1 - { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S6_OUT 2 - { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM4}, // S5_OUT 3 - { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S1_OUT 4 - { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S2_OUT - { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP , GPIO_AF3_TIM9}, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM5}, // S7_OUT - { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP , GPIO_AF1_TIM2}, // S8_OUT - { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP , GPIO_AF2_TIM3}, // S9_OUT + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S10_OUT 1 + { TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S6_OUT 2 + { TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM4}, // S5_OUT 3 + { TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S1_OUT 4 + { TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S2_OUT + { TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF3_TIM9}, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM5}, // S7_OUT + { TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF1_TIM2}, // S8_OUT + { TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF2_TIM3}, // S9_OUT }; #endif diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 82adbb72fa..e88dce7400 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // RC PPM - PB7 - TIM17_CH1N AF1, TIM4_CH2 AF2, TIM8_BKIN AF5, TIM3_CH4 AF10 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10,DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM4 - PB9 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM1 - PA6 - TIM3_CH1 AF2, TIM8_BKIN AF4, TIM1_BKIN AF6, *TIM16_CH1 AF1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2 AF2, TIM8_CH1 AF4, TIM1_CH1N AF6, *TIM17_CH1 AF1 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10,DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM3 - PB8 - TIM16_CH1 AF1, TIM4_CH3 AF2, TIM8_CH2 AF10, TIM1_BKIN AF12 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA2_Channel2, DMA2_CH2_HANDLER }, // PWM4 - PB9 - TIM17_CH1 AF1, TIM4_CH4 AF2, TIM8_CH3 AF10 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL RX - TIM3_CH3 AF2, TIM8_CH2N AF4, TIM1_CH2N AF6 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // SOFTSERIAL TX - TIM3_CH4 AF2, TIM8_CH3N AF4, TIM1_CH3N AF6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0 - TIM2_CH1 AF1, TIM8_BKIN AF9, TIM8_ETR AF10 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - TIM2_CH3 AF1, TIM15_CH1 AF9 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index f7d6db277d..4806240672 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -45,11 +45,8 @@ #define USE_EXTI #define USE_DSHOT - -// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4 -#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT) -#undef USE_UART1_TX_DMA -#endif +#define REMAP_TIM16_DMA +#define REMAP_TIM17_DMA #define USB_IO @@ -66,11 +63,11 @@ #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 @@ -81,7 +78,6 @@ #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 -//GPIO_AF_1 #define SPI1_NSS_PIN PA15 #define SPI1_SCK_PIN PB3 diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index c82953b221..3e7a18ac30 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -23,12 +23,12 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S6_OUT }; diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c0a1dd07c2..71a6462bb4 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -23,17 +23,17 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // S1_IN - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // S4_IN - Current - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // S5_IN - Vbattery - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // S6_IN - PPM IN - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP }, // S1_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP }, // S2_OUT - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP }, // S3_OUT - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP }, // S4_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP } // S6_OUT + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, }, // S1_IN + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, }, // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, }, // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, }, // S4_IN - Current + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, }, // S5_IN - Vbattery + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, }, // S6_IN - PPM IN + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S1_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S2_OUT + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, }, // S3_OUT + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, }, // S4_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, }, // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, } // S6_OUT }; diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 8867ca53e6..1986a350ce 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -24,23 +24,23 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // INPUTS CH1-8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM2 - PB8 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM3 - PB9 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM4 - PC6 - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM5 - PC7 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4 }, // PWM6 - PC8 - { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM7 - PF9 - { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_3 }, // PWM8 - PF10 - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PD12 - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM10 - PD13 - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM11 - PD14 - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM12 - PD15 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM13 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM14 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM15 - PA3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM16 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM17 - PB1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 } // PWM18 - PA4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM1 - PA8 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM2 - PB8 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM3 - PB9 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM4 - PC6 + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM5 - PC7 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4 }, // PWM6 - PC8 + { TIM15, IO_TAG(PF9), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM7 - PF9 + { TIM15, IO_TAG(PF10), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_PWM, 0, GPIO_AF_3 }, // PWM8 - PF10 + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PD12 + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM10 - PD13 + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM11 - PD14 + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM12 - PD15 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM13 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM14 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM15 - PA3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM16 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM17 - PB1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 } // PWM18 - PA4 }; diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index d784dfcb9f..30f0e1b00e 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11),TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index b9cb1b1704..fd5c5d4272 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -24,21 +24,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S8_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM10 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM11 }, // S8_OUT + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM5 }, // S8_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM12 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM10 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM11 }, // S8_OUT }; diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 9bf7e986c0..08fd1255e1 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index b75f0e7dd8..58f98c9a5c 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -23,17 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB9 - - { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PMW4 - PA10 - { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM5 - PA9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM8 - PB1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PWM9 - PB0 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB9 + { TIM2, IO_TAG(PA10), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PMW4 - PA10 + { TIM2, IO_TAG(PA9), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM5 - PA9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 - PB1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM9 - PB0 }; diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 42608606e9..cf7aab2df1 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,24 +6,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT - - { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM4 }, // S8_IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S8_OUT + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 504e3b6c2d..4f50ab5d01 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -25,16 +25,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PPM IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 - { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 - { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, (1 | TIMER_OUTPUT_N_CHANNEL), IOCFG_AF_PP, GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP - + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_6, NULL, 0 }, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM4 - S1 + { TIM8, IO_TAG(PB6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_5, DMA2_Channel3, DMA2_CH3_HANDLER }, // PWM5 - S2 + { TIM8, IO_TAG(PB5), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, (1 | TIMER_OUTPUT_N_CHANNEL), GPIO_AF_3, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM6 - S3 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM7 - S4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index af6feb53c6..322753dd75 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -24,12 +24,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN - - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM_IN + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT // { TIM5, GPIOA, Pin_0, TIM_Channel_1, TIM5_IRQn, 1, GPIO_Mode_AF, GPIO_PinSource0, GPIO_AF_TIM5 }, // LED Strip }; diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 04200bc0b4..6e8a09cba2 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -22,13 +22,13 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2}, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6}, // LED_STRIP }; diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 370f9eb358..5add051e06 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index d998cdba92..5a4f973de0 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -23,25 +23,24 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM |TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index d36fa9903a..6c27661b0d 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -24,18 +24,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel5, DMA1_CH5_HANDLER }, - { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_2, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel6, DMA1_CH6_HANDLER }, - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PB0), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_6, DMA1_Channel3, DMA1_CH3_HANDLER }, + { TIM15, IO_TAG(PB15), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel5, DMA1_CH5_HANDLER }, + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0}, - { TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_10, NULL, 0}, - { TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_5, NULL, 0}, + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0}, // TODO - Cleanup. KISS FC uses the same pin for serial and PPM + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0}, + //{ TIM4, IO_TAG(PA13), TIM_Channel_2, TIM4_IRQn, TIM_USE_PWM, 0, GPIO_AF_10, NULL, 0}, + //{ TIM8, IO_TAG(PA14), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_5, NULL, 0}, }; diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index f8a6062776..fcb2c38a03 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -84,5 +84,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 07c5716f90..9352353f82 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -23,16 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_6 }, // PWM1 - PA8 - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PC6 - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PC7 - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PMW4 - PC8 - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PC9 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA2 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PA3 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM10 - PB14 - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PWM11 - PB15 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_6 }, // PWM1 - PA8 + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PC6 + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PC7 + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PMW4 - PC8 + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA2 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM9 - PA3 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM10 - PB14 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM11 - PB15 }; diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index e838c785ba..4319f6fb45 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 0 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 0 } // PWM14 - OUT6 }; diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 2628264522..f61806f3b7 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -91,7 +91,6 @@ #undef GPS #undef USE_SERVOS #define USE_QUAD_MIXER_ONLY -#define DISPLAY // IO - assuming all IOs on 48pin package diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index 43db149492..10cb9ad654 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -23,14 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 6a7ae42a0b..93e92d61cb 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -91,7 +91,6 @@ #define SENSORS_SET (SENSOR_ACC) #undef GPS -#define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index af48f988d7..c9bc95fe04 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -23,19 +23,19 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 - { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_IPD }, // PWM11 - OUT3 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_IPD }, // PWM12 - OUT4 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_IPD }, // PWM13 - OUT5 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_IPD } // PWM14 - OUT6 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM | TIM_USE_PWM, 0 }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0 }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0 }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_MOTOR, 1 }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1 }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1 } // PWM14 - OUT6 }; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index eb8ab5503a..d7586fe858 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -118,8 +118,6 @@ //#define SONAR_TRIGGER_PIN_PWM PB8 //#define SONAR_ECHO_PIN_PWM PB9 -//#define DISPLAY - #define USE_UART1 #define USE_UART2 /* only 2 uarts available on the NAZE, add ifdef here if present on other boards */ diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 1f8269072b..0e15861a61 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -25,18 +25,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2, NULL, 0 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 - { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 + { TIM8, IO_TAG(PB8), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel5, DMA2_CH5_HANDLER }, // PWM1 - PB8 + { TIM8, IO_TAG(PB9), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA2_Channel1, DMA2_CH1_HANDLER }, // PWM2 - PB9 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, DMA1_Channel5, DMA1_CH5_HANDLER }, // PWM4 - PA2 // UART3 RX/TX - //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) - //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 - //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP + //{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + //{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM7 - PB7 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM8 - PB6 + //{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index d47158b8e6..c0f6166c12 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -92,11 +92,13 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +//#define USE_DASHBOARD + // Configuratoin Menu System #define CMS // Use external OSD to run CMS -#define CANVAS +//#define CANVAS // USE I2C OLED display to run CMS #define OLEDCMS diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index f16c836968..fc1c528bdd 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -13,8 +13,12 @@ TARGET_SRC = \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c \ + io/vtx_smartaudio.c \ drivers/max7456.c \ io/osd.c \ - io/cms.c \ io/canvas.c \ - io/vtx_smartaudio.c + io/cms.c \ + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/OMNIBUS/target.mk.orig b/src/main/target/OMNIBUS/target.mk.orig index 3de6d6e1a0..74528b3add 100644 --- a/src/main/target/OMNIBUS/target.mk.orig +++ b/src/main/target/OMNIBUS/target.mk.orig @@ -15,9 +15,14 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ + io/canvas.c \ + io/cms.c \ <<<<<<< HEAD + io/canvas.c \ io/vtx_smartaudio.c ======= - io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c >>>>>>> bfdev-osd-cms-separation-poc diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 5e63b90590..e088371565 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -24,17 +24,17 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S4_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, NULL, 0, 0 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1, NULL, 0, 0 }, // S6_OUT }; diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index c7cf0ad933..962731e61a 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -6,7 +6,12 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ drivers/max7456.c \ + io/vtx_smartaudio.c \ io/osd.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index 43db149492..10cb9ad654 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -23,14 +23,14 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PA4 - *TIM3_CH2 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index cc2c688932..dc726133e3 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -23,12 +23,11 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, - - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - PC6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - PC7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PMW4 - PC8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PC9 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PPM, 1, GPIO_AF_1}, + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PC9 }; diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 8ac51e6713..730d71b682 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -24,10 +24,10 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - PA4 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - PA7 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM3 - PA8 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - PB0 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PB1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - PPM + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1}, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6}, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2}, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1}, // PWM6 - PPM }; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index a1f5f8527d..2f19300b0d 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -130,7 +130,6 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE -#define DISPLAY #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index f8be6e2eab..66da4ce854 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -24,16 +24,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12, NULL, 0, 0 }, // S2_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 1, GPIO_AF_TIM3, DMA1_Stream7, DMA_Channel_5, DMA1_ST7_HANDLER }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream6, DMA_Channel_3, DMA1_ST6_HANDLER }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, DMA1_Stream1, DMA_Channel_3, DMA1_ST1_HANDLER }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5, DMA1_Stream4, DMA_Channel_6, DMA1_ST4_HANDLER }, // S5_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5, DMA1_Stream2, DMA_Channel_6, DMA1_ST2_HANDLER }, // S6_OUT }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index eed70b40de..4d73c99355 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -144,6 +144,3 @@ #define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) - -#define CMS -#define CANVAS diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9349b40100..1ddf9641cc 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -23,18 +23,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_IN - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_IN - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_IN - { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_OUT - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S3_OUT - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4 }, // S4_OUT - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S2_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S3_IN + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S4_IN + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM3 }, // S5_IN + { TIM2, IO_TAG(PA5), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM2 }, // S6_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM1 }, // S1_OUT + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S3_OUT + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM4 }, // S4_OUT + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR | TIM_USE_LED, 1, GPIO_AF_TIM5 }, // S5_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index 61603c7b4e..72523ef2b9 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -23,22 +23,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 0926431f19..7176e7bc7d 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -23,15 +23,15 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PPM/SERIAL RX - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // SOFTSERIAL1 TX - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // LED_STRIP + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM6 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 RX (NC) + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // SOFTSERIAL1 TX + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // LED_STRIP }; diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index be26986a86..2927c0b960 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -24,15 +24,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB6 - { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB6 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 - - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM1 - PB6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM2 - PB6 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 9a9f16e331..d1fb69fc83 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -137,6 +137,8 @@ //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_DASHBOARD + #define OSD // Configuratoin Menu System diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 6481f94182..e64c9781b3 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -13,5 +13,9 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/SOULF4/target.c b/src/main/target/SOULF4/target.c index f0673d116c..0b8819b4a3 100644 --- a/src/main/target/SOULF4/target.c +++ b/src/main/target/SOULF4/target.c @@ -22,16 +22,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 44aa07090a..f7a44f00d7 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -24,18 +24,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // 6 3-pin headers - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 0, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 // PWM7 - PMW10 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_PWM, 1, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_PPM, 0, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index 87abe2834b..359c09910c 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S2_IN + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM8 }, // S5_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM5 }, // S6_OUT }; diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index b96e424066..fe9518d8a9 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -24,25 +24,25 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easierTIM_USE_MOTOR, to using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 7a53ebdbcd..da2e8eaca6 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -140,6 +140,8 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USE_DASHBOARD + // Configuratoin Menu System #define CMS diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index dd7702375e..f2a3df7442 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -9,6 +9,10 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ + io/canvas.c \ io/cms.c \ - io/canvas.c + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 16541c9aeb..3aeac1bd84 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -24,17 +24,17 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX - { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index 13e7ee222c..6d40631b0a 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -25,27 +25,27 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB5 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PPM, 0, GPIO_AF_2 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM8 - PA1 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, 1, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) // LED Strip Pad - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 8cd9a3584f..54a5dbded7 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -24,19 +24,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1, NULL, 0 }, - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 } + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6, DMA1_Channel2, DMA1_CH2_HANDLER }, + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel3, DMA2_CH3_HANDLER }, + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel5, DMA2_CH5_HANDLER }, + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_4, DMA2_Channel1, DMA2_CH1_HANDLER }, + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, 0, GPIO_AF_2, NULL, 0 }, + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 }, + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, 0, GPIO_AF_1, NULL, 0 } }; diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 6b718b5b0a..4142776c47 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -27,4 +27,10 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/max7456.c \ io/osd.c \ - io/cms.c + io/canvas.c \ + io/cms.c \ + io/cms_imu.c \ + io/cms_blackbox.c \ + io/cms_vtx.c \ + io/cms_ledstrip.c + diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index af85c900d0..c5fc07b418 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -23,17 +23,16 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PPM - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S2_IN - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S3_IN - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S4_IN - { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S5_IN - { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S6_IN - - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S1_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_TIM1 }, // PPM + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S2_IN + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S3_IN + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM1 }, // S4_IN + { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S5_IN + { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, TIM_USE_PWM, 0, GPIO_AF_TIM9 }, // S6_IN + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index b59d0ba6a3..d83ee38e58 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -8,22 +8,20 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 951a147791..20dbd87572 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -22,11 +22,11 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS2 - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS3 - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // MS4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS5 - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // MS6 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, TIM_USE_PPM, 0, GPIO_AF_TIM8 }, // PPM IN + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM2 }, // MS4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS5 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_TIM3 }, // MS6 }; diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index d6197448da..9f99cf2be4 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -8,24 +8,22 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM3 - PA11 - { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 }, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 - - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, TIM_USE_PWM | TIM_USE_PPM, 0, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, TIM_USE_PWM, 0, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, TIM_USE_PWM, 0, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_10 }, // PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIM_USE_MOTOR, 1, GPIO_AF_9 }, // PWM8 - PA3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIM_USE_LED, 1, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/common.h b/src/main/target/common.h index 8d0ea828d2..42dfed36de 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -55,7 +55,7 @@ #endif #if (FLASH_SIZE > 128) -#define DISPLAY +#define USE_DASHBOARD #define TELEMETRY_MAVLINK #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index c54f0f3c2a..b9023cc6d2 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -14,6 +14,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/maths.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -23,6 +24,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/fc_msp.h" #include "io/beeper.h" #include "io/motors.h" @@ -53,6 +55,8 @@ #include "config/config_profile.h" #include "config/feature.h" +#include "msp/msp.h" + extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; @@ -67,7 +71,13 @@ enum enum { FSSP_START_STOP = 0x7E, + + FSSP_DLE = 0x7D, + FSSP_DLE_XOR = 0x20, + FSSP_DATA_FRAME = 0x10, + FSSP_MSPC_FRAME = 0x30, // MSP client frame + FSSP_MSPS_FRAME = 0x32, // MSP server frame // ID of sensor. Must be something that is polled by FrSky RX FSSP_SENSOR_ID1 = 0x1B, @@ -149,33 +159,116 @@ static uint8_t smartPortHasRequest = 0; static uint8_t smartPortIdCnt = 0; static uint32_t smartPortLastRequestTime = 0; +typedef struct smartPortFrame_s { + uint8_t sensorId; + uint8_t frameId; + uint16_t valueId; + uint32_t data; + uint8_t crc; +} __attribute__((packed)) smartPortFrame_t; + +#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) +#define SMARTPORT_TX_BUF_SIZE 256 + +#define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId) +#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1) + +static smartPortFrame_t smartPortRxBuffer; +static uint8_t smartPortRxBytes = 0; +static bool smartPortFrameReceived = false; + +#define SMARTPORT_MSP_VERSION 1 +#define SMARTPORT_MSP_VER_SHIFT 5 +#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) +#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) + +#define SMARTPORT_MSP_ERROR_FLAG (1 << 5) +#define SMARTPORT_MSP_START_FLAG (1 << 4) +#define SMARTPORT_MSP_SEQ_MASK 0x0F + +#define SMARTPORT_MSP_RX_BUF_SIZE 64 + +static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; +static mspPacket_t smartPortMspReply; +static bool smartPortMspReplyPending = false; + +#define SMARTPORT_MSP_RES_ERROR (-10) + +enum { + SMARTPORT_MSP_VER_MISMATCH=0, + SMARTPORT_MSP_CRC_ERROR=1, + SMARTPORT_MSP_ERROR=2 +}; + static void smartPortDataReceive(uint16_t c) { + static bool skipUntilStart = true; + static bool byteStuffing = false; + static uint16_t checksum = 0; + uint32_t now = millis(); - // look for a valid request sequence - static uint8_t lastChar; - if (lastChar == FSSP_START_STOP) { - smartPortState = SPSTATE_WORKING; - if (c == FSSP_SENSOR_ID1 && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + if (c == FSSP_START_STOP) { + smartPortRxBytes = 0; + smartPortHasRequest = 0; + skipUntilStart = false; + return; + } else if (skipUntilStart) { + return; + } + + uint8_t* rxBuffer = (uint8_t*)&smartPortRxBuffer; + if (smartPortRxBytes == 0) { + if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + + // our slot is starting... smartPortLastRequestTime = now; smartPortHasRequest = 1; - // we only responde to these IDs - // the X4R-SB does send other IDs, we ignore them, but take note of the time + } else if (c == FSSP_SENSOR_ID2) { + rxBuffer[smartPortRxBytes++] = c; + checksum = 0; + } + else { + skipUntilStart = true; + } + } + else { + + if (c == FSSP_DLE) { + byteStuffing = true; + return; + } + + if (byteStuffing) { + c ^= FSSP_DLE_XOR; + byteStuffing = false; + } + + rxBuffer[smartPortRxBytes++] = c; + + if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) { + if (c == (0xFF - checksum)) { + smartPortFrameReceived = true; + } + skipUntilStart = true; + } else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) { + checksum += c; + checksum += checksum >> 8; + checksum &= 0x00FF; } } - lastChar = c; } static void smartPortSendByte(uint8_t c, uint16_t *crcp) { // smart port escape sequence - if (c == 0x7D || c == 0x7E) { - serialWrite(smartPortSerialPort, 0x7D); - c ^= 0x20; + if (c == FSSP_DLE || c == FSSP_START_STOP) { + serialWrite(smartPortSerialPort, FSSP_DLE); + serialWrite(smartPortSerialPort, c ^ FSSP_DLE_XOR); + } + else { + serialWrite(smartPortSerialPort, c); } - - serialWrite(smartPortSerialPort, c); if (crcp == NULL) return; @@ -187,21 +280,30 @@ static void smartPortSendByte(uint8_t c, uint16_t *crcp) *crcp = crc; } -static void smartPortSendPackage(uint16_t id, uint32_t val) +static void smartPortSendPackageEx(uint8_t frameId, uint8_t* data) { uint16_t crc = 0; - smartPortSendByte(FSSP_DATA_FRAME, &crc); - uint8_t *u8p = (uint8_t*)&id; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - u8p = (uint8_t*)&val; - smartPortSendByte(u8p[0], &crc); - smartPortSendByte(u8p[1], &crc); - smartPortSendByte(u8p[2], &crc); - smartPortSendByte(u8p[3], &crc); + smartPortSendByte(frameId, &crc); + for(unsigned i = 0; i < SMARTPORT_PAYLOAD_SIZE; i++) { + smartPortSendByte(*data++, &crc); + } smartPortSendByte(0xFF - (uint8_t)crc, NULL); } +static void smartPortSendPackage(uint16_t id, uint32_t val) +{ + uint8_t payload[SMARTPORT_PAYLOAD_SIZE]; + uint8_t *dst = payload; + *dst++ = id & 0xFF; + *dst++ = id >> 8; + *dst++ = val & 0xFF; + *dst++ = (val >> 8) & 0xFF; + *dst++ = (val >> 16) & 0xFF; + *dst++ = (val >> 24) & 0xFF; + + smartPortSendPackageEx(FSSP_DATA_FRAME,payload); +} + void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) { telemetryConfig = initialTelemetryConfig; @@ -267,6 +369,196 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } +static void initSmartPortMspReply(int16_t cmd) +{ + smartPortMspReply.buf.ptr = smartPortMspTxBuffer; + smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); + + smartPortMspReply.cmd = cmd; + smartPortMspReply.result = 0; +} + +static void processMspPacket(mspPacket_t* packet) +{ + initSmartPortMspReply(0); + + if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) { + sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR); + } + + // change streambuf direction + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; +} + +/** + * Request frame format: + * - Header: 1 byte + * - Reserved: 2 bits (future use) + * - Error-flag: 1 bit + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - if Error-flag == 0: + * - size: 1 byte + * - payload + * - CRC (request type included) + * - if Error-flag == 1: + * - size: 1 byte (== 1) + * - error: 1 Byte + * - 0: Version mismatch (type=0) + * - 1: Sequence number error + * - 2: MSP error + * - CRC (request type included) + */ +bool smartPortSendMspReply() +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; + uint8_t* p = packet; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; + + sbuf_t* txBuf = &smartPortMspReply.buf; + + // detect first reply packet + if (txBuf->ptr == smartPortMspTxBuffer) { + + // header + uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); + if (smartPortMspReply.result < 0) { + head |= SMARTPORT_MSP_ERROR_FLAG; + } + *p++ = head; + + uint8_t size = sbufBytesRemaining(txBuf); + *p++ = size; + + checksum = size ^ smartPortMspReply.cmd; + } + else { + // header + *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK); + } + + while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { + *p = sbufReadU8(txBuf); + checksum ^= *p++; // MSP checksum + } + + // to be continued... + if (p == end) { + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return true; + } + + // nothing left in txBuf, + // append the MSP checksum + *p++ = checksum; + + // pad with zeros + while (p < end) + *p++ = 0; + + smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); + return false; +} + +void smartPortSendErrorReply(uint8_t error, int16_t cmd) +{ + initSmartPortMspReply(cmd); + sbufWriteU8(&smartPortMspReply.buf,error); + smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR; + + sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); + smartPortMspReplyPending = true; +} + +/** + * Request frame format: + * - Header: 1 byte + * - Version: 3 bits + * - Start-flag: 1 bit + * - CSeq: 4 bits + * + * - MSP payload: + * - Size: 1 Byte + * - Type: 1 Byte + * - payload... + * - CRC + */ +void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) +{ + static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; + static uint8_t mspStarted = 0; + static uint8_t lastSeq = 0; + static uint8_t checksum = 0; + static mspPacket_t cmd; + + // re-assemble MSP frame & forward to MSP port when complete + uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; + uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; + + uint8_t head = *p++; + uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; + uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; + + if (version != SMARTPORT_MSP_VERSION) { + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0); + return; + } + + // check start-flag + if (head & SMARTPORT_MSP_START_FLAG) { + + //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error! + uint8_t p_size = *p++; + cmd.cmd = *p++; + cmd.result = 0; + + cmd.buf.ptr = mspBuffer; + cmd.buf.end = mspBuffer + p_size; + + checksum = p_size ^ cmd.cmd; + mspStarted = 1; + } else if (!mspStarted) { + // no start packet yet, throw this one away + return; + } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { + // packet loss detected! + mspStarted = 0; + return; + } + + // copy payload bytes + while ((p < end) && sbufBytesRemaining(&cmd.buf)) { + checksum ^= *p; + sbufWriteU8(&cmd.buf,*p++); + } + + // reached end of smart port frame + if (p == end) { + lastSeq = seq; + return; + } + + // last byte must be the checksum + if (checksum != *p) { + mspStarted = 0; + smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd); + return; + } + + // end of MSP packet reached + mspStarted = 0; + sbufSwitchToReader(&cmd.buf,mspBuffer); + + processMspPacket(&cmd); +} + void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); @@ -292,6 +584,17 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortFrameReceived) { + smartPortFrameReceived = false; + // do not check the physical ID here again + // unless we start receiving other sensors' packets + if(smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { + + // Pass only the payload: skip sensorId & frameId + handleSmartPortMspFrame(&smartPortRxBuffer); + } + } + while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { @@ -299,6 +602,12 @@ void handleSmartPortTelemetry(void) return; } + if(smartPortMspReplyPending) { + smartPortMspReplyPending = smartPortSendMspReply(); + smartPortHasRequest = 0; + return; + } + // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back @@ -308,7 +617,7 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; - uint32_t tmp2; + uint32_t tmp2 = 0; static uint8_t t1Cnt = 0; static uint8_t t2Cnt = 0; @@ -455,13 +764,10 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif - } - else if (feature(FEATURE_GPS)) { + } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } - - else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index dbddd6850c..3723a7bc65 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -48,7 +48,6 @@ extern uint32_t APP_Rx_ptr_in; APP TX is the circular buffer for data that is transmitted from the APP (host) to the USB device (flight controller). */ -#define APP_TX_DATA_SIZE 1024 static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; static uint32_t APP_Tx_ptr_out = 0; static uint32_t APP_Tx_ptr_in = 0; @@ -195,7 +194,9 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - while (CDC_Send_FreeBytes() <= 0); + while (CDC_Send_FreeBytes() == 0) { + delay(1); + } } return USBD_OK; @@ -247,15 +248,11 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) return USBD_FAIL; } - __disable_irq(); - for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; } - __enable_irq(); - return USBD_OK; } diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index 87481c2ef1..d062c1e431 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -42,19 +42,21 @@ /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ #ifdef USE_USB_OTG_HS - #define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ - #define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL*8 */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #else - #define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ - #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ +#define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ - #define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ - #define APP_RX_DATA_SIZE 1024 /* Total size of IN buffer: +#define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ +#define APP_RX_DATA_SIZE 2048 /* Total size of IN (outbound from FC) buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ +#define APP_TX_DATA_SIZE 2048 /* total size of the OUT (inbound to FC) buffer */ #endif /* USE_USB_OTG_HS */ #define APP_FOPS VCP_fops diff --git a/src/test/SpMsp.lua b/src/test/SpMsp.lua new file mode 100644 index 0000000000..93b47131be --- /dev/null +++ b/src/test/SpMsp.lua @@ -0,0 +1,207 @@ +-- +-- Test script for the MSP/SPORT bridge +-- + +-- Protocol version +SPORT_MSP_VERSION = 1 + +-- Sensor ID used by the local LUA script +LOCAL_SENSOR_ID = 0x0D + +-- Sensor ID used by the MSP server (BF, CF, MW, etc...) +REMOTE_SENSOR_ID = 0x1B + +REQUEST_FRAME_ID = 0x30 +REPLY_FRAME_ID = 0x32 + +-- Sequence number for next MSP/SPORT packet +local sportMspSeq = 0 + +local mspRxBuf = {} +local mspRxIdx = 1 +local mspRxCRC = 0 +local mspStarted = false + +-- Stats +mspRequestsSent = 0 +mspRepliesReceived = 0 +mspPkRxed = 0 +mspErrorPk = 0 +mspStartPk = 0 +mspOutOfOrder = 0 +mspCRCErrors = 0 + +local function mspResetStats() + mspRequestsSent = 0 + mspRepliesReceived = 0 + mspPkRxed = 0 + mspErrorPk = 0 + mspStartPk = 0 + mspOutOfOrderPk = 0 + mspCRCErrors = 0 +end + +local function mspSendRequest(cmd) + + local dataId = 0 + dataId = sportMspSeq -- sequence number + dataId = dataId + bit32.lshift(1,4) -- start flag + dataId = dataId + bit32.lshift(SPORT_MSP_VERSION,5) -- MSP/SPORT version + -- size is 0 for now, no need to add it to dataId + -- dataId = dataId + bit32.lshift(0,8) + sportMspSeq = bit32.band(sportMspSeq + 1, 0x0F) + + local value = 0 + value = bit32.band(cmd,0xFF) -- MSP command + value = value + bit32.lshift(cmd,8) -- CRC + + mspRequestsSent = requestsSent + 1 + return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) +end + +local function mspReceivedReply(payload) + + mspPkRxed = mspPkRxed + 1 + + local idx = 1 + local head = payload[idx] + local err_flag = (bit32.band(head,0x20) ~= 0) + idx = idx + 1 + + if err_flag then + -- error flag set + mspStarted = false + + mspErrorPk = mspErrorPk + 1 + + -- return error + -- CRC checking missing + + --return payload[idx] + return nil + end + + local start = (bit32.band(head,0x10) ~= 0) + local seq = bit32.band(head,0x0F) + + if start then + -- start flag set + mspRxIdx = 1 + mspRxBuf = {} + + mspRxSize = payload[idx] + mspRxCRC = mspRxSize + idx = idx + 1 + mspStarted = true + + mspStartPk = mspStartPk + 1 + + elseif not mspStarted then + mspOutOfOrder = mspOutOfOrder + 1 + return nil + + elseif bit32.band(lastSeq+1,0x0F) ~= seq then + mspOutOfOrder = mspOutOfOrder + 1 + mspStarted = false + return nil + end + + while (idx <= 6) and (mspRxIdx <= mspRxSize) do + mspRxBuf[mspRxIdx] = payload[idx] + mspRxCRC = bit32.bxor(mspRxCRC,payload[idx]) + mspRxIdx = mspRxIdx + 1 + idx = idx + 1 + end + + if idx > 6 then + lastRxSeq = seq + return + end + + -- check CRC + if mspRxCRC ~= payload[idx] then + mspStarted = false + mspCRCErrors = mspCRCErrors + 1 + end + + mspRepliesReceived = mspRepliesReceived + 1 + mspStarted = false + return mspRxBuf +end + +local function mspPollReply() + local sensorId, frameId, dataId, value = sportTelemetryPop() + if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then + + local payload = {} + payload[1] = bit32.band(dataId,0xFF) + dataId = bit32.rshift(dataId,8) + payload[2] = bit32.band(dataId,0xFF) + + payload[3] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[4] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[5] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[6] = bit32.band(value,0xFF) + + return mspReceivedReply(payload) + end +end + +local lastReqTS = 0 + +local function run(event) + + local now = getTime() + + if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then + requestsSent = 0 + repliesReceived = 0 + mspReceivedReply_cnt = 0 + mspReceivedReply_cnt1 = 0 + mspReceivedReply_cnt2 = 0 + mspReceivedReply_cnt3 = 0 + end + + lcd.clear() + + -- do we have valid telemetry data? + if getValue("rssi") > 0 then + + -- draw screen + lcd.drawText(1,11,"Requests:",0) + lcd.drawNumber(60,11,mspRequestsSent) + + lcd.drawText(1,21,"Replies:",0) + lcd.drawNumber(60,21,mspRepliesReceived) + + lcd.drawText(1,31,"PkRxed:",0) + lcd.drawNumber(30,31,mspPkRxed) + + lcd.drawText(1,41,"ErrorPk:",0) + lcd.drawNumber(30,41,mspErrorPk) + + lcd.drawText(71,31,"StartPk:",0) + lcd.drawNumber(100,31,mspStartPk) + + lcd.drawText(71,41,"OutOfOrder:",0) + lcd.drawNumber(100,41,mspOutOfOrder) + + lcd.drawText(1,51,"CRCErrors:",0) + lcd.drawNumber(30,51,mspCRCErrors) + + -- last request is at least 2s old + if lastReqTS + 200 <= now then + mspSendRequest(117) -- MSP_PIDNAMES + lastReqTS = now + end + else + lcd.drawText(20,30,"No telemetry signal", XXLSIZE + BLINK) + end + + pollReply() +end + +return {run=run} From fb518ce37bf38e1a2c0e264ce9693a47603578a7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 03:24:05 +0900 Subject: [PATCH 15/40] Revert -flto option --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index c6ccb2b20f..73683454fe 100644 --- a/Makefile +++ b/Makefile @@ -737,7 +737,7 @@ OPTIMIZE = -Os ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) else -LTO_FLAGS = -fuse-linker-plugin $(OPTIMIZE) +LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) endif endif From 10427cb63c5e11b00af666bd3d1442f918b90a4d Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 4 Nov 2016 08:57:15 +0900 Subject: [PATCH 16/40] Minor work --- src/main/io/cms.c | 110 +---------------------------------- src/main/io/vtx_smartaudio.c | 39 ++++++++----- 2 files changed, 26 insertions(+), 123 deletions(-) diff --git a/src/main/io/cms.c b/src/main/io/cms.c index d4504f5d9d..f4fbd648a4 100644 --- a/src/main/io/cms.c +++ b/src/main/io/cms.c @@ -191,7 +191,7 @@ void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc) // #define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) +#define RIGHT_MENU_COLUMN(p) ((p)->cols - 10) #define MAX_MENU_ITEMS(p) ((p)->rows - 2) displayPort_t currentDisplay; @@ -873,114 +873,6 @@ void cmsx_InfoInit(void) } } -#if 0 -#ifdef VTX_SMARTAUDIO -#include "io/vtx_smartaudio.h" - -static const char * const smartAudioBandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; -OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; - -static const char * const smartAudioChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; - -OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; - -static const char * const smartAudioPowerNames[] = { - "---", - " 25", - "200", - "500", - "800", -}; - -OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; - -static const char * const smartAudioTxModeNames[] = { - "------", - "PIT-OR", - "PIT-IR", - "ACTIVE", -}; - -OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; - -OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; - -static const char * const smartAudioOpModelNames[] = { - "FREE", - "PIT", -}; - -OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; - -static const char * const smartAudioPitFModeNames[] = { - "IN-RANGE", - "OUT-RANGE", -}; - -OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; - -OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; - -OSD_Entry menu_smartAudioConfig[] = { - { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL, 0 }, - { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel, 0 }, - { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode, 0 }, - { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq, 0 }, // OME_Poll_UINT16 - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -static const char * const smartAudioStatusNames[] = { - "OFFLINE", - "ONLINE V1", - "ONLINE V2", -}; - -OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; -OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; - -OSD_Entry menu_smartAudioStats[] = { - { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL, 0 }, - { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline, 0 }, - { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate, 0 }, - { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre, 0 }, - { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen, 0 }, - { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr, 0 }, - { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -OSD_Entry menu_vtxSmartAudio[] = -{ - { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL, 0 }, - { smartAudioStatusString, OME_Label, NULL, NULL, 0 }, - { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode, 0 }, - { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand, 0 }, - { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan, 0 }, - { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq, 0 }, - { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower, 0 }, - { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig[0], 0 }, - { "STAT", OME_Submenu, cmsMenuChange, &menu_smartAudioStats[0], 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; -#endif // VTX_SMARTAUDIO -#endif // 0 - // Features OSD_Entry menuFeatures[] = diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 4b401b384b..bf50711e49 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -637,10 +637,11 @@ bool smartAudioInit() return false; } - smartAudioOpModel = masterConfig.vtx_smartaudio_opmodel; + saOpmodel = masterConfig.vtx_smartaudio_opmodel; return true; } + void smartAudioProcess(uint32_t now) { static bool initialSent = false; @@ -689,28 +690,38 @@ void smartAudioProcess(uint32_t now) } #ifdef CMS -char smartAudioStatusString[31] = "- - ---- --- ---- -"; +// m bc ffff ppp +// 0123456789012 +char saStatusString[31] = "- -- ---- ---"; +uint8_t saOpmodel = 0; uint8_t smartAudioBand = 0; uint8_t smartAudioChan = 0; uint8_t smartAudioPower = 0; uint16_t smartAudioFreq = 0; -static void smartAudioUpdateStatusString(void) +static void saUpdateStatusString(void) { if (sa_vers == 0) return; - tfp_sprintf(smartAudioStatusString, "%c %d %4d %3d ", - "ABEFR"[sa_chan / 8], - (sa_chan % 8) + 1, - saFreqTable[sa_chan / 8][sa_chan % 8], - (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower); - - if (sa_vers == 2) - tfp_sprintf(&smartAudioStatusString[13], "%4d", sa_pitfreq); - else - tfp_sprintf(&smartAudioStatusString[13], "%4s", "----"); + saStatusString[0] = "-FP"[saOpmodel]; + saStatusString[2] = "ABEFR"[sa_chan / 8]; + saStatusString[3] = '1' + (sa_chan % 8); + tfp_sprintf(&smartAudioStatusString[5], "%4d", + saFreqTable[sa_chan / 8][sa_chan % 8]); + saStatusString[9] = ' '; + if (sa_opmode & SA_MODE_GET_PITMODE) { + saStatusString[10] = 'P'; + if (sa_opmode & SA_MODE_GET_IN_RANGE_PITMODE) { + saStatusString[10] = 'I'; + } else { + saStatusString[10] = 'O'; + } + saStatusString[11] = 0; + } else { + tfp_sprintf(&saStatusString[10], "%3d", (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower); + } } long smartAudioConfigureBandByGvar(displayPort_t *pDisp, void *self) @@ -938,7 +949,7 @@ OSD_Entry menu_smartAudioStats[] = { OSD_Entry cmsx_menuVtxSmartAudio[] = { - { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL, 0 }, + { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, { smartAudioStatusString, OME_Label, NULL, NULL, 0 }, { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode, 0 }, { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand, 0 }, From e39f03597e2cc48881fa65ab070168132101bce4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 15 Nov 2016 19:03:59 +0900 Subject: [PATCH 17/40] WIP --- src/main/config/config_master.h | 4 - src/main/fc/config.c | 4 - src/main/io/serial_cli.c | 4 - src/main/io/vtx_smartaudio.c | 809 +++++++++++++++++++------------- src/main/io/vtx_smartaudio.h | 4 +- 5 files changed, 474 insertions(+), 351 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 6482e28bbf..1c4332fc82 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -204,10 +204,6 @@ typedef struct master_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; #endif -#ifdef VTX_SMARTAUDIO - uint8_t vtx_smartaudio_opmodel; // 0=FREE, 1=PIT -#endif - #ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 0b1a2535c3..35874be325 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -744,10 +744,6 @@ void createDefaultConfig(master_t *config) config->vtx_mhz = 5740; //F0 #endif -#ifdef VTX_SMARTAUDIO - config->vtx_smartaudio_opmodel = 1; // PIT -#endif - #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 100bd0d258..57c8719103 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -932,10 +932,6 @@ const clivalue_t valueTable[] = { { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } }, #endif -#ifdef VTX_SMARTAUDIO - { "vtx_smartaudio_opmodel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_smartaudio_opmodel, .config.minmax = { 0, 1 } }, -#endif - #ifdef MAG { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 7539db8da5..d87dc39ba9 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -9,6 +9,7 @@ #include "cms/cms.h" #include "cms/cms_types.h" +#include "string.h" #include "common/printf.h" #include "drivers/system.h" #include "drivers/serial.h" @@ -28,8 +29,6 @@ #include "build/build_config.h" -#define SMARTAUDIO_EXTENDED_API - #define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR @@ -60,6 +59,7 @@ enum { SA_CMD_GET_SETTINGS_V2 = 0x09 // Response only } smartAudioCommand_e; +// This is not a good design #define SACMD(cmd) (((cmd) << 1) | 1) // opmode flags, GET side @@ -68,53 +68,42 @@ enum { #define SA_MODE_GET_IN_RANGE_PITMODE 4 #define SA_MODE_GET_OUT_RANGE_PITMODE 8 #define SA_MODE_GET_UNLOCK 16 +#define SA_MODE_GET_DEFERRED_FREQ 32 + +#define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE) +#define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE)) +#define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE)) // 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_IN_RANGE_PITMODE 1 // Immediate +#define SA_MODE_SET_OUT_RANGE_PITMODE 2 // Immediate +#define SA_MODE_CLR_PITMODE 4 // Immediate #define SA_MODE_SET_UNLOCK 8 #define SA_MODE_SET_LOCK 0 // ~UNLOCK +#define SA_MODE_SET_DEFERRED_FREQ 16 // SetFrequency flags, for pit mode frequency manipulation #define SA_FREQ_GETPIT (1 << 14) #define SA_FREQ_SETPIT (1 << 15) +#define SA_FREQ_MASK (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT)) -// Error counters, may be good for post production debugging. -uint16_t saerr_badpre = 0; -uint16_t saerr_badlen = 0; -uint16_t saerr_crc = 0; -uint16_t saerr_oooresp = 0; +// Statistical counters, for user side trouble shooting. -// Receive frame reassembly buffer -#define SA_MAX_RCVLEN 11 -static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard +typedef struct smartAudioStat_s { + uint16_t badpre; + uint16_t badlen; + uint16_t crc; + uint16_t ooopresp; + uint16_t badcode; +} smartAudioStat_t; -// 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; -} +static smartAudioStat_t saStat = { + .badpre = 0, + .badlen = 0, + .crc = 0, + .ooopresp = 0, + .badcode = 0, +}; // The band/chan to frequency table // XXX Should really be consolidated among different vtx drivers @@ -140,49 +129,79 @@ static saPowerTable_t saPowerTable[] = { { 800, 40, 3 }, }; -// Driver defined modes - -#define SA_OPMODEL_FREE 0 // Power up transmitting -#define SA_OPMODEL_PIT 1 // Power up in pit mode - -#define SA_TXMODE_NODEF 0 -#define SA_TXMODE_PIT_OUTRANGE 1 -#define SA_TXMODE_PIT_INRANGE 2 -#define SA_TXMODE_ACTIVE 3 - // Last received device ('hard') 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; +typedef struct smartAudioDevice_s { + int8_t version; + int8_t chan; + int8_t power; + int8_t mode; + uint16_t freq; + uint16_t orfreq; +} smartAudioDevice_t; -static int8_t sa_overs = 0; -static int8_t sa_ochan; -static int8_t sa_opower; -static int8_t sa_oopmode; -static uint16_t sa_ofreq; +static smartAudioDevice_t saDevice = { + .version = 0, + .chan = -1, + .power = -1, + .mode = 0, + .freq = 0, + .orfreq = 0, +}; + +static smartAudioDevice_t saDevicePrev = { + .version = 0, +}; + +static uint8_t saLockMode = SA_MODE_SET_UNLOCK; + +// Receive frame reassembly buffer +#define SA_MAX_RCVLEN 11 +static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard + +// +// CRC8 computations +// + +#define POLYGEN 0xd5 + +static uint8_t CRC8(const uint8_t *data, const 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; +} -static uint16_t sa_pitfreq = 0; static void smartAudioPrintSettings(void) { #ifdef SMARTAUDIO_DPRINTF - - - 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(("Current status: version: %d\r\n", saDevice.version)); + dprintf((" mode(0x%x): fmode=%s", saDevice.mode, (saDevice.mode & 1) ? "freq" : "chan")); + dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off")); + dprintf((" inb=%s", (saDevice.mode & 4) ? "on " : "off")); + dprintf((" outb=%s", (saDevice.mode & 8) ? "on " : "off")); + dprintf((" lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked")); + dprintf((" deferred=%s\r\n", (saDevice.mode & 32) ? "on" : "off")); + dprintf((" chan: %d ", saDevice.chan)); + dprintf(("freq: %d ", saDevice.freq)); + dprintf(("power: %d ", saDevice.power)); + dprintf(("pitfreq: %d ", saDevice.orfreq)); dprintf(("\r\n")); - #endif } @@ -197,13 +216,15 @@ static int saDacToPowerIndex(int dac) return(3); } +// // Autobauding +// #define SMARTBAUD_MIN 4800 #define SMARTBAUD_MAX 4950 -uint16_t smartAudioSmartbaud = SMARTBAUD_MIN; -static int adjdir = 1; // -1=going down, 1=going up -static int baudstep = 50; +uint16_t sa_smartbaud = SMARTBAUD_MIN; +static int sa_adjdir = 1; // -1=going down, 1=going up +static int sa_baudstep = 50; #define SMARTAUDIO_CMD_TIMEOUT 120 @@ -220,7 +241,7 @@ static void saAutobaud(void) #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", - smartAudioSmartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent))); + sa_smartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent))); #endif if (((sa_pktrcvd * 100) / sa_pktsent) >= 70) { @@ -232,31 +253,35 @@ static void saAutobaud(void) dprintf(("autobaud: adjusting\r\n")); - if ((adjdir == 1) && (smartAudioSmartbaud == SMARTBAUD_MAX)) { - adjdir = -1; + if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) { + sa_adjdir = -1; dprintf(("autobaud: now going down\r\n")); - } else if ((adjdir == -1 && smartAudioSmartbaud == SMARTBAUD_MIN)) { - adjdir = 1; + } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) { + sa_adjdir = 1; dprintf(("autobaud: now going up\r\n")); } - smartAudioSmartbaud += baudstep * adjdir; + sa_smartbaud += sa_baudstep * sa_adjdir; - dprintf(("autobaud: %d\r\n", smartAudioSmartbaud)); + dprintf(("autobaud: %d\r\n", sa_smartbaud)); - smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, smartAudioSmartbaud); + smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud); sa_pktsent = 0; sa_pktrcvd = 0; } -// Transport level protocol variables +// Transport level 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 +#ifdef CMS +void smartAudioCmsUpdate(void); +#endif + static void saProcessResponse(uint8_t *buf, int len) { uint8_t resp = buf[0]; @@ -266,7 +291,7 @@ static void saProcessResponse(uint8_t *buf, int len) } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) { sa_outstanding = SA_CMD_NONE; } else { - saerr_oooresp++; + saStat.ooopresp++; dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); } @@ -276,58 +301,22 @@ static void saProcessResponse(uint8_t *buf, int len) 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]; - - if ((sa_overs == sa_vers) - && (sa_ochan == sa_chan) - && (sa_opower == sa_power) - && (sa_oopmode == sa_opmode) - && (sa_ofreq == sa_freq)) - break; - - // Debug - smartAudioPrintSettings(); - - // Export current settings for CMS + saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; + saDevice.chan = buf[2]; + saDevice.power = buf[3]; + saDevice.mode = buf[4]; + saDevice.freq = (buf[5] << 8)|buf[6]; #ifdef CMS - smartAudioBand = (sa_chan / 8) + 1; - smartAudioChan = (sa_chan % 8) + 1; - smartAudioFreq = saFreqTable[sa_chan / 8][sa_chan % 8]; - - if ((sa_opmode & SA_MODE_GET_PITMODE) == 0) { - smartAudioTxMode = SA_TXMODE_ACTIVE; - } else if (sa_opmode & SA_MODE_GET_IN_RANGE_PITMODE) { - smartAudioTxMode = SA_TXMODE_PIT_INRANGE; - } else { - smartAudioTxMode = SA_TXMODE_PIT_OUTRANGE; - } - - saUpdateStatusString(); - - if (sa_vers == 2) { - smartAudioPower = sa_power + 1; // XXX Take care V1 - } else { - smartAudioPower = saDacToPowerIndex(sa_power) + 1; - } + // Export current device status for CMS + smartAudioCmsUpdate(); #endif - #ifdef SMARTAUDIO_DEBUG_MONITOR - debug[0] = sa_vers * 100 + sa_opmode; - debug[1] = sa_chan; - debug[2] = sa_freq; - debug[3] = sa_power; + debug[0] = saDevice.version * 100 + saDevice.mode; + debug[1] = saDevice.chan; + debug[2] = saDevice.freq; + debug[3] = saDevice.power; #endif - sa_overs = sa_vers; - sa_ochan = sa_chan; - sa_opower = sa_power; - sa_oopmode = sa_opmode; - sa_ofreq = sa_freq; - break; case SA_CMD_SET_POWER: // Set Power @@ -343,24 +332,41 @@ static void saProcessResponse(uint8_t *buf, int len) uint16_t freq = (buf[2] << 8)|buf[3]; if (freq & SA_FREQ_GETPIT) { - sa_pitfreq = freq & ~SA_FREQ_GETPIT; - dprintf(("saProcessResponse: GETPIT freq %d\r\n", sa_pitfreq)); -#ifdef CMS - saUpdateStatusString(); -#endif + saDevice.orfreq = freq & SA_FREQ_MASK; + dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq)); } else if (freq & SA_FREQ_SETPIT) { - dprintf(("saProcessResponse: SETPIT freq %d\r\n", freq)); + saDevice.orfreq = freq & SA_FREQ_MASK; + dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq)); } else { - dprintf(("saProcessResponse: GETFREQ freq %d\r\n", freq)); + saDevice.freq = freq; + dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq)); } break; case SA_CMD_SET_MODE: // Set Mode dprintf(("resp SET_MODE 0x%x\r\n", buf[2])); break; + + default: + saStat.badcode++; + return; } + + // Debug + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + smartAudioPrintSettings(); + + saDevicePrev = saDevice; + +#ifdef CMS + saUpdateStatusString(); +#endif } +// +// Datalink +// + static void saReceiveFramer(uint8_t c) { @@ -389,7 +395,7 @@ static void saReceiveFramer(uint8_t c) if (c == 0x55) { state = S_WAITRESP; } else { - saerr_badpre++; + saStat.badpre++; state = S_WAITPRE1; } break; @@ -404,7 +410,7 @@ static void saReceiveFramer(uint8_t c) len = c; if (len > SA_MAX_RCVLEN - 2) { - saerr_badlen++; + saStat.badlen++; state = S_WAITPRE1; } else if (len == 0) { state = S_WAITCRC; @@ -415,6 +421,7 @@ static void saReceiveFramer(uint8_t c) break; case S_DATA: + // XXX Should check buffer overflow (-> saerr_overflow) sa_rbuf[2 + dlen] = c; if (++dlen == len) { state = S_WAITCRC; @@ -429,15 +436,13 @@ static void saReceiveFramer(uint8_t c) } else if (sa_rbuf[0] & 1) { // Command echo (may be) } else { - saerr_crc++; + saStat.crc++; } state = S_WAITPRE1; break; } } -// Output framer - static void saSendFrame(uint8_t *buf, int len) { int i; @@ -453,7 +458,6 @@ static void saSendFrame(uint8_t *buf, int len) sa_pktsent++; } - /* * Retransmission and command queuing * @@ -557,7 +561,13 @@ static void saSetFreq(uint16_t freq) { static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 }; - dprintf(("smartAudioSetFreq: freq %d\r\n", freq)); + if (freq & SA_FREQ_GETPIT) { + dprintf(("smartAudioSetFreq: GETPIT\r\n")); + } else if (freq & SA_FREQ_SETPIT) { + dprintf(("smartAudioSetFreq: SETPIT %d\r\n", freq & SA_FREQ_MASK)); + } else { + dprintf(("smartAudioSetFreq: SET %d\r\n", freq)); + } buf[4] = (freq >> 8) & 0xff; buf[5] = freq & 0xff; @@ -566,7 +576,6 @@ static void saSetFreq(uint16_t freq) saQueueCmd(buf, 7); } -#ifdef SMARTAUDIO_EXTENDED_API static void saSetPitFreq(uint16_t freq) { saSetFreq(freq | SA_FREQ_SETPIT); @@ -576,9 +585,8 @@ static void saGetPitFreq(void) { saSetFreq(SA_FREQ_GETPIT); } -#endif -void smartAudioSetBandChan(uint8_t band, uint8_t chan) +void saSetBandChan(uint8_t band, uint8_t chan) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 }; @@ -592,19 +600,19 @@ static void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; - buf[4] = mode & 0x1f; + buf[4] = (mode & 0x1f)|saLockMode; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } -void smartAudioSetPowerByIndex(uint8_t index) +void saSetPowerByIndex(uint8_t index) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; - dprintf(("smartAudioSetPowerByIndex: index %d\r\n", index)); + dprintf(("saSetPowerByIndex: index %d\r\n", index)); - if (sa_vers == 0) { + if (saDevice.version == 0) { // Unknown or yet unknown version. return; } @@ -612,7 +620,7 @@ void smartAudioSetPowerByIndex(uint8_t index) if (index > 3) return; - buf[4] = (sa_vers == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; + buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } @@ -638,15 +646,11 @@ bool smartAudioInit() return false; } - // saOpmodel = masterConfig.vtx_smartaudio_opmodel; - return true; } void smartAudioProcess(uint32_t now) { - debug[0]++; - static bool initialSent = false; if (smartAudioSerialPort == NULL) @@ -664,6 +668,7 @@ void smartAudioProcess(uint32_t now) saGetSettings(); saGetPitFreq(); saSendQueue(); + saSetPitFreq(5705); initialSent = true; return; } @@ -671,70 +676,141 @@ void smartAudioProcess(uint32_t now) if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out + // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); saResendCmd(); } else if (!saQueueEmpty()) { // Command pending. Send it. + // dprintf(("process: sending queue\r\n")); saSendQueue(); } else if (now - sa_lastTransmission >= 1000) { // Heart beat for autobauding - -#ifdef SMARTAUDIO_PITMODE_DEBUG - static int turn = 0; - if ((turn++ % 2) == 0) { - saGetSettings(); - } else { - smartAudioPitMode(); - } -#else + //dprintf(("process: sending heartbeat\r\n")); saGetSettings(); -#endif saSendQueue(); } } #ifdef CMS -// m bc ffff ppp -// 0123456789012 -char saStatusString[31] = "- -- ---- ---"; -uint8_t saOpmodel = 0; +// CMS menu variables + +// Operational Model and RF modes (CMS) + +#define SA_OPMODEL_UNDEF 0 // Not known yet +#define SA_OPMODEL_FREE 1 // Power up transmitting +#define SA_OPMODEL_PIT 2 // Power up in pit mode + +#define SA_TXMODE_NODEF 0 +#define SA_TXMODE_PIT_OUTRANGE 1 +#define SA_TXMODE_PIT_INRANGE 2 +#define SA_TXMODE_ACTIVE 3 + +uint8_t saCmsOpmodel = SA_OPMODEL_UNDEF; uint8_t smartAudioBand = 0; uint8_t smartAudioChan = 0; uint8_t smartAudioPower = 0; uint16_t smartAudioFreq = 0; +uint8_t smartAudioOpModel = 0; +uint8_t smartAudioStatus = 0; +uint8_t smartAudioPower; +uint8_t smartAudioTxMode; // RF state; ACTIVE, PIR, POR +uint8_t smartAudioPitFMode; // In-Range or Out-Range +uint16_t smartAudioORFreq = 0; // POR frequency +uint8_t smartAudioFreqMode; // Channel or User defined +uint16_t smartAudioUserFreq = 0; // User defined frequency + + +void smartAudioCmsUpdate(void) +{ + if (saCmsOpmodel == SA_OPMODEL_UNDEF) { + // This is a first valid response to GET_SETTINGS. + saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SA_OPMODEL_PIT : SA_OPMODEL_FREE; + } + + smartAudioBand = (saDevice.chan / 8) + 1; + smartAudioChan = (saDevice.chan % 8) + 1; + smartAudioFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + + if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { + smartAudioTxMode = SA_TXMODE_ACTIVE; + } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + smartAudioTxMode = SA_TXMODE_PIT_INRANGE; + } else { + smartAudioTxMode = SA_TXMODE_PIT_OUTRANGE; + } + + if (saDevice.version == 2) { + smartAudioPower = saDevice.power + 1; // XXX Take care V1 + } else { + smartAudioPower = saDacToPowerIndex(saDevice.power) + 1; + } + + saUpdateStatusString(); +} + +char saCmsStatusString[31] = "- -- ---- ---"; +// m bc ffff ppp +// 0123456789012 + +long saConfigureOpModelByGvar(displayPort_t *, const void *self); +long saConfigurePitFModeByGvar(displayPort_t *, const void *self); +long saConfigureBandByGvar(displayPort_t *, const void *self); +long saConfigureChanByGvar(displayPort_t *, const void *self); +long saConfigurePowerByGvar(displayPort_t *, const void *self); + static void saUpdateStatusString(void) { - if (sa_vers == 0) + if (saDevice.version == 0) return; -smartAudioStatus = sa_vers; +// XXX These should be done somewhere else +if (smartAudioStatus == 0 && saDevice.version != 0) + smartAudioStatus = saDevice.version; +if (smartAudioORFreq == 0 && saDevice.orfreq != 0) + smartAudioORFreq = saDevice.orfreq; +if (smartAudioUserFreq == 0 && saDevice.freq != 0) + smartAudioUserFreq = saDevice.freq; +if (smartAudioOpModel == 0 && saCmsOpmodel != 0) + smartAudioOpModel = saCmsOpmodel + 1; - saStatusString[0] = "-FP"[saOpmodel]; - saStatusString[2] = "ABEFR"[sa_chan / 8]; - saStatusString[3] = '1' + (sa_chan % 8); - tfp_sprintf(&smartAudioStatusString[5], "%4d", - saFreqTable[sa_chan / 8][sa_chan % 8]); - saStatusString[9] = ' '; - if (sa_opmode & SA_MODE_GET_PITMODE) { - saStatusString[10] = 'P'; - if (sa_opmode & SA_MODE_GET_IN_RANGE_PITMODE) { - saStatusString[10] = 'I'; + saCmsStatusString[0] = "-FP"[saCmsOpmodel]; + saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8]; + saCmsStatusString[3] = '1' + (saDevice.chan % 8); + + if ((saDevice.mode & SA_MODE_GET_PITMODE) + && (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)) + tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.orfreq); + else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) + tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq); + else + tfp_sprintf(&saCmsStatusString[5], "%4d", + saFreqTable[saDevice.chan / 8][saDevice.chan % 8]); + + saCmsStatusString[9] = ' '; + + if (saDevice.mode & SA_MODE_GET_PITMODE) { + saCmsStatusString[10] = 'P'; + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + saCmsStatusString[11] = 'I'; } else { - saStatusString[10] = 'O'; + saCmsStatusString[11] = 'O'; } - saStatusString[11] = 0; + saCmsStatusString[12] = 'R'; + saCmsStatusString[13] = 0; } else { - tfp_sprintf(&saStatusString[10], "%3d", (sa_vers == 2) ? saPowerTable[sa_power].rfpower : saPowerTable[saDacToPowerIndex(sa_power)].rfpower); + tfp_sprintf(&saCmsStatusString[10], "%3d", (saDevice.version == 2) ? saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower); } } -long smartAudioConfigureBandByGvar(displayPort_t *pDisp, const void *self) +static long sacms_SetupTopMenu(void); + +long saConfigureBandByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (sa_vers == 0) { + if (saDevice.version == 0) { // Bounce back; not online yet smartAudioBand = 0; return 0; @@ -746,17 +822,19 @@ long smartAudioConfigureBandByGvar(displayPort_t *pDisp, const void *self) return 0; } - smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); return 0; } -long smartAudioConfigureChanByGvar(displayPort_t *pDisp, const void *self) +long saConfigureChanByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (sa_vers == 0) { +dprintf(("saConfigureBandByGvar: called\r\n")); + + if (saDevice.version == 0) { // Bounce back; not online yet smartAudioChan = 0; return 0; @@ -768,17 +846,19 @@ long smartAudioConfigureChanByGvar(displayPort_t *pDisp, const void *self) return 0; } - smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); +dprintf(("saConfigureBandByGvar: calling saSetBandChan\r\n")); + + saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); return 0; } -long smartAudioConfigurePowerByGvar(displayPort_t *pDisp, const void *self) +long saConfigurePowerByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (sa_vers == 0) { + if (saDevice.version == 0) { // Bounce back; not online yet smartAudioPower = 0; return 0; @@ -790,51 +870,17 @@ long smartAudioConfigurePowerByGvar(displayPort_t *pDisp, const void *self) return 0; } - smartAudioSetPowerByIndex(smartAudioPower - 1); + saSetPowerByIndex(smartAudioPower - 1); return 0; } -long smartAudioSetTxModeByGvar(displayPort_t *pDisp, const void *self) +long saConfigurePitFModeByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (sa_vers != 2) { - // Bounce back; not online yet or can't handle mode (V1) - smartAudioTxMode = SA_TXMODE_NODEF; - return 0; - } - - if (smartAudioTxMode == 0) { - // Bouce back; no going back to undef state - ++smartAudioTxMode; - return 0; - } - - if (smartAudioTxMode == SA_TXMODE_ACTIVE) { - if (smartAudioOpModel == SA_OPMODEL_FREE) { - saSetMode(SA_MODE_CLR_PITMODE); - } else { - if (smartAudioPitFMode == 0) - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); - else - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); - } - } else { - if ((sa_opmode & SA_MODE_GET_PITMODE) == 0) { - // Can't go back to pit mode - smartAudioTxMode = SA_TXMODE_ACTIVE; - } - } - - return 0; -} - -long smartAudioConfigurePitFModeByGvar(displayPort_t *pDisp, const void *self) -{ - UNUSED(pDisp); - UNUSED(self); + dprintf(("saConfigurePitFmodeByGbar: smartAudioPitFMode %d\r\n", smartAudioPitFMode)); if (smartAudioPitFMode == 0) { saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); @@ -845,118 +891,49 @@ long smartAudioConfigurePitFModeByGvar(displayPort_t *pDisp, const void *self) return 0; } -long smartAudioConfigureOpModelByGvar(displayPort_t *pDisp, const void *self) +long saConfigureOpModelByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - masterConfig.vtx_smartaudio_opmodel = smartAudioOpModel; + uint8_t opmodel = smartAudioOpModel; - if (smartAudioOpModel == SA_OPMODEL_FREE) { + dprintf(("saConfigureOpModelByGvar: opmodel %d\r\n", opmodel)); + + if (opmodel == SA_OPMODEL_FREE) { // VTX should power up transmitting. // Turn off In-Range and Out-Range bits saSetMode(0); - } else { + } else if (opmodel == SA_OPMODEL_PIT) { // VTX should power up in pit mode. // Setup In-Range or Out-Range bits - smartAudioConfigurePitFModeByGvar(pDisp, self); + saConfigurePitFModeByGvar(pDisp, self); } return 0; } -static const char * const smartAudioBandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; -OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; - -static const char * const smartAudioChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; - -OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; - -static const char * const smartAudioPowerNames[] = { - "---", - " 25", - "200", - "500", - "800", -}; - -OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; - -static const char * const smartAudioTxModeNames[] = { - "------", - "PIT-OR", - "PIT-IR", - "ACTIVE", -}; - -OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; - -OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; - -static const char * const smartAudioOpModelNames[] = { - "FREE", - "PIT", -}; - -OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; - -static const char * const smartAudioPitFModeNames[] = { - "IN-RANGE", - "OUT-RANGE", -}; - -OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; - -OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; - -OSD_Entry menu_smartAudioConfigEntries[] = { - { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL, 0 }, - { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel, 0 }, - { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode, 0 }, - { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq, 0 }, // OME_Poll_UINT16 - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } -}; - -CMS_Menu menu_smartAudioConfig = { - .GUARD_text = "XSACFG", - .GUARD_type = OME_MENU, - .onEnter = NULL, - .onExit = NULL, - .onGlobalExit = NULL, - .entries = menu_smartAudioConfigEntries -}; - static const char * const smartAudioStatusNames[] = { - "OFFLINE", - "ONLINE V1", - "ONLINE V2", + "OFFL", + "ONL V1", + "ONL V2", }; -OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, smartAudioStatusNames }; -OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; +OSD_TAB_t cmsEntOnline = { &smartAudioStatus, 2, smartAudioStatusNames }; +OSD_UINT16_t cmsEntBaudrate = { &sa_smartbaud, 0, 0, 0 }; +OSD_UINT16_t cmsEntStatBadpre = { &saStat.badpre, 0, 0, 0 }; +OSD_UINT16_t cmsEntStatBadlen = { &saStat.badlen, 0, 0, 0 }; +OSD_UINT16_t cmsEntStatCrcerr = { &saStat.crc, 0, 0, 0 }; +OSD_UINT16_t cmsEntStatOooerr = { &saStat.ooopresp, 0, 0, 0 }; OSD_Entry menu_smartAudioStatsEntries[] = { - { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL, 0 }, - { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline, DYNAMIC }, - { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate, DYNAMIC }, - { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre, DYNAMIC }, - { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen, DYNAMIC }, - { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr, DYNAMIC }, - { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr, DYNAMIC }, + { "- SA STATS -", OME_Label, NULL, NULL, 0 }, + { "STATUS", OME_TAB, NULL, &cmsEntOnline, DYNAMIC }, + { "BAUDRATE", OME_UINT16, NULL, &cmsEntBaudrate, DYNAMIC }, + { "BADPRE", OME_UINT16, NULL, &cmsEntStatBadpre, DYNAMIC }, + { "BADLEN", OME_UINT16, NULL, &cmsEntStatBadlen, DYNAMIC }, + { "CRCERR", OME_UINT16, NULL, &cmsEntStatCrcerr, DYNAMIC }, + { "OOOERR", OME_UINT16, NULL, &cmsEntStatOooerr, DYNAMIC }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; @@ -970,29 +947,187 @@ CMS_Menu menu_smartAudioStats = { .entries = menu_smartAudioStatsEntries }; -OSD_Entry cmsx_menuVtxSmartAudioEntries[] = +static const char * const smartAudioBandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +OSD_TAB_t cmsEntBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; + +static const char * const smartAudioChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +OSD_TAB_t cmsEntChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; + +static const char * const smartAudioPowerNames[] = { + "---", + "25 ", + "200", + "500", + "800", +}; + +OSD_TAB_t cmsEntPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; + +static const char * const smartAudioTxModeNames[] = { + "------", + "PIT-OR", + "PIT-IR", + "ACTIVE", +}; + +OSD_TAB_t cmsEntTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; + +OSD_UINT16_t cmsEntFreq = { &smartAudioFreq, 5600, 5900, 0 }; + +static const char * const smartAudioOpModelNames[] = { + "----", + "FREE", + "PIT ", +}; + +OSD_TAB_t cmsEntOpModel = { &smartAudioOpModel, 2, &smartAudioOpModelNames[0] }; + +static const char * const smartAudioPitFModeNames[] = { + "IN-R ", + "OUT-R" +}; + +OSD_TAB_t cmsEntPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; + +OSD_UINT16_t cmsEntORFreq = { &smartAudioORFreq, 5000, 5900, 1 }; + +static const char * const smartAudioFreqModeNames[] = { + "CHAN", + "USER" +}; + +OSD_TAB_t cmsEntFreqMode = { &smartAudioFreqMode, 1, smartAudioFreqModeNames }; + +OSD_UINT16_t cmsEntUserFreq = { &smartAudioUserFreq, 5000, 5900, 1 }; + +long saConfigureUserFreqByGvar(displayPort_t *pDisp, const void *self) { - { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, - { "", OME_String, NULL, smartAudioStatusString, DYNAMIC }, - { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode, 0 }, - { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand, 0 }, - { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan, 0 }, - { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq, 0 }, - { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower, 0 }, - { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, - { "STAT", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, + UNUSED(pDisp); + UNUSED(self); + + saSetFreq(smartAudioFreq); + + return 0; +} + +long saConfigureFreqModeByGvar(displayPort_t *pDisp, const void *self) +{ + if (smartAudioFreqMode == 0) { + // CHAN + saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + } else { + // USER + saConfigureUserFreqByGvar(pDisp, self); + } + + sacms_SetupTopMenu(); + + return 0; +} + +long saClearPitMode(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (smartAudioPitFMode == 0) + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); + else + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); + + return 0; +} + +OSD_Entry menu_smartAudioConfigEntries[] = { + { "- SA CONFIG -", OME_Label, NULL, NULL, 0 }, + { "OP MODEL", OME_TAB, saConfigureOpModelByGvar, &cmsEntOpModel, 0 }, + { "PIT FMODE", OME_TAB, saConfigurePitFModeByGvar, &cmsEntPitFMode, 0 }, + { "POR FREQ", OME_UINT16, NULL, &cmsEntORFreq, 0 }, + { "FREQ MODE", OME_TAB, saConfigureFreqModeByGvar, &cmsEntFreqMode, 0 }, + { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu cmsx_menuVtxSmartAudio = { - .GUARD_text = "XVTXSA", +CMS_Menu menu_smartAudioConfig = { + .GUARD_text = "XSACFG", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = cmsx_menuVtxSmartAudioEntries + .entries = menu_smartAudioConfigEntries +}; + +OSD_Entry saMenuFreqModeEntries[] = { + { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "FREQ", OME_UINT16, NULL, &cmsEntUserFreq, 0 }, + { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, + { "START", OME_Funcall, saClearPitMode, NULL, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +OSD_Entry saMenuChanModeEntries[] = +{ + { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "BAND", OME_TAB, saConfigureBandByGvar, &cmsEntBand, 0 }, + { "CHAN", OME_TAB, saConfigureChanByGvar, &cmsEntChan, 0 }, + { "FREQ", OME_UINT16, NULL, &cmsEntFreq, DYNAMIC }, + { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, + { "START", OME_Funcall, saClearPitMode, NULL, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +OSD_Entry saMenuOfflineEntries[] = +{ + { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuVtxSmartAudio; + +static long sacms_SetupTopMenu(void) +{ + if (smartAudioStatus) { + if (smartAudioFreqMode == 0) + cmsx_menuVtxSmartAudio.entries = saMenuChanModeEntries; + else + cmsx_menuVtxSmartAudio.entries = saMenuFreqModeEntries; + } else { + cmsx_menuVtxSmartAudio.entries = saMenuOfflineEntries; + } + + return 0; +} + +CMS_Menu cmsx_menuVtxSmartAudio = { + .GUARD_text = "XVTXSA", + .GUARD_type = OME_MENU, + .onEnter = sacms_SetupTopMenu, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saMenuOfflineEntries, }; #endif // CMS + #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 2cb67e783b..d11ba621a1 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -4,6 +4,7 @@ bool smartAudioInit(); void smartAudioProcess(uint32_t); +#if 0 #ifdef CMS uint16_t smartAudioSmartbaud; @@ -13,8 +14,6 @@ uint16_t saerr_badlen; uint16_t saerr_crcerr; uint16_t saerr_oooresp; -char smartAudioStatusString[31]; - uint8_t smartAudioOpModel; uint8_t smartAudioStatus; uint8_t smartAudioBand; @@ -33,3 +32,4 @@ long smartAudioConfigurePowerByGvar(displayPort_t *, const void *self); long smartAudioSetTxModeByGvar(displayPort_t *, const void *self); #endif +#endif From 96235c2be06bdf58396ded74edb458d37e3c43f5 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 22 Nov 2016 13:58:10 +0900 Subject: [PATCH 18/40] wip --- src/main/io/vtx_smartaudio.c | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index d87dc39ba9..c3d276bf1b 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -344,7 +344,7 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_MODE: // Set Mode - dprintf(("resp SET_MODE 0x%x\r\n", buf[2])); + dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2])); break; default: @@ -600,7 +600,7 @@ static void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; - buf[4] = (mode & 0x1f)|saLockMode; + buf[4] = (mode & 0x3f)|saLockMode; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); @@ -668,7 +668,6 @@ void smartAudioProcess(uint32_t now) saGetSettings(); saGetPitFreq(); saSendQueue(); - saSetPitFreq(5705); initialSent = true; return; } @@ -677,6 +676,7 @@ void smartAudioProcess(uint32_t now) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); + // XXX Todo: Resend termination and possible offline transition saResendCmd(); } else if (!saQueueEmpty()) { // Command pending. Send it. @@ -774,6 +774,11 @@ if (smartAudioUserFreq == 0 && saDevice.freq != 0) if (smartAudioOpModel == 0 && saCmsOpmodel != 0) smartAudioOpModel = saCmsOpmodel + 1; +if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + smartAudioPitFMode = 1; +else + smartAudioPitFMode = 0; + saCmsStatusString[0] = "-FP"[saCmsOpmodel]; saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8]; saCmsStatusString[3] = '1' + (saDevice.chan % 8); @@ -900,13 +905,16 @@ long saConfigureOpModelByGvar(displayPort_t *pDisp, const void *self) dprintf(("saConfigureOpModelByGvar: opmodel %d\r\n", opmodel)); + if (opmodel == SA_OPMODEL_FREE) { // VTX should power up transmitting. // Turn off In-Range and Out-Range bits saSetMode(0); } else if (opmodel == SA_OPMODEL_PIT) { // VTX should power up in pit mode. - // Setup In-Range or Out-Range bits + // Default PitFMode is in-range to prevent users without + // out-range receivers from getting blinded. + smartAudioPitFMode = 0; saConfigurePitFModeByGvar(pDisp, self); } @@ -1069,12 +1077,28 @@ CMS_Menu menu_smartAudioConfig = { .entries = menu_smartAudioConfigEntries }; +OSD_Entry saMenuCommenceEntries[] = { + { "- CONFIRM -", OME_Label, NULL, NULL, 0 }, + { "YES", OME_Funcall, saClearPitMode, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu saMenuCommence = { + .GUARD_text = "XVTXCOM", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saMenuCommenceEntries, +}; + OSD_Entry saMenuFreqModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, { "FREQ", OME_UINT16, NULL, &cmsEntUserFreq, 0 }, { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, - { "START", OME_Funcall, saClearPitMode, NULL, 0 }, + { "START", OME_Submenu, cmsMenuChange, &saMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -1088,7 +1112,7 @@ OSD_Entry saMenuChanModeEntries[] = { "CHAN", OME_TAB, saConfigureChanByGvar, &cmsEntChan, 0 }, { "FREQ", OME_UINT16, NULL, &cmsEntFreq, DYNAMIC }, { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, - { "START", OME_Funcall, saClearPitMode, NULL, 0 }, + { "START", OME_Submenu, cmsMenuChange, &saMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } From 46f6bffbba78f193dace1e0f86fdf469290ec93b Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 03:28:08 +0900 Subject: [PATCH 19/40] Some cleanup --- src/main/io/vtx_smartaudio.c | 392 ++++++++++++++++++++--------------- 1 file changed, 224 insertions(+), 168 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index c3d276bf1b..a7c8c58862 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -59,7 +59,7 @@ enum { SA_CMD_GET_SETTINGS_V2 = 0x09 // Response only } smartAudioCommand_e; -// This is not a good design +// This is not a good design; can't distinguish command from response this way. #define SACMD(cmd) (((cmd) << 1) | 1) // opmode flags, GET side @@ -115,6 +115,7 @@ static const uint16_t saFreqTable[5][8] = { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand }; +// XXX Should frequencies not usable in locked state be detected? typedef struct saPowerTable_s { int rfpower; @@ -153,6 +154,7 @@ static smartAudioDevice_t saDevicePrev = { .version = 0, }; +// XXX Possible compliance problem here. Need LOCK/UNLOCK menu? static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // Receive frame reassembly buffer @@ -187,7 +189,7 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len) } -static void smartAudioPrintSettings(void) +static void saPrintSettings(void) { #ifdef SMARTAUDIO_DPRINTF dprintf(("Current status: version: %d\r\n", saDevice.version)); @@ -279,7 +281,7 @@ static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission static int sa_oslen; // And associate length #ifdef CMS -void smartAudioCmsUpdate(void); +void saCmsUpdate(void); #endif static void saProcessResponse(uint8_t *buf, int len) @@ -297,7 +299,7 @@ static void saProcessResponse(uint8_t *buf, int len) switch(resp) { 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) break; @@ -307,10 +309,6 @@ static void saProcessResponse(uint8_t *buf, int len) saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; -#ifdef CMS - // Export current device status for CMS - smartAudioCmsUpdate(); -#endif #ifdef SMARTAUDIO_DEBUG_MONITOR debug[0] = saDevice.version * 100 + saDevice.mode; debug[1] = saDevice.chan; @@ -354,11 +352,12 @@ static void saProcessResponse(uint8_t *buf, int len) // Debug if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) - smartAudioPrintSettings(); - + saPrintSettings(); saDevicePrev = saDevice; #ifdef CMS + // Export current device status for CMS + saCmsUpdate(); saUpdateStatusString(); #endif } @@ -387,7 +386,7 @@ static void saReceiveFramer(uint8_t c) if (c == 0xAA) { state = S_WAITPRE2; } else { - state = S_WAITPRE1; // Don't need this + state = S_WAITPRE1; // Don't need this (no change) } break; @@ -434,7 +433,9 @@ static void saReceiveFramer(uint8_t c) saProcessResponse(sa_rbuf, len + 2); sa_pktrcvd++; } else if (sa_rbuf[0] & 1) { - // Command echo (may be) + // Command echo + // XXX There is an exceptional case (V2 response) + // XXX Should check crc in the command format? } else { saStat.crc++; } @@ -576,6 +577,7 @@ static void saSetFreq(uint16_t freq) saQueueCmd(buf, 7); } +#if 0 static void saSetPitFreq(uint16_t freq) { saSetFreq(freq | SA_FREQ_SETPIT); @@ -585,6 +587,7 @@ static void saGetPitFreq(void) { saSetFreq(SA_FREQ_GETPIT); } +#endif void saSetBandChan(uint8_t band, uint8_t chan) { @@ -666,7 +669,7 @@ void smartAudioProcess(uint32_t now) if (!initialSent) { saGetSettings(); - saGetPitFreq(); + saSetFreq(SA_FREQ_GETPIT); saSendQueue(); initialSent = true; return; @@ -692,8 +695,6 @@ void smartAudioProcess(uint32_t now) #ifdef CMS -// CMS menu variables - // Operational Model and RF modes (CMS) #define SA_OPMODEL_UNDEF 0 // Not known yet @@ -705,45 +706,48 @@ void smartAudioProcess(uint32_t now) #define SA_TXMODE_PIT_INRANGE 2 #define SA_TXMODE_ACTIVE 3 -uint8_t saCmsOpmodel = SA_OPMODEL_UNDEF; -uint8_t smartAudioBand = 0; -uint8_t smartAudioChan = 0; -uint8_t smartAudioPower = 0; -uint16_t smartAudioFreq = 0; +uint8_t saCmsOpmodel = SA_OPMODEL_UNDEF; +uint8_t saCmsBand = 0; +uint8_t saCmsChan = 0; +uint8_t saCmsPower = 0; +uint16_t saCmsDeviceFreq = 0; -uint8_t smartAudioOpModel = 0; -uint8_t smartAudioStatus = 0; -uint8_t smartAudioPower; -uint8_t smartAudioTxMode; // RF state; ACTIVE, PIR, POR -uint8_t smartAudioPitFMode; // In-Range or Out-Range -uint16_t smartAudioORFreq = 0; // POR frequency -uint8_t smartAudioFreqMode; // Channel or User defined -uint16_t smartAudioUserFreq = 0; // User defined frequency +uint8_t saCmsDeviceStatus = 0; +uint8_t saCmsPower; +uint8_t saCmsPitFMode; // In-Range or Out-Range +uint8_t saCmsFreqMode; // Channel or User defined +uint16_t saCmsORFreq = 0; // POR frequency +uint16_t saCmsORFreqNew; // POR frequency -void smartAudioCmsUpdate(void) +uint16_t saCmsUserFreq = 0; // User defined frequency +uint16_t saCmsUserFreqNew; // User defined frequency + +uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently used + +void saCmsUpdate(void) { if (saCmsOpmodel == SA_OPMODEL_UNDEF) { // This is a first valid response to GET_SETTINGS. saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SA_OPMODEL_PIT : SA_OPMODEL_FREE; } - smartAudioBand = (saDevice.chan / 8) + 1; - smartAudioChan = (saDevice.chan % 8) + 1; - smartAudioFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsBand = (saDevice.chan / 8) + 1; + saCmsChan = (saDevice.chan % 8) + 1; + saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { - smartAudioTxMode = SA_TXMODE_ACTIVE; + saCmsRFState = SA_TXMODE_ACTIVE; } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - smartAudioTxMode = SA_TXMODE_PIT_INRANGE; + saCmsRFState = SA_TXMODE_PIT_INRANGE; } else { - smartAudioTxMode = SA_TXMODE_PIT_OUTRANGE; + saCmsRFState = SA_TXMODE_PIT_OUTRANGE; } if (saDevice.version == 2) { - smartAudioPower = saDevice.power + 1; // XXX Take care V1 + saCmsPower = saDevice.power + 1; // XXX Take care V1 } else { - smartAudioPower = saDacToPowerIndex(saDevice.power) + 1; + saCmsPower = saDacToPowerIndex(saDevice.power) + 1; } saUpdateStatusString(); @@ -753,11 +757,11 @@ char saCmsStatusString[31] = "- -- ---- ---"; // m bc ffff ppp // 0123456789012 -long saConfigureOpModelByGvar(displayPort_t *, const void *self); -long saConfigurePitFModeByGvar(displayPort_t *, const void *self); -long saConfigureBandByGvar(displayPort_t *, const void *self); -long saConfigureChanByGvar(displayPort_t *, const void *self); -long saConfigurePowerByGvar(displayPort_t *, const void *self); +static long saCmsConfigOpModelByGvar(displayPort_t *, const void *self); +static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self); +static long saCmsConfigBandByGvar(displayPort_t *, const void *self); +static long saCmsConfigChanByGvar(displayPort_t *, const void *self); +static long saCmsConfigPowerByGvar(displayPort_t *, const void *self); static void saUpdateStatusString(void) { @@ -765,19 +769,17 @@ static void saUpdateStatusString(void) return; // XXX These should be done somewhere else -if (smartAudioStatus == 0 && saDevice.version != 0) - smartAudioStatus = saDevice.version; -if (smartAudioORFreq == 0 && saDevice.orfreq != 0) - smartAudioORFreq = saDevice.orfreq; -if (smartAudioUserFreq == 0 && saDevice.freq != 0) - smartAudioUserFreq = saDevice.freq; -if (smartAudioOpModel == 0 && saCmsOpmodel != 0) - smartAudioOpModel = saCmsOpmodel + 1; +if (saCmsDeviceStatus == 0 && saDevice.version != 0) + saCmsDeviceStatus = saDevice.version; +if (saCmsORFreq == 0 && saDevice.orfreq != 0) + saCmsORFreq = saDevice.orfreq; +if (saCmsUserFreq == 0 && saDevice.freq != 0) + saCmsUserFreq = saDevice.freq; if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) - smartAudioPitFMode = 1; + saCmsPitFMode = 1; else - smartAudioPitFMode = 0; + saCmsPitFMode = 0; saCmsStatusString[0] = "-FP"[saCmsOpmodel]; saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8]; @@ -808,86 +810,80 @@ else } } -static long sacms_SetupTopMenu(void); - -long saConfigureBandByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); if (saDevice.version == 0) { // Bounce back; not online yet - smartAudioBand = 0; + saCmsBand = 0; return 0; } - if (smartAudioBand == 0) { + if (saCmsBand == 0) { // Bouce back, no going back to undef state - smartAudioBand = 1; + saCmsBand = 1; return 0; } - saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; } -long saConfigureChanByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); -dprintf(("saConfigureBandByGvar: called\r\n")); - if (saDevice.version == 0) { // Bounce back; not online yet - smartAudioChan = 0; + saCmsChan = 0; return 0; } - if (smartAudioChan == 0) { + if (saCmsChan == 0) { // Bounce back; no going back to undef state - smartAudioChan = 1; + saCmsChan = 1; return 0; } -dprintf(("saConfigureBandByGvar: calling saSetBandChan\r\n")); - - saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; } -long saConfigurePowerByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); if (saDevice.version == 0) { // Bounce back; not online yet - smartAudioPower = 0; + saCmsPower = 0; return 0; } - if (smartAudioPower == 0) { + if (saCmsPower == 0) { // Bouce back; no going back to undef state - smartAudioPower = 1; + saCmsPower = 1; return 0; } - saSetPowerByIndex(smartAudioPower - 1); + saSetPowerByIndex(saCmsPower - 1); return 0; } -long saConfigurePitFModeByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - dprintf(("saConfigurePitFmodeByGbar: smartAudioPitFMode %d\r\n", smartAudioPitFMode)); + dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode)); - if (smartAudioPitFMode == 0) { + if (saCmsPitFMode == 0) { saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); } else { saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); @@ -896,14 +892,14 @@ long saConfigurePitFModeByGvar(displayPort_t *pDisp, const void *self) return 0; } -long saConfigureOpModelByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigOpModelByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - uint8_t opmodel = smartAudioOpModel; + uint8_t opmodel = saCmsOpmodel; - dprintf(("saConfigureOpModelByGvar: opmodel %d\r\n", opmodel)); + dprintf(("saCmsConfigOpModelByGvar: opmodel %d\r\n", opmodel)); if (opmodel == SA_OPMODEL_FREE) { @@ -914,39 +910,39 @@ long saConfigureOpModelByGvar(displayPort_t *pDisp, const void *self) // VTX should power up in pit mode. // Default PitFMode is in-range to prevent users without // out-range receivers from getting blinded. - smartAudioPitFMode = 0; - saConfigurePitFModeByGvar(pDisp, self); + saCmsPitFMode = 0; + saCmsConfigPitFModeByGvar(pDisp, self); } return 0; } -static const char * const smartAudioStatusNames[] = { +static const char * const saCmsDeviceStatusNames[] = { "OFFL", "ONL V1", "ONL V2", }; -OSD_TAB_t cmsEntOnline = { &smartAudioStatus, 2, smartAudioStatusNames }; -OSD_UINT16_t cmsEntBaudrate = { &sa_smartbaud, 0, 0, 0 }; -OSD_UINT16_t cmsEntStatBadpre = { &saStat.badpre, 0, 0, 0 }; -OSD_UINT16_t cmsEntStatBadlen = { &saStat.badlen, 0, 0, 0 }; -OSD_UINT16_t cmsEntStatCrcerr = { &saStat.crc, 0, 0, 0 }; -OSD_UINT16_t cmsEntStatOooerr = { &saStat.ooopresp, 0, 0, 0 }; +static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; +static OSD_UINT16_t saCmsEntBaudrate = { &sa_smartbaud, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatBadpre = { &saStat.badpre, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatBadlen = { &saStat.badlen, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatCrcerr = { &saStat.crc, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatOooerr = { &saStat.ooopresp, 0, 0, 0 }; -OSD_Entry menu_smartAudioStatsEntries[] = { +static OSD_Entry menu_smartAudioStatsEntries[] = { { "- SA STATS -", OME_Label, NULL, NULL, 0 }, - { "STATUS", OME_TAB, NULL, &cmsEntOnline, DYNAMIC }, - { "BAUDRATE", OME_UINT16, NULL, &cmsEntBaudrate, DYNAMIC }, - { "BADPRE", OME_UINT16, NULL, &cmsEntStatBadpre, DYNAMIC }, - { "BADLEN", OME_UINT16, NULL, &cmsEntStatBadlen, DYNAMIC }, - { "CRCERR", OME_UINT16, NULL, &cmsEntStatCrcerr, DYNAMIC }, - { "OOOERR", OME_UINT16, NULL, &cmsEntStatOooerr, DYNAMIC }, + { "STATUS", OME_TAB, NULL, &saCmsEntOnline, DYNAMIC }, + { "BAUDRATE", OME_UINT16, NULL, &saCmsEntBaudrate, DYNAMIC }, + { "BADPRE", OME_UINT16, NULL, &saCmsEntStatBadpre, DYNAMIC }, + { "BADLEN", OME_UINT16, NULL, &saCmsEntStatBadlen, DYNAMIC }, + { "CRCERR", OME_UINT16, NULL, &saCmsEntStatCrcerr, DYNAMIC }, + { "OOOERR", OME_UINT16, NULL, &saCmsEntStatOooerr, DYNAMIC }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu menu_smartAudioStats = { +static CMS_Menu menu_smartAudioStats = { .GUARD_text = "XSAST", .GUARD_type = OME_MENU, .onEnter = NULL, @@ -955,7 +951,7 @@ CMS_Menu menu_smartAudioStats = { .entries = menu_smartAudioStatsEntries }; -static const char * const smartAudioBandNames[] = { +static const char * const saCmsBandNames[] = { "--------", "BOSCAM A", "BOSCAM B", @@ -964,15 +960,15 @@ static const char * const smartAudioBandNames[] = { "RACEBAND", }; -OSD_TAB_t cmsEntBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; +static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, &saCmsBandNames[0], NULL }; -static const char * const smartAudioChanNames[] = { +static const char * const saCmsChanNames[] = { "-", "1", "2", "3", "4", "5", "6", "7", "8", }; -OSD_TAB_t cmsEntChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; +static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &saCmsChanNames[0], NULL }; -static const char * const smartAudioPowerNames[] = { +static const char * const saCmsPowerNames[] = { "---", "25 ", "200", @@ -980,63 +976,45 @@ static const char * const smartAudioPowerNames[] = { "800", }; -OSD_TAB_t cmsEntPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; +static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, &saCmsPowerNames[0]}; -static const char * const smartAudioTxModeNames[] = { - "------", - "PIT-OR", - "PIT-IR", - "ACTIVE", -}; +static OSD_UINT16_t saCmsEntFreq = { &saCmsDeviceFreq, 5600, 5900, 0 }; -OSD_TAB_t cmsEntTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; - -OSD_UINT16_t cmsEntFreq = { &smartAudioFreq, 5600, 5900, 0 }; - -static const char * const smartAudioOpModelNames[] = { +static const char * const saCmsOpmodelNames[] = { "----", "FREE", "PIT ", }; -OSD_TAB_t cmsEntOpModel = { &smartAudioOpModel, 2, &smartAudioOpModelNames[0] }; +static OSD_TAB_t saCmsEntOpModel = { &saCmsOpmodel, 2, &saCmsOpmodelNames[0] }; -static const char * const smartAudioPitFModeNames[] = { - "IN-R ", - "OUT-R" -}; - -OSD_TAB_t cmsEntPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; - -OSD_UINT16_t cmsEntORFreq = { &smartAudioORFreq, 5000, 5900, 1 }; - -static const char * const smartAudioFreqModeNames[] = { +static const char * const saCmsFreqModeNames[] = { "CHAN", "USER" }; -OSD_TAB_t cmsEntFreqMode = { &smartAudioFreqMode, 1, smartAudioFreqModeNames }; +static OSD_TAB_t saCmsEntFreqMode = { &saCmsFreqMode, 1, saCmsFreqModeNames }; -OSD_UINT16_t cmsEntUserFreq = { &smartAudioUserFreq, 5000, 5900, 1 }; +static const char * const saCmsPitFModeNames[] = { + "IN-R ", + "OUT-R" +}; -long saConfigureUserFreqByGvar(displayPort_t *pDisp, const void *self) +static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, &saCmsPitFModeNames[0] }; + +static long sacms_SetupTopMenu(void); // Forward + +static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - saSetFreq(smartAudioFreq); - - return 0; -} - -long saConfigureFreqModeByGvar(displayPort_t *pDisp, const void *self) -{ - if (smartAudioFreqMode == 0) { + if (saCmsFreqMode == 0) { // CHAN - saSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + saSetBandChan(saCmsBand - 1, saCmsChan - 1); } else { // USER - saConfigureUserFreqByGvar(pDisp, self); + saSetFreq(saCmsUserFreq); } sacms_SetupTopMenu(); @@ -1044,12 +1022,12 @@ long saConfigureFreqModeByGvar(displayPort_t *pDisp, const void *self) return 0; } -long saClearPitMode(displayPort_t *pDisp, const void *self) +static long saCmsClearPitMode(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (smartAudioPitFMode == 0) + if (saCmsPitFMode == 0) saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); else saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); @@ -1057,18 +1035,96 @@ long saClearPitMode(displayPort_t *pDisp, const void *self) return 0; } -OSD_Entry menu_smartAudioConfigEntries[] = { +static OSD_UINT16_t saCmsEntORFreq = { &saCmsORFreq, 5000, 5900, 0 }; +static OSD_UINT16_t saCmsEntORFreqNew = { &saCmsORFreqNew, 5000, 5900, 1 }; + +static long saCmsSetPORFreqOnEnter(void) +{ + saCmsORFreqNew = saCmsORFreq; + + return 0; +} + +static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT); + + return 0; +} + +static OSD_UINT16_t saCmsEntUserFreq = { &saCmsUserFreq, 5000, 5900, 0 }; +static OSD_UINT16_t saCmsEntUserFreqNew = { &saCmsUserFreqNew, 5000, 5900, 1 }; + +static long saCmsSetUserFreqOnEnter(void) +{ + saCmsUserFreqNew = saCmsUserFreq; + + return 0; +} + +static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + saSetFreq(saCmsUserFreqNew); + + return 0; +} + +static OSD_Entry saCmsMenuPORFreqEntries[] = { + { "- POR FREQ -", OME_Label, NULL, NULL, 0 }, + { "CUR FREQ", OME_UINT16, NULL, &saCmsEntORFreq, DYNAMIC }, + { "NEW FREQ", OME_UINT16, NULL, &saCmsEntORFreqNew, 0 }, + { "SET", OME_Funcall, saCmsSetPORFreq, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu saCmsMenuPORFreq = +{ + .GUARD_text = "XSAPOR", + .GUARD_type = OME_MENU, + .onEnter = saCmsSetPORFreqOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuPORFreqEntries, +}; + +static OSD_Entry saCmsMenuUserFreqEntries[] = { + { "- USER FREQ -", OME_Label, NULL, NULL, 0 }, + { "CUR FREQ", OME_UINT16, NULL, &saCmsEntUserFreq, DYNAMIC }, + { "NEW FREQ", OME_UINT16, NULL, &saCmsEntUserFreqNew, 0 }, + { "SET", OME_Funcall, saCmsSetUserFreq, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu saCmsMenuUserFreq = +{ + .GUARD_text = "XSAUFQ", + .GUARD_type = OME_MENU, + .onEnter = saCmsSetUserFreqOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuUserFreqEntries, +}; + +static OSD_Entry menu_smartAudioConfigEntries[] = { { "- SA CONFIG -", OME_Label, NULL, NULL, 0 }, - { "OP MODEL", OME_TAB, saConfigureOpModelByGvar, &cmsEntOpModel, 0 }, - { "PIT FMODE", OME_TAB, saConfigurePitFModeByGvar, &cmsEntPitFMode, 0 }, - { "POR FREQ", OME_UINT16, NULL, &cmsEntORFreq, 0 }, - { "FREQ MODE", OME_TAB, saConfigureFreqModeByGvar, &cmsEntFreqMode, 0 }, + { "OP MODEL", OME_TAB, saCmsConfigOpModelByGvar, &saCmsEntOpModel, 0 }, + { "FREQ MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFreqMode, 0 }, + { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 }, + { "POR FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuPORFreq, 0 }, { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu menu_smartAudioConfig = { +static CMS_Menu menu_smartAudioConfig = { .GUARD_text = "XSACFG", .GUARD_type = OME_MENU, .onEnter = NULL, @@ -1077,48 +1133,48 @@ CMS_Menu menu_smartAudioConfig = { .entries = menu_smartAudioConfigEntries }; -OSD_Entry saMenuCommenceEntries[] = { - { "- CONFIRM -", OME_Label, NULL, NULL, 0 }, - { "YES", OME_Funcall, saClearPitMode, NULL, 0 }, +static OSD_Entry saCmsMenuCommenceEntries[] = { + { "CONFIRM", OME_Label, NULL, NULL, 0 }, + { "YES", OME_Funcall, saCmsClearPitMode, NULL, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu saMenuCommence = { +static CMS_Menu saCmsMenuCommence = { .GUARD_text = "XVTXCOM", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = saMenuCommenceEntries, + .entries = saCmsMenuCommenceEntries, }; -OSD_Entry saMenuFreqModeEntries[] = { +static OSD_Entry saCmsMenuFreqModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "FREQ", OME_UINT16, NULL, &cmsEntUserFreq, 0 }, - { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, - { "START", OME_Submenu, cmsMenuChange, &saMenuCommence, 0 }, + { "FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuUserFreq, 0 }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "START", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -OSD_Entry saMenuChanModeEntries[] = +static OSD_Entry saCmsMenuChanModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "BAND", OME_TAB, saConfigureBandByGvar, &cmsEntBand, 0 }, - { "CHAN", OME_TAB, saConfigureChanByGvar, &cmsEntChan, 0 }, - { "FREQ", OME_UINT16, NULL, &cmsEntFreq, DYNAMIC }, - { "POWER", OME_TAB, saConfigurePowerByGvar, &cmsEntPower, 0 }, - { "START", OME_Submenu, cmsMenuChange, &saMenuCommence, 0 }, + { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, + { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, + { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreq, DYNAMIC }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "START", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -OSD_Entry saMenuOfflineEntries[] = +static OSD_Entry saCmsMenuOfflineEntries[] = { { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, @@ -1127,17 +1183,17 @@ OSD_Entry saMenuOfflineEntries[] = { NULL, OME_END, NULL, NULL, 0 } }; -CMS_Menu cmsx_menuVtxSmartAudio; +CMS_Menu cmsx_menuVtxSmartAudio; // Forward static long sacms_SetupTopMenu(void) { - if (smartAudioStatus) { - if (smartAudioFreqMode == 0) - cmsx_menuVtxSmartAudio.entries = saMenuChanModeEntries; + if (saCmsDeviceStatus) { + if (saCmsFreqMode == 0) + cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries; else - cmsx_menuVtxSmartAudio.entries = saMenuFreqModeEntries; + cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries; } else { - cmsx_menuVtxSmartAudio.entries = saMenuOfflineEntries; + cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries; } return 0; @@ -1149,7 +1205,7 @@ CMS_Menu cmsx_menuVtxSmartAudio = { .onEnter = sacms_SetupTopMenu, .onExit = NULL, .onGlobalExit = NULL, - .entries = saMenuOfflineEntries, + .entries = saCmsMenuOfflineEntries, }; #endif // CMS From d074cafd3960362a3e89a6a943f16f7c954b1555 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 04:05:02 +0900 Subject: [PATCH 20/40] Fix OWNER_SERIAL in serial_uart_stm32f10x.c --- src/main/drivers/serial_uart_stm32f10x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index c4ba3751be..df273ee054 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -122,7 +122,7 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option // UART1_TX PA9 // UART1_RX PA10 if (options & SERIAL_BIDIR) { - IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL, RESOURCE_UART_TXRX, 1); + IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL_TX, 1); IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { @@ -193,7 +193,7 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option // UART2_TX PA2 // UART2_RX PA3 if (options & SERIAL_BIDIR) { - IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL, RESOURCE_UART_TXRX, 2); + IOInit(IOGetByTag(IO_TAG(PA2)), OWNER_SERIAL_TX, 2); IOConfigGPIO(IOGetByTag(IO_TAG(PA2)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { From 3fe8c4b53bed21229ff2eed0c080893874c05b9f Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 04:15:18 +0900 Subject: [PATCH 21/40] Ditto for F4 --- src/main/drivers/serial_uart_stm32f4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index cad49c7a66..0e83575b02 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -339,7 +339,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po RCC_AHB1PeriphClockCmd(uart->rcc_ahb1, ENABLE); if (options & SERIAL_BIDIR) { - IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device)); + IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); if (options & SERIAL_BIDIR_PP) IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); else From af6010eb63d77e4a0aa54ad3ec5fa5d434964885 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 04:22:05 +0900 Subject: [PATCH 22/40] More OWNER_SERIAL_TXRX !? --- src/main/drivers/serial_uart_stm32f10x.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index df273ee054..493146644e 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -255,7 +255,7 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option RCC_ClockCmd(RCC_APB1(USART3), ENABLE); if (options & SERIAL_BIDIR) { - IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL, RESOURCE_UART_TXRX, 3); + IOInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), OWNER_SERIAL_TX, 3); IOConfigGPIO(IOGetByTag(IO_TAG(UART3_TX_PIN)), (options & SERIAL_BIDIR_PP) ? IOCFG_AF_PP : IOCFG_AF_OD); } else { if (mode & MODE_TX) { From 2c4e399ca16abcf1e9506f5fdf3b3f601851e3b0 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 10:52:01 +0900 Subject: [PATCH 23/40] Some adjustment for debugging on F4 (OMNIBUSF4) --- src/main/io/vtx_smartaudio.c | 37 ++++++++++++++++++----------- src/main/target/OMNIBUSF4/target.h | 5 ++++ src/main/target/OMNIBUSF4/target.mk | 3 ++- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index a7c8c58862..81a60daf63 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -33,7 +33,13 @@ //#define SMARTAUDIO_DEBUG_MONITOR #ifdef SMARTAUDIO_DPRINTF + +#ifdef OMNIBUSF4 +#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 +#else #define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 +#endif + serialPort_t *debugSerialPort = NULL; #define dprintf(x) if (debugSerialPort) printf x #else @@ -90,6 +96,8 @@ enum { // Statistical counters, for user side trouble shooting. typedef struct smartAudioStat_s { + uint16_t pktsent; + uint16_t pktrcvd; uint16_t badpre; uint16_t badlen; uint16_t crc; @@ -98,6 +106,8 @@ typedef struct smartAudioStat_s { } smartAudioStat_t; static smartAudioStat_t saStat = { + .pktsent = 0, + .pktrcvd = 0, .badpre = 0, .badlen = 0, .crc = 0, @@ -230,26 +240,21 @@ static int sa_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) + if (saStat.pktsent < 10) // Not enough samples collected return; #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", - sa_smartbaud, sa_pktrcvd, sa_pktsent, ((sa_pktrcvd * 100) / sa_pktsent))); + sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent))); #endif - if (((sa_pktrcvd * 100) / sa_pktsent) >= 70) { + if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) { // This is okay - sa_pktsent = 0; // Should be more moderate? - sa_pktrcvd = 0; + saStat.pktsent = 0; // Should be more moderate? + saStat.pktrcvd = 0; return; } @@ -269,8 +274,8 @@ static void saAutobaud(void) smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud); - sa_pktsent = 0; - sa_pktrcvd = 0; + saStat.pktsent = 0; + saStat.pktrcvd = 0; } // Transport level variables @@ -431,7 +436,7 @@ static void saReceiveFramer(uint8_t c) if (CRC8(sa_rbuf, 2 + len) == c) { // Got a response saProcessResponse(sa_rbuf, len + 2); - sa_pktrcvd++; + saStat.pktrcvd++; } else if (sa_rbuf[0] & 1) { // Command echo // XXX There is an exceptional case (V2 response) @@ -456,7 +461,7 @@ static void saSendFrame(uint8_t *buf, int len) serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this sa_lastTransmission = millis(); - sa_pktsent++; + saStat.pktsent++; } /* @@ -925,6 +930,8 @@ static const char * const saCmsDeviceStatusNames[] = { static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; static OSD_UINT16_t saCmsEntBaudrate = { &sa_smartbaud, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatPktSent = { &saStat.pktsent, 0, 0, 0 }; +static OSD_UINT16_t saCmsEntStatPktRcvd = { &saStat.pktrcvd, 0, 0, 0 }; static OSD_UINT16_t saCmsEntStatBadpre = { &saStat.badpre, 0, 0, 0 }; static OSD_UINT16_t saCmsEntStatBadlen = { &saStat.badlen, 0, 0, 0 }; static OSD_UINT16_t saCmsEntStatCrcerr = { &saStat.crc, 0, 0, 0 }; @@ -934,6 +941,8 @@ static OSD_Entry menu_smartAudioStatsEntries[] = { { "- SA STATS -", OME_Label, NULL, NULL, 0 }, { "STATUS", OME_TAB, NULL, &saCmsEntOnline, DYNAMIC }, { "BAUDRATE", OME_UINT16, NULL, &saCmsEntBaudrate, DYNAMIC }, + { "SENT", OME_UINT16, NULL, &saCmsEntStatPktSent, DYNAMIC }, + { "RCVD", OME_UINT16, NULL, &saCmsEntStatPktRcvd, DYNAMIC }, { "BADPRE", OME_UINT16, NULL, &saCmsEntStatBadpre, DYNAMIC }, { "BADLEN", OME_UINT16, NULL, &saCmsEntStatBadlen, DYNAMIC }, { "CRCERR", OME_UINT16, NULL, &saCmsEntStatCrcerr, DYNAMIC }, diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index c3f3529647..2ec1fc1c56 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -101,6 +101,11 @@ #define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +// VTX monitor task +#define VTX_CONTROL +// VTX device type +#define VTX_SMARTAUDIO + #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 333e4cdcb9..af8a1adbfc 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -5,5 +5,6 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c + drivers/max7456.c \ + io/vtx_smartaudio.c From 457afbcaf02ffee46347b89fffd18fa1283a9507 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 23 Nov 2016 21:14:29 +0900 Subject: [PATCH 24/40] MAX7456: Detect video standard dynamically --- src/main/drivers/max7456.c | 112 ++++++++++++++++++++++++++++++------- 1 file changed, 93 insertions(+), 19 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 3e4d541d87..d1d1a71884 100755 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -37,9 +37,8 @@ #include "max7456.h" #include "max7456_symbols.h" - - //MAX7456 opcodes +//XXX These are not opcodes but reg addrs. Substitute with MAX7456ADD_xxx. #define DMM_REG 0x04 #define DMAH_REG 0x05 #define DMAL_REG 0x06 @@ -52,15 +51,21 @@ #define MAX7456_RESET 0x02 #define VERTICAL_SYNC_NEXT_VSYNC 0x04 #define OSD_ENABLE 0x08 + #define SYNC_MODE_AUTO 0x00 #define SYNC_MODE_INTERNAL 0x30 #define SYNC_MODE_EXTERNAL 0x20 + #define VIDEO_MODE_PAL 0x40 #define VIDEO_MODE_NTSC 0x00 +#define VIDEO_MODE_MASK 0x40 +#define VIDEO_MODE_IS_PAL(val) (((val) & VIDEO_MODE_MASK) == VIDEO_MODE_PAL) +#define VIDEO_MODE_IS_NTSC(val) (((val) & VIDEO_MODE_MASK) == VIDEO_MODE_NTSC) + +#define VIDEO_SIGNAL_DEBOUNCE_MS 100 // Time to wait for input to stabilize // video mode register 1 bits - // duty cycle is on_off #define BLINK_DUTY_CYCLE_50_50 0x00 #define BLINK_DUTY_CYCLE_33_66 0x01 @@ -85,9 +90,33 @@ #define BACKGROUND_MODE_GRAY 0x40 -//MAX7456 commands +// STAT register bits + +#define STAT_PAL 0x01 +#define STAT_NTSC 0x02 +#define STAT_LOS 0x04 +#define STAT_NVR_BUSY 0x20 + +#define STAT_IS_PAL(val) ((val) & STAT_PAL) +#define STAT_IS_NTSC(val) ((val) & STAT_NTSC) +#define STAT_IS_LOS(val) ((val) & STAT_LOS) + +#define VIN_IS_PAL(val) (!STAT_IS_LOS(val) && STAT_IS_PAL(val)) +#define VIN_IS_NTSC(val) (!STAT_IS_LOS(val) && STAT_IS_NTSC(val)) + +// Kluege warning! +// There are occasions that NTSC is not detected even with !LOS (AB7456 specific?) +// When this happens, lower 3 bits of STAT register is read as zero. +// To cope with this case, this macro defines !LOS && !PAL as NTSC. +// Should be compatible with MAX7456 and non-problematic case. + +#define VIN_IS_NTSC_alt(val) (!STAT_IS_LOS(val) && !STAT_IS_PAL(val)) + +// DMM special bits #define CLEAR_DISPLAY 0x04 #define CLEAR_DISPLAY_VERT 0x06 + +// Special address for terminating incremental write #define END_STRING 0xff @@ -125,7 +154,6 @@ #define NVM_RAM_SIZE 54 #define WRITE_NVR 0xA0 -#define STATUS_REG_NVR_BUSY 0x20 /** Line multiples, for convenience & one less op at runtime **/ #define LINE 30 @@ -147,12 +175,6 @@ #define LINE16 450 - - - - - - //on shared SPI buss we want to change clock for OSD chip and restore for other devices #ifdef MAX7456_SPI_CLK #define ENABLE_MAX7456 {spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK);IOLo(max7456CsPin);} @@ -315,8 +337,12 @@ uint8_t max7456GetRowsCount(void) return (videoSignalReg & VIDEO_MODE_PAL) ? VIDEO_LINES_PAL : VIDEO_LINES_NTSC; } +#if 0 +// XXX Remove this comment, too. //because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup //and in case of restart we need to reinitialize chip +#endif + void max7456ReInit(void) { uint8_t maxScreenRows; @@ -324,9 +350,13 @@ void max7456ReInit(void) uint16_t x; static bool firstInit = true; +#if 0 +// XXX We don't have to wait for the cam any more. +// XXX Remove this when everything is stable. //do not init MAX before camera power up correctly if (millis() < 1500) return; +#endif ENABLE_MAX7456; @@ -334,15 +364,22 @@ void max7456ReInit(void) case VIDEO_SYSTEM_PAL: videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; break; + case VIDEO_SYSTEM_NTSC: videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; break; + case VIDEO_SYSTEM_AUTO: srdata = max7456Send(MAX7456ADD_STAT, 0x00); - if ((0x02 & srdata) == 0x02) + + if (VIN_IS_NTSC(srdata)) { videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; - else + } else if (VIN_IS_PAL(srdata)) { videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; + } else { + // No valid input signal, fallback to default (XXX NTSC for now) + videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; + } break; } @@ -436,22 +473,60 @@ bool max7456DmaInProgres(void) } #endif +#include "build/debug.h" + void max7456DrawScreen(void) { - uint8_t check; + uint8_t stallCheck; + uint8_t videoSense; + static uint32_t videoDetectTimeMs = 0; static uint16_t pos = 0; int k = 0, buff_len=0; if (!max7456Lock && !fontIsLoading) { - //-----------------detect MAX7456 fail, or initialize it at startup when it is ready-------- + + // Detect MAX7456 fail, or initialize it at startup when it is ready + max7456Lock = true; ENABLE_MAX7456; - check = max7456Send(VM0_REG | 0x80, 0x00); + stallCheck = max7456Send(VM0_REG | 0x80, 0x00); DISABLE_MAX7456; - if ( check != videoSignalReg) + if (stallCheck != videoSignalReg) { max7456ReInit(); + } else if (videoSignalCfg == VIDEO_SYSTEM_AUTO) { + + // Adjust output format based on the current input format. + + ENABLE_MAX7456; + videoSense = max7456Send(MAX7456ADD_STAT, 0x00); + DISABLE_MAX7456; + + debug[0] = videoSignalReg & VIDEO_MODE_MASK; + debug[1] = videoSense & 0x7; + debug[3] = max7456GetRowsCount(); + + if (videoSense & STAT_LOS) { + videoDetectTimeMs = 0; + } else { + // There is a case that NTSC is not detected for some reason (AB7456 specific?) + // Here we force NTSC detection if not LOS and not PAL. + if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg)) + || (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) { + if (videoDetectTimeMs) { + if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) { + max7456ReInit(); + debug[2]++; + } + } else { + // Wait for signal to stabilize + videoDetectTimeMs = millis(); + } + } + } + } + //------------ end of (re)init------------------------------------- for (k=0; k< MAX_CHARS2UPDATE; k++) { @@ -545,12 +620,11 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data) max7456Send(MAX7456ADD_CMM, WRITE_NVR); // wait until bit 5 in the status register returns to 0 (12ms) - while ((max7456Send(MAX7456ADD_STAT, 0x00) & STATUS_REG_NVR_BUSY) != 0x00); + while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00); DISABLE_MAX7456; max7456Lock = false; } - #endif From ed09ee4a9bd4ec408303e3caeefbe9167b4c6e52 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 24 Nov 2016 16:33:21 +0900 Subject: [PATCH 25/40] Trial for displaying optional string on right column for submenus --- src/main/cms/cms.c | 12 ++++++++++++ src/main/cms/cms_types.h | 5 +++++ src/main/io/vtx_smartaudio.c | 11 ++++++++++- 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 388fd6b037..5cae422e91 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -234,6 +234,13 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) case OME_Funcall: if (IS_PRINTVALUE(p)) { cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); + + // Special case of sub menu entry with func used as a function + // returning a string to print on the right column. + if (p->type == OME_Submenu && p->func) { + if (p->flags & OPTSTRING) + cnt += displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) + 1, row, ((CMSMenuOptFuncPtr)p->func)()); + } CLR_PRINTVALUE(p); } break; @@ -645,6 +652,11 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) switch (p->type) { case OME_Submenu: + if (p->func && key == KEY_RIGHT) { + cmsMenuChange(pDisplay, p->data); + res = BUTTON_PAUSE; + } + break; case OME_Funcall: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index 427e1d38e6..5650ca97ac 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -66,6 +66,7 @@ typedef struct #define PRINT_VALUE 0x01 // Value has been changed, need to redraw #define PRINT_LABEL 0x02 // Text label should be printed #define DYNAMIC 0x04 // Value should be updated dynamically +#define OPTSTRING 0x08 // (Temporary) Flag for OME_Submenu, indicating func should be called to get a string to display. #define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE) #define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; } @@ -153,3 +154,7 @@ typedef struct { char *val; } OSD_String_t; + +// This is a function used in the func member if the type is OME_Submenu. + +typedef char * (*CMSMenuOptFuncPtr)(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 81a60daf63..565ea15bf6 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1064,6 +1064,15 @@ static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self) return 0; } +static char *saCmsORFreqGetString(void) +{ + static char pbuf[5]; + + tfp_sprintf(pbuf, "%4d", saCmsORFreq); + + return pbuf; +} + static OSD_UINT16_t saCmsEntUserFreq = { &saCmsUserFreq, 5000, 5900, 0 }; static OSD_UINT16_t saCmsEntUserFreqNew = { &saCmsUserFreqNew, 5000, 5900, 1 }; @@ -1127,7 +1136,7 @@ static OSD_Entry menu_smartAudioConfigEntries[] = { { "OP MODEL", OME_TAB, saCmsConfigOpModelByGvar, &saCmsEntOpModel, 0 }, { "FREQ MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFreqMode, 0 }, { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 }, - { "POR FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuPORFreq, 0 }, + { "POR FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq, OPTSTRING }, { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } From 5d5ded213128f6e3db745e04c6a0b6900f2cd2fd Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 25 Nov 2016 18:27:55 +0900 Subject: [PATCH 26/40] FREE mode band/chan change requires SET to be executed --- src/main/cms/cms.c | 45 ++++++++++++++++++----- src/main/cms/cms_menu_imu.c | 7 ++-- src/main/io/vtx_smartaudio.c | 70 ++++++++++++++++++++++-------------- 3 files changed, 84 insertions(+), 38 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 5cae422e91..14fb32fc7e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -230,20 +230,28 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_Submenu: case OME_Funcall: if (IS_PRINTVALUE(p)) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">"); - // Special case of sub menu entry with func used as a function - // returning a string to print on the right column. - if (p->type == OME_Submenu && p->func) { - if (p->flags & OPTSTRING) - cnt += displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) + 1, row, ((CMSMenuOptFuncPtr)p->func)()); + int colPos = RIGHT_MENU_COLUMN(pDisplay); + + if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) { + + // Special case of sub menu entry with optional value display. + + char *str = ((CMSMenuOptFuncPtr)p->func)(); + cnt = displayWrite(pDisplay, colPos, row, str); + colPos += strlen(str); } + + cnt += displayWrite(pDisplay, colPos, row, ">"); + CLR_PRINTVALUE(p); } break; + case OME_Bool: if (IS_PRINTVALUE(p) && p->data) { if (*((uint8_t *)(p->data))) { @@ -254,7 +262,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; - case OME_TAB: { + + case OME_TAB: if (IS_PRINTVALUE(p)) { OSD_TAB_t *ptr = p->data; //cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]); @@ -262,7 +271,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; - } + #ifdef OSD case OME_VISIBLE: if (IS_PRINTVALUE(p) && p->data) { @@ -280,6 +289,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) } break; #endif + case OME_UINT8: if (IS_PRINTVALUE(p) && p->data) { OSD_UINT8_t *ptr = p->data; @@ -289,6 +299,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_INT8: if (IS_PRINTVALUE(p) && p->data) { OSD_INT8_t *ptr = p->data; @@ -298,6 +309,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_UINT16: if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; @@ -307,6 +319,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_INT16: if (IS_PRINTVALUE(p) && p->data) { OSD_UINT16_t *ptr = p->data; @@ -316,6 +329,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_FLOAT: if (IS_PRINTVALUE(p) && p->data) { OSD_FLOAT_t *ptr = p->data; @@ -325,6 +339,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_Label: if (IS_PRINTVALUE(p) && p->data) { // A label with optional string, immediately following text @@ -332,10 +347,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row) CLR_PRINTVALUE(p); } break; + case OME_OSD_Exit: case OME_END: case OME_Back: break; + case OME_MENU: // Fall through default: @@ -657,6 +674,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) res = BUTTON_PAUSE; } break; + case OME_Funcall: case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { @@ -664,10 +682,12 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) res = BUTTON_PAUSE; } break; + case OME_Back: cmsMenuBack(pDisplay); res = BUTTON_PAUSE; break; + case OME_Bool: if (p->data) { uint8_t *val = p->data; @@ -678,6 +698,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; + #ifdef OSD case OME_VISIBLE: if (p->data) { @@ -694,6 +715,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } break; #endif + case OME_UINT8: case OME_FLOAT: if (p->data) { @@ -712,6 +734,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } break; + case OME_TAB: if (p->type == OME_TAB) { OSD_TAB_t *ptr = p->data; @@ -729,6 +752,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) SET_PRINTVALUE(p); } break; + case OME_INT8: if (p->data) { OSD_INT8_t *ptr = p->data; @@ -746,6 +770,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } break; + case OME_UINT16: if (p->data) { OSD_UINT16_t *ptr = p->data; @@ -763,6 +788,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } break; + case OME_INT16: if (p->data) { OSD_INT16_t *ptr = p->data; @@ -780,11 +806,14 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } break; + case OME_String: break; + case OME_Label: case OME_END: break; + case OME_MENU: // Shouldn't happen break; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 24b988cb07..647be77336 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -360,14 +360,13 @@ static OSD_Entry cmsx_menuImuEntries[] = {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1}, 0}, {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0}, - {"OTHER PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, + {"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, + {"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, - {"FLT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, - - {"FLT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, + {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 565ea15bf6..4e9de00e38 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -165,7 +165,10 @@ static smartAudioDevice_t saDevicePrev = { }; // XXX Possible compliance problem here. Need LOCK/UNLOCK menu? -static uint8_t saLockMode = SA_MODE_SET_UNLOCK; +static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable? + +// XXX Should be configurable by user? +static bool saDeferred = true; // saCms variable? // Receive frame reassembly buffer #define SA_MAX_RCVLEN 11 @@ -732,27 +735,29 @@ uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently void saCmsUpdate(void) { +// XXX Take care of pit mode update somewhere??? + if (saCmsOpmodel == SA_OPMODEL_UNDEF) { // This is a first valid response to GET_SETTINGS. saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SA_OPMODEL_PIT : SA_OPMODEL_FREE; - } - saCmsBand = (saDevice.chan / 8) + 1; - saCmsChan = (saDevice.chan % 8) + 1; - saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsBand = (saDevice.chan / 8) + 1; + saCmsChan = (saDevice.chan % 8) + 1; + saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; - if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { - saCmsRFState = SA_TXMODE_ACTIVE; - } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - saCmsRFState = SA_TXMODE_PIT_INRANGE; - } else { - saCmsRFState = SA_TXMODE_PIT_OUTRANGE; - } + if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { + saCmsRFState = SA_TXMODE_ACTIVE; + } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + saCmsRFState = SA_TXMODE_PIT_INRANGE; + } else { + saCmsRFState = SA_TXMODE_PIT_OUTRANGE; + } - if (saDevice.version == 2) { - saCmsPower = saDevice.power + 1; // XXX Take care V1 - } else { - saCmsPower = saDacToPowerIndex(saDevice.power) + 1; + if (saDevice.version == 2) { + saCmsPower = saDevice.power + 1; // XXX Take care V1 + } else { + saCmsPower = saDacToPowerIndex(saDevice.power) + 1; + } } saUpdateStatusString(); @@ -826,13 +831,18 @@ static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) return 0; } +dprintf(("saCmsConfigBand: band req %d ", saCmsBand)); + if (saCmsBand == 0) { // Bouce back, no going back to undef state saCmsBand = 1; +dprintf(("--> %d\r\n", saCmsBand)); return 0; } +dprintf(("--> %d\r\n", saCmsBand)); - saSetBandChan(saCmsBand - 1, saCmsChan - 1); + if (!(saCmsOpmodel == SA_OPMODEL_FREE && saDeferred)) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; } @@ -854,7 +864,8 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) return 0; } - saSetBandChan(saCmsBand - 1, saCmsChan - 1); + if (!(saCmsOpmodel == SA_OPMODEL_FREE && saDeferred)) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; } @@ -1031,15 +1042,22 @@ static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self) return 0; } -static long saCmsClearPitMode(displayPort_t *pDisp, const void *self) +static long saCmsCommence(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); - if (saCmsPitFMode == 0) - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); - else - saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); + if (saCmsOpmodel == SA_OPMODEL_PIT) { + if (saCmsPitFMode == 0) + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); + else + saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); + } else { + if (saCmsFreqMode == 0) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + else + saSetFreq(saCmsUserFreq); + } return 0; } @@ -1153,7 +1171,7 @@ static CMS_Menu menu_smartAudioConfig = { static OSD_Entry saCmsMenuCommenceEntries[] = { { "CONFIRM", OME_Label, NULL, NULL, 0 }, - { "YES", OME_Funcall, saCmsClearPitMode, NULL, 0 }, + { "YES", OME_Funcall, saCmsCommence, NULL, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; @@ -1172,7 +1190,7 @@ static OSD_Entry saCmsMenuFreqModeEntries[] = { { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, { "FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuUserFreq, 0 }, { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, - { "START", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -1186,7 +1204,7 @@ static OSD_Entry saCmsMenuChanModeEntries[] = { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreq, DYNAMIC }, { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, - { "START", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } From f1035c52eb90fe172581a18c5a0d25a429de57f0 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 25 Nov 2016 20:20:29 +0900 Subject: [PATCH 27/40] Minor touch up OME_Submenu unconditionally calls cmsMenuChange, so p->func test is not required. --- src/main/cms/cms.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 14fb32fc7e..5ee3cf05fa 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -669,7 +669,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) switch (p->type) { case OME_Submenu: - if (p->func && key == KEY_RIGHT) { + if (key == KEY_RIGHT) { cmsMenuChange(pDisplay, p->data); res = BUTTON_PAUSE; } From fc1c8d759695dab8c1752f1087cfca67cc282940 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 25 Nov 2016 22:01:28 +0900 Subject: [PATCH 28/40] OME_Funcall now chains functions based on p->func's return value --- src/main/cms/cms.c | 11 ++++++++++- src/main/cms/cms_types.h | 4 +++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 5ee3cf05fa..980022008e 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -675,7 +675,16 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } break; - case OME_Funcall: + case OME_Funcall:; + long retval; + if (p->func && key == KEY_RIGHT) { + retval = p->func(pDisplay, p->data); + if (retval == MENU_CHAIN_BACK) + cmsMenuBack(pDisplay); + res = BUTTON_PAUSE; + } + break; + case OME_OSD_Exit: if (p->func && key == KEY_RIGHT) { p->func(pDisplay, p->data); diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index 5650ca97ac..4bea82e62a 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -78,9 +78,11 @@ typedef struct #define IS_DYNAMIC(p) ((p)->flags & DYNAMIC) - typedef long (*CMSMenuFuncPtr)(void); +// Special return value(s) for function chaining by CMSMenuFuncPtr +#define MENU_CHAIN_BACK (-1) // Causes automatic cmsMenuBack + /* onExit function is called with self: (1) Pointer to an OSD_Entry when cmsMenuBack() was called. From 093fd146e40acd9158f0da5db173223faf362189 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 26 Nov 2016 23:09:00 +0900 Subject: [PATCH 29/40] Menu contents tidy --- src/main/io/vtx_smartaudio.c | 147 +++++++++++++++++------------------ 1 file changed, 73 insertions(+), 74 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 4e9de00e38..392dedfd00 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -940,35 +940,28 @@ static const char * const saCmsDeviceStatusNames[] = { }; static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; -static OSD_UINT16_t saCmsEntBaudrate = { &sa_smartbaud, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatPktSent = { &saStat.pktsent, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatPktRcvd = { &saStat.pktrcvd, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatBadpre = { &saStat.badpre, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatBadlen = { &saStat.badlen, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatCrcerr = { &saStat.crc, 0, 0, 0 }; -static OSD_UINT16_t saCmsEntStatOooerr = { &saStat.ooopresp, 0, 0, 0 }; -static OSD_Entry menu_smartAudioStatsEntries[] = { +static OSD_Entry saCmsMenuStatsEntries[] = { { "- SA STATS -", OME_Label, NULL, NULL, 0 }, - { "STATUS", OME_TAB, NULL, &saCmsEntOnline, DYNAMIC }, - { "BAUDRATE", OME_UINT16, NULL, &saCmsEntBaudrate, DYNAMIC }, - { "SENT", OME_UINT16, NULL, &saCmsEntStatPktSent, DYNAMIC }, - { "RCVD", OME_UINT16, NULL, &saCmsEntStatPktRcvd, DYNAMIC }, - { "BADPRE", OME_UINT16, NULL, &saCmsEntStatBadpre, DYNAMIC }, - { "BADLEN", OME_UINT16, NULL, &saCmsEntStatBadlen, DYNAMIC }, - { "CRCERR", OME_UINT16, NULL, &saCmsEntStatCrcerr, DYNAMIC }, - { "OOOERR", OME_UINT16, NULL, &saCmsEntStatOooerr, DYNAMIC }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + { "STATUS", OME_TAB, NULL, &saCmsEntOnline, DYNAMIC }, + { "BAUDRATE", OME_UINT16, NULL, &(OSD_UINT16_t){ &sa_smartbaud, 0, 0, 0 }, DYNAMIC }, + { "SENT", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktsent, 0, 0, 0 }, DYNAMIC }, + { "RCVD", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.pktrcvd, 0, 0, 0 }, DYNAMIC }, + { "BADPRE", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badpre, 0, 0, 0 }, DYNAMIC }, + { "BADLEN", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.badlen, 0, 0, 0 }, DYNAMIC }, + { "CRCERR", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.crc, 0, 0, 0 }, DYNAMIC }, + { "OOOERR", OME_UINT16, NULL, &(OSD_UINT16_t){ &saStat.ooopresp, 0, 0, 0 }, DYNAMIC }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; -static CMS_Menu menu_smartAudioStats = { +static CMS_Menu saCmsMenuStats = { .GUARD_text = "XSAST", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = menu_smartAudioStatsEntries + .entries = saCmsMenuStatsEntries }; static const char * const saCmsBandNames[] = { @@ -996,7 +989,7 @@ static const char * const saCmsPowerNames[] = { "800", }; -static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, &saCmsPowerNames[0]}; +static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; static OSD_UINT16_t saCmsEntFreq = { &saCmsDeviceFreq, 5600, 5900, 0 }; @@ -1006,21 +999,17 @@ static const char * const saCmsOpmodelNames[] = { "PIT ", }; -static OSD_TAB_t saCmsEntOpModel = { &saCmsOpmodel, 2, &saCmsOpmodelNames[0] }; - static const char * const saCmsFreqModeNames[] = { "CHAN", "USER" }; -static OSD_TAB_t saCmsEntFreqMode = { &saCmsFreqMode, 1, saCmsFreqModeNames }; - static const char * const saCmsPitFModeNames[] = { "IN-R ", "OUT-R" }; -static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, &saCmsPitFModeNames[0] }; +static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames }; static long sacms_SetupTopMenu(void); // Forward @@ -1062,9 +1051,6 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) return 0; } -static OSD_UINT16_t saCmsEntORFreq = { &saCmsORFreq, 5000, 5900, 0 }; -static OSD_UINT16_t saCmsEntORFreqNew = { &saCmsORFreqNew, 5000, 5900, 1 }; - static long saCmsSetPORFreqOnEnter(void) { saCmsORFreqNew = saCmsORFreq; @@ -1091,9 +1077,6 @@ static char *saCmsORFreqGetString(void) return pbuf; } -static OSD_UINT16_t saCmsEntUserFreq = { &saCmsUserFreq, 5000, 5900, 0 }; -static OSD_UINT16_t saCmsEntUserFreqNew = { &saCmsUserFreqNew, 5000, 5900, 1 }; - static long saCmsSetUserFreqOnEnter(void) { saCmsUserFreqNew = saCmsUserFreq; @@ -1112,12 +1095,14 @@ static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self) } static OSD_Entry saCmsMenuPORFreqEntries[] = { - { "- POR FREQ -", OME_Label, NULL, NULL, 0 }, - { "CUR FREQ", OME_UINT16, NULL, &saCmsEntORFreq, DYNAMIC }, - { "NEW FREQ", OME_UINT16, NULL, &saCmsEntORFreqNew, 0 }, - { "SET", OME_Funcall, saCmsSetPORFreq, NULL, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + { "- POR FREQ -", OME_Label, NULL, NULL, 0 }, + + { "CUR FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsORFreq, 5000, 5900, 0 }, DYNAMIC }, + { "NEW FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 }, 0 }, + { "SET", OME_Funcall, saCmsSetPORFreq, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; static CMS_Menu saCmsMenuPORFreq = @@ -1131,12 +1116,14 @@ static CMS_Menu saCmsMenuPORFreq = }; static OSD_Entry saCmsMenuUserFreqEntries[] = { - { "- USER FREQ -", OME_Label, NULL, NULL, 0 }, - { "CUR FREQ", OME_UINT16, NULL, &saCmsEntUserFreq, DYNAMIC }, - { "NEW FREQ", OME_UINT16, NULL, &saCmsEntUserFreqNew, 0 }, - { "SET", OME_Funcall, saCmsSetUserFreq, NULL, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + { "- USER FREQ -", OME_Label, NULL, NULL, 0 }, + + { "CUR FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsUserFreq, 5000, 5900, 0 }, DYNAMIC }, + { "NEW FREQ", OME_UINT16, NULL, &(OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 }, 0 }, + { "SET", OME_Funcall, saCmsSetUserFreq, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; static CMS_Menu saCmsMenuUserFreq = @@ -1149,31 +1136,37 @@ static CMS_Menu saCmsMenuUserFreq = .entries = saCmsMenuUserFreqEntries, }; -static OSD_Entry menu_smartAudioConfigEntries[] = { +static OSD_TAB_t saCmsEntFreqMode = { &saCmsFreqMode, 1, saCmsFreqModeNames }; + +static OSD_Entry saCmsMenuConfigEntries[] = { { "- SA CONFIG -", OME_Label, NULL, NULL, 0 }, - { "OP MODEL", OME_TAB, saCmsConfigOpModelByGvar, &saCmsEntOpModel, 0 }, - { "FREQ MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFreqMode, 0 }, - { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 }, - { "POR FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq, OPTSTRING }, - { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, + + { "OP MODEL", OME_TAB, saCmsConfigOpModelByGvar, &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, 0 }, + { "FREQ MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFreqMode, 0 }, + { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 }, + { "POR FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq, OPTSTRING }, + { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; -static CMS_Menu menu_smartAudioConfig = { +static CMS_Menu saCmsMenuConfig = { .GUARD_text = "XSACFG", .GUARD_type = OME_MENU, .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = menu_smartAudioConfigEntries + .entries = saCmsMenuConfigEntries }; static OSD_Entry saCmsMenuCommenceEntries[] = { - { "CONFIRM", OME_Label, NULL, NULL, 0 }, - { "YES", OME_Funcall, saCmsCommence, NULL, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + { "CONFIRM", OME_Label, NULL, NULL, 0 }, + + { "YES", OME_Funcall, saCmsCommence, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; static CMS_Menu saCmsMenuCommence = { @@ -1187,11 +1180,13 @@ static CMS_Menu saCmsMenuCommence = { static OSD_Entry saCmsMenuFreqModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, - { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuUserFreq, 0 }, - { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, - { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, - { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, + + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuUserFreq, 0 }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; @@ -1199,24 +1194,28 @@ static OSD_Entry saCmsMenuFreqModeEntries[] = { static OSD_Entry saCmsMenuChanModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, - { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, - { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, - { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreq, DYNAMIC }, - { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, - { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, - { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, + { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, + { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreq, DYNAMIC }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; static OSD_Entry saCmsMenuOfflineEntries[] = { { "- VTX SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, - { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "STATX", OME_Submenu, cmsMenuChange, &menu_smartAudioStats, 0 }, - { "BACK", OME_Back, NULL, NULL, 0 }, - { NULL, OME_END, NULL, NULL, 0 } + + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } }; CMS_Menu cmsx_menuVtxSmartAudio; // Forward From aad3fc58cfff637a25051628c46e8dfbd79d3565 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 30 Nov 2016 11:19:25 +0900 Subject: [PATCH 30/40] Variable naming consistency --- src/main/io/vtx_smartaudio.c | 64 +++++++++++++++++++++--------------- 1 file changed, 38 insertions(+), 26 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 392dedfd00..9261984f3c 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -703,18 +703,23 @@ void smartAudioProcess(uint32_t now) #ifdef CMS +// Interface to CMS + // Operational Model and RF modes (CMS) -#define SA_OPMODEL_UNDEF 0 // Not known yet -#define SA_OPMODEL_FREE 1 // Power up transmitting -#define SA_OPMODEL_PIT 2 // Power up in pit mode +#define SACMS_OPMODEL_UNDEF 0 // Not known yet +#define SACMS_OPMODEL_FREE 1 // Freestyle model: Power up transmitting +#define SACMS_OPMODEL_RACE 2 // Race model: Power up in pit mode -#define SA_TXMODE_NODEF 0 -#define SA_TXMODE_PIT_OUTRANGE 1 -#define SA_TXMODE_PIT_INRANGE 2 -#define SA_TXMODE_ACTIVE 3 +uint8_t saCmsOpmodel = SACMS_OPMODEL_UNDEF; + +#define SACMS_TXMODE_NODEF 0 +#define SACMS_TXMODE_PIT_OUTRANGE 1 +#define SACMS_TXMODE_PIT_INRANGE 2 +#define SACMS_TXMODE_ACTIVE 3 + +uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently used -uint8_t saCmsOpmodel = SA_OPMODEL_UNDEF; uint8_t saCmsBand = 0; uint8_t saCmsChan = 0; uint8_t saCmsPower = 0; @@ -731,26 +736,24 @@ uint16_t saCmsORFreqNew; // POR frequency uint16_t saCmsUserFreq = 0; // User defined frequency uint16_t saCmsUserFreqNew; // User defined frequency -uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently used - void saCmsUpdate(void) { // XXX Take care of pit mode update somewhere??? - if (saCmsOpmodel == SA_OPMODEL_UNDEF) { + if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { // This is a first valid response to GET_SETTINGS. - saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SA_OPMODEL_PIT : SA_OPMODEL_FREE; + saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; saCmsBand = (saDevice.chan / 8) + 1; saCmsChan = (saDevice.chan % 8) + 1; saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { - saCmsRFState = SA_TXMODE_ACTIVE; + saCmsRFState = SACMS_TXMODE_ACTIVE; } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - saCmsRFState = SA_TXMODE_PIT_INRANGE; + saCmsRFState = SACMS_TXMODE_PIT_INRANGE; } else { - saCmsRFState = SA_TXMODE_PIT_OUTRANGE; + saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; } if (saDevice.version == 2) { @@ -841,7 +844,7 @@ dprintf(("--> %d\r\n", saCmsBand)); } dprintf(("--> %d\r\n", saCmsBand)); - if (!(saCmsOpmodel == SA_OPMODEL_FREE && saDeferred)) + if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; @@ -864,7 +867,7 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) return 0; } - if (!(saCmsOpmodel == SA_OPMODEL_FREE && saDeferred)) + if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); return 0; @@ -918,11 +921,11 @@ static long saCmsConfigOpModelByGvar(displayPort_t *pDisp, const void *self) dprintf(("saCmsConfigOpModelByGvar: opmodel %d\r\n", opmodel)); - if (opmodel == SA_OPMODEL_FREE) { + if (opmodel == SACMS_OPMODEL_FREE) { // VTX should power up transmitting. // Turn off In-Range and Out-Range bits saSetMode(0); - } else if (opmodel == SA_OPMODEL_PIT) { + } else if (opmodel == SACMS_OPMODEL_RACE) { // VTX should power up in pit mode. // Default PitFMode is in-range to prevent users without // out-range receivers from getting blinded. @@ -996,7 +999,7 @@ static OSD_UINT16_t saCmsEntFreq = { &saCmsDeviceFreq, 5600, 5900, 0 }; static const char * const saCmsOpmodelNames[] = { "----", "FREE", - "PIT ", + "RACE", }; static const char * const saCmsFreqModeNames[] = { @@ -1036,7 +1039,7 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - if (saCmsOpmodel == SA_OPMODEL_PIT) { + if (saCmsOpmodel == SACMS_OPMODEL_RACE) { if (saCmsPitFMode == 0) saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); else @@ -1077,6 +1080,15 @@ static char *saCmsORFreqGetString(void) return pbuf; } +static char *saCmsUserFreqGetString(void) +{ + static char pbuf[5]; + + tfp_sprintf(pbuf, "%4d", saCmsUserFreq); + + return pbuf; +} + static long saCmsSetUserFreqOnEnter(void) { saCmsUserFreqNew = saCmsUserFreq; @@ -1181,11 +1193,11 @@ static CMS_Menu saCmsMenuCommence = { static OSD_Entry saCmsMenuFreqModeEntries[] = { { "- SMARTAUDIO -", OME_Label, NULL, NULL, 0 }, - { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, - { "FREQ", OME_Submenu, cmsMenuChange, &saCmsMenuUserFreq, 0 }, - { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, - { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, - { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, + { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, + { "FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsUserFreqGetString, &saCmsMenuUserFreq, OPTSTRING }, + { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, + { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } From 336f8714058b9e638d30e4cb24664e3e06d2430f Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 30 Nov 2016 12:57:14 +0900 Subject: [PATCH 31/40] Fixed frequency reference for band/chan mode to dynamically update --- src/main/io/vtx_smartaudio.c | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 9261984f3c..7b23301b08 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -723,6 +723,10 @@ uint8_t saCmsRFState; // RF state; ACTIVE, PIR, POR XXX Not currently uint8_t saCmsBand = 0; uint8_t saCmsChan = 0; uint8_t saCmsPower = 0; + +// Frequency derived from channel table (used for reference in band/chan mode) +uint16_t saCmsFreqRef = 0; + uint16_t saCmsDeviceFreq = 0; uint8_t saCmsDeviceStatus = 0; @@ -746,6 +750,8 @@ void saCmsUpdate(void) saCmsBand = (saDevice.chan / 8) + 1; saCmsChan = (saDevice.chan % 8) + 1; + saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { @@ -839,14 +845,14 @@ dprintf(("saCmsConfigBand: band req %d ", saCmsBand)); if (saCmsBand == 0) { // Bouce back, no going back to undef state saCmsBand = 1; -dprintf(("--> %d\r\n", saCmsBand)); return 0; } -dprintf(("--> %d\r\n", saCmsBand)); if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); + saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1]; + return 0; } @@ -870,6 +876,8 @@ static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) if (!(saCmsOpmodel == SACMS_OPMODEL_FREE && saDeferred)) saSetBandChan(saCmsBand - 1, saCmsChan - 1); + saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1]; + return 0; } @@ -994,8 +1002,11 @@ static const char * const saCmsPowerNames[] = { static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; +// Frequency the vtx is currently transmitting at static OSD_UINT16_t saCmsEntFreq = { &saCmsDeviceFreq, 5600, 5900, 0 }; +static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 }; + static const char * const saCmsOpmodelNames[] = { "----", "FREE", @@ -1051,7 +1062,7 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) saSetFreq(saCmsUserFreq); } - return 0; + return MENU_CHAIN_BACK; } static long saCmsSetPORFreqOnEnter(void) @@ -1210,7 +1221,7 @@ static OSD_Entry saCmsMenuChanModeEntries[] = { "", OME_Label, NULL, saCmsStatusString, DYNAMIC }, { "BAND", OME_TAB, saCmsConfigBandByGvar, &saCmsEntBand, 0 }, { "CHAN", OME_TAB, saCmsConfigChanByGvar, &saCmsEntChan, 0 }, - { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreq, DYNAMIC }, + { "(FREQ)", OME_UINT16, NULL, &saCmsEntFreqRef, DYNAMIC }, { "POWER", OME_TAB, saCmsConfigPowerByGvar, &saCmsEntPower, 0 }, { "SET", OME_Submenu, cmsMenuChange, &saCmsMenuCommence, 0 }, { "CONFIG", OME_Submenu, cmsMenuChange, &saCmsMenuConfig, 0 }, From 0b41697a8db694a53c1aed903c57e11cfce1966d Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 9 Dec 2016 17:56:39 +0900 Subject: [PATCH 32/40] Disabled dprintf & set OPTIMIZE to -O2 (for testing) Makefile should eventually get coherent with system wide settings once #1747 is done. --- Makefile | 3 +- src/main/io/serial.h.orig | 158 ----------------------------------- src/main/io/vtx_smartaudio.c | 2 +- 3 files changed, 3 insertions(+), 160 deletions(-) delete mode 100644 src/main/io/serial.h.orig diff --git a/Makefile b/Makefile index f2edc40d58..244922fecb 100644 --- a/Makefile +++ b/Makefile @@ -760,7 +760,8 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) OPTIMIZE = -Os else -OPTIMIZE = -Ofast +#OPTIMIZE = -Ofast +OPTIMIZE = -O2 endif LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) endif diff --git a/src/main/io/serial.h.orig b/src/main/io/serial.h.orig deleted file mode 100644 index cc23aa7c99..0000000000 --- a/src/main/io/serial.h.orig +++ /dev/null @@ -1,158 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#include "drivers/serial.h" - -typedef enum { - PORTSHARING_UNUSED = 0, - PORTSHARING_NOT_SHARED, - PORTSHARING_SHARED -} portSharing_e; - -typedef enum { - FUNCTION_NONE = 0, - FUNCTION_MSP = (1 << 0), // 1 - FUNCTION_GPS = (1 << 1), // 2 - FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4 - FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8 - FUNCTION_TELEMETRY_LTM = (1 << 4), // 16 - FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32 - FUNCTION_RX_SERIAL = (1 << 6), // 64 - FUNCTION_BLACKBOX = (1 << 7), // 128 - - FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 -<<<<<<< HEAD - FUNCTION_TELEMETRY_ESC = (1 << 10), // 1024 - FUNCTION_VTX_CONTROL = (1 << 11),// 2048 -======= - FUNCTION_TELEMETRY_ESC = (1 << 10) // 1024 ->>>>>>> betaflight/master -} serialPortFunction_e; - -typedef enum { - BAUD_AUTO = 0, - BAUD_9600, - BAUD_19200, - BAUD_38400, - BAUD_57600, - BAUD_115200, - BAUD_230400, - BAUD_250000, - BAUD_400000, - BAUD_460800, - BAUD_500000, - BAUD_921600, - BAUD_1000000, - BAUD_1500000, - BAUD_2000000, - BAUD_2470000 -} baudRate_e; - -extern const uint32_t baudRates[]; - -// serial port identifiers are now fixed, these values are used by MSP commands. -typedef enum { - SERIAL_PORT_NONE = -1, - SERIAL_PORT_USART1 = 0, - SERIAL_PORT_USART2, - SERIAL_PORT_USART3, - SERIAL_PORT_USART4, - SERIAL_PORT_USART5, - SERIAL_PORT_USART6, - SERIAL_PORT_USART7, - SERIAL_PORT_USART8, - SERIAL_PORT_USB_VCP = 20, - SERIAL_PORT_SOFTSERIAL1 = 30, - SERIAL_PORT_SOFTSERIAL2 -} serialPortIdentifier_e; - -extern const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT]; - -// -// runtime -// -typedef struct serialPortUsage_s { - serialPortIdentifier_e identifier; - serialPort_t *serialPort; - serialPortFunction_e function; -} serialPortUsage_t; - -serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); -serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); - -// -// configuration -// -typedef struct serialPortConfig_s { - serialPortIdentifier_e identifier; - uint16_t functionMask; - uint8_t msp_baudrateIndex; - uint8_t gps_baudrateIndex; - uint8_t blackbox_baudrateIndex; - uint8_t telemetry_baudrateIndex; // not used for all telemetry systems, e.g. HoTT only works at 19200. -} serialPortConfig_t; - -typedef struct serialConfig_s { - uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. - serialPortConfig_t portConfigs[SERIAL_PORT_COUNT]; -} serialConfig_t; - -typedef void serialConsumer(uint8_t); - -// -// configuration -// -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); -void serialRemovePort(serialPortIdentifier_e identifier); -uint8_t serialGetAvailablePortCount(void); -bool serialIsPortAvailable(serialPortIdentifier_e identifier); -bool isSerialConfigValid(serialConfig_t *serialConfig); -serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); -bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); -serialPortConfig_t *findSerialPortConfig(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); - -serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); -int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); -// -// runtime -// -serialPort_t *openSerialPort( - serialPortIdentifier_e identifier, - serialPortFunction_e function, - serialReceiveCallbackPtr rxCallback, - uint32_t baudrate, - portMode_t mode, - portOptions_t options -); -void closeSerialPort(serialPort_t *serialPort); - -void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort); - -baudRate_e lookupBaudRateIndex(uint32_t baudRate); - - -// -// msp/cli/bootloader -// -void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar); -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 7b23301b08..5b335a2246 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -29,7 +29,7 @@ #include "build/build_config.h" -#define SMARTAUDIO_DPRINTF +//#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR #ifdef SMARTAUDIO_DPRINTF From e2e7835cf8cb6fa6d571a9d500446af82c22d30b Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 10 Dec 2016 20:53:31 +0900 Subject: [PATCH 33/40] Add cms files and vtx_smartaudio.c as size optimized --- Makefile | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Makefile b/Makefile index ba33689dbd..d53e7dc246 100644 --- a/Makefile +++ b/Makefile @@ -686,6 +686,15 @@ SIZE_OPTIMISED_SRC = \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ msp/msp_serial.c \ + cms/cms.c \ + cms/cms_menu_blackbox.c \ + cms/cms_menu_builtin.c \ + cms/cms_menu_imu.c \ + cms/cms_menu_ledstrip.c \ + cms/cms_menu_misc.c \ + cms/cms_menu_osd.c \ + cms/cms_menu_vtx.c \ + io/vtx_smartaudio.c ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ From 53e637d01e95c96339b8620e97dfa15cac08099e Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 13 Dec 2016 13:35:59 +0900 Subject: [PATCH 34/40] Fix minor glitches --- src/main/cms/cms_menu_builtin.c | 2 +- src/main/io/vtx_smartaudio.c | 49 +++++++++++++++++++-------------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 83909fc969..a0dc6b19ea 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -97,7 +97,7 @@ static OSD_Entry menuFeaturesEntries[] = {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 #if defined(VTX_SMARTAUDIO) - {"VTX SA", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, + {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, #endif #ifdef LED_STRIP {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 5b335a2246..ede9e58ca7 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -732,7 +732,7 @@ uint16_t saCmsDeviceFreq = 0; uint8_t saCmsDeviceStatus = 0; uint8_t saCmsPower; uint8_t saCmsPitFMode; // In-Range or Out-Range -uint8_t saCmsFreqMode; // Channel or User defined +uint8_t saCmsFselMode; // Channel(0) or User defined(1) uint16_t saCmsORFreq = 0; // POR frequency uint16_t saCmsORFreqNew; // POR frequency @@ -748,6 +748,8 @@ void saCmsUpdate(void) // This is a first valid response to GET_SETTINGS. saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; + saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; + saCmsBand = (saDevice.chan / 8) + 1; saCmsChan = (saDevice.chan % 8) + 1; saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; @@ -776,7 +778,7 @@ char saCmsStatusString[31] = "- -- ---- ---"; // m bc ffff ppp // 0123456789012 -static long saCmsConfigOpModelByGvar(displayPort_t *, const void *self); +static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self); static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self); static long saCmsConfigBandByGvar(displayPort_t *, const void *self); static long saCmsConfigChanByGvar(displayPort_t *, const void *self); @@ -800,7 +802,7 @@ if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) else saCmsPitFMode = 0; - saCmsStatusString[0] = "-FP"[saCmsOpmodel]; + saCmsStatusString[0] = "-FP"[(saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE]; saCmsStatusString[2] = "ABEFR"[saDevice.chan / 8]; saCmsStatusString[3] = '1' + (saDevice.chan % 8); @@ -919,15 +921,14 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) return 0; } -static long saCmsConfigOpModelByGvar(displayPort_t *pDisp, const void *self) +static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); UNUSED(self); uint8_t opmodel = saCmsOpmodel; - dprintf(("saCmsConfigOpModelByGvar: opmodel %d\r\n", opmodel)); - + dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel)); if (opmodel == SACMS_OPMODEL_FREE) { // VTX should power up transmitting. @@ -939,6 +940,9 @@ static long saCmsConfigOpModelByGvar(displayPort_t *pDisp, const void *self) // out-range receivers from getting blinded. saCmsPitFMode = 0; saCmsConfigPitFModeByGvar(pDisp, self); + } else { + // Trying to go back to unknown state; bounce back + saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1; } return 0; @@ -1002,9 +1006,6 @@ static const char * const saCmsPowerNames[] = { static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; -// Frequency the vtx is currently transmitting at -static OSD_UINT16_t saCmsEntFreq = { &saCmsDeviceFreq, 5600, 5900, 0 }; - static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 }; static const char * const saCmsOpmodelNames[] = { @@ -1013,14 +1014,14 @@ static const char * const saCmsOpmodelNames[] = { "RACE", }; -static const char * const saCmsFreqModeNames[] = { +static const char * const saCmsFselModeNames[] = { "CHAN", "USER" }; static const char * const saCmsPitFModeNames[] = { - "IN-R ", - "OUT-R" + "PIR", + "POR" }; static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames }; @@ -1032,12 +1033,17 @@ static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - if (saCmsFreqMode == 0) { + if (saCmsFselMode == 0) { // CHAN saSetBandChan(saCmsBand - 1, saCmsChan - 1); } else { - // USER - saSetFreq(saCmsUserFreq); + // USER: User frequency mode is only available in FREE opmodel. + if (saCmsOpmodel == SACMS_OPMODEL_FREE) { + saSetFreq(saCmsUserFreq); + } else { + // Bounce back + saCmsFselMode = 0; + } } sacms_SetupTopMenu(); @@ -1056,7 +1062,7 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) else saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE); } else { - if (saCmsFreqMode == 0) + if (saCmsFselMode == 0) saSetBandChan(saCmsBand - 1, saCmsChan - 1); else saSetFreq(saCmsUserFreq); @@ -1112,7 +1118,8 @@ static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - saSetFreq(saCmsUserFreqNew); + saCmsUserFreq = saCmsUserFreqNew; + saSetFreq(saCmsUserFreq); return 0; } @@ -1159,13 +1166,13 @@ static CMS_Menu saCmsMenuUserFreq = .entries = saCmsMenuUserFreqEntries, }; -static OSD_TAB_t saCmsEntFreqMode = { &saCmsFreqMode, 1, saCmsFreqModeNames }; +static OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames }; static OSD_Entry saCmsMenuConfigEntries[] = { { "- SA CONFIG -", OME_Label, NULL, NULL, 0 }, - { "OP MODEL", OME_TAB, saCmsConfigOpModelByGvar, &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, 0 }, - { "FREQ MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFreqMode, 0 }, + { "OP MODEL", OME_TAB, saCmsConfigOpmodelByGvar, &(OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, 0 }, + { "FSEL MODE", OME_TAB, saCmsConfigFreqModeByGvar, &saCmsEntFselMode, 0 }, { "PIT FMODE", OME_TAB, saCmsConfigPitFModeByGvar, &saCmsEntPitFMode, 0 }, { "POR FREQ", OME_Submenu, (CMSEntryFuncPtr)saCmsORFreqGetString, &saCmsMenuPORFreq, OPTSTRING }, { "STATX", OME_Submenu, cmsMenuChange, &saCmsMenuStats, 0 }, @@ -1246,7 +1253,7 @@ CMS_Menu cmsx_menuVtxSmartAudio; // Forward static long sacms_SetupTopMenu(void) { if (saCmsDeviceStatus) { - if (saCmsFreqMode == 0) + if (saCmsFselMode == 0) cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries; else cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries; From 177da8b3e3a0b05787a0e4a6f48bd9474bc040cf Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 18 Dec 2016 18:50:48 +0900 Subject: [PATCH 35/40] Delete .orig Is there any reason not to add .orig to .gitignore? --- Makefile.orig | 1134 ------------------------------------------------- 1 file changed, 1134 deletions(-) delete mode 100644 Makefile.orig diff --git a/Makefile.orig b/Makefile.orig deleted file mode 100644 index 6d60e98c3a..0000000000 --- a/Makefile.orig +++ /dev/null @@ -1,1134 +0,0 @@ -############################################################################### -# "THE BEER-WARE LICENSE" (Revision 42): -# wrote this file. As long as you retain this notice you -# can do whatever you want with this stuff. If we meet some day, and you think -# this stuff is worth it, you can buy me a beer in return -############################################################################### -# -# Makefile for building the betaflight firmware. -# -# Invoke this with 'make help' to see the list of supported targets. -# -############################################################################### - - -# Things that the user might override on the commandline -# - -# The target to build, see VALID_TARGETS below -TARGET ?= NAZE - -# Compile-time options -OPTIONS ?= - -# compile for OpenPilot BootLoader support -OPBL ?= no - -# Debugger optons, must be empty or GDB -DEBUG ?= - -# Insert the debugging hardfault debugger -# releases should not be built with this flag as it does not disable pwm output -DEBUG_HARDFAULTS ?= - -# Serial port/Device for flashing -SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found) - -# Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override. -FLASH_SIZE ?= - -## V : Set verbosity level based on the V= parameter -## V=0 Low -## V=1 High -export AT := @ - -ifndef V -export V0 := -export V1 := $(AT) -export STDOUT := -else ifeq ($(V), 0) -export V0 := $(AT) -export V1 := $(AT) -export STDOUT:= "> /dev/null" -export MAKE := $(MAKE) --no-print-directory -else ifeq ($(V), 1) -export V0 := -export V1 := -export STDOUT := -endif - -############################################################################### -# Things that need to be maintained as the source changes -# - -FORKNAME = betaflight - -# Working directories -ROOT := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) -SRC_DIR = $(ROOT)/src/main -OBJECT_DIR = $(ROOT)/obj/main -BIN_DIR = $(ROOT)/obj -CMSIS_DIR = $(ROOT)/lib/main/CMSIS -INCLUDE_DIRS = $(SRC_DIR) \ - $(ROOT)/src/main/target -LINKER_DIR = $(ROOT)/src/main/target/link - -# Build tools, so we all share the same versions -# import macros common to all supported build systems -include $(ROOT)/make/system-id.mk -# developer preferences, edit these at will, they'll be gitignored -include $(ROOT)/make/local.mk - -# configure some directories that are relative to wherever ROOT_DIR is located -TOOLS_DIR := $(ROOT)/tools -BUILD_DIR := $(ROOT)/build -DL_DIR := $(ROOT)/downloads - -export RM := rm - -# import macros that are OS specific -include $(ROOT)/make/$(OSFAMILY).mk - -# include the tools makefile -include $(ROOT)/make/tools.mk - -# default xtal value for F4 targets -HSE_VALUE = 8000000 - -# used for turning on features like VCP and SDCARD -FEATURES = - -SAMPLE_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 CC3D FURYF4 NAZE REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY -ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk))))) -OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS)) - -#VALID_TARGETS = $(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) -VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk)) -VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS))) -VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS) -VALID_TARGETS := $(sort $(VALID_TARGETS)) - -ifeq ($(filter $(TARGET),$(ALT_TARGETS)), $(TARGET)) -BASE_TARGET := $(firstword $(subst /,, $(subst ./src/main/target/,, $(dir $(wildcard $(ROOT)/src/main/target/*/$(TARGET).mk))))) --include $(ROOT)/src/main/target/$(BASE_TARGET)/$(TARGET).mk -else -BASE_TARGET := $(TARGET) -endif - -ifeq ($(filter $(TARGET),$(OPBL_TARGETS)), $(TARGET)) -OPBL = yes -endif - -# silently ignore if the file is not present. Allows for target specific. --include $(ROOT)/src/main/target/$(BASE_TARGET)/target.mk - -F4_TARGETS = $(F405_TARGETS) $(F411_TARGETS) -F7_TARGETS = $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS) - -ifeq ($(filter $(TARGET),$(VALID_TARGETS)),) -$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?) -endif - -ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),) -$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411 or F7x5. Have you prepared a valid target.mk?) -endif - -128K_TARGETS = $(F1_TARGETS) -256K_TARGETS = $(F3_TARGETS) -512K_TARGETS = $(F411_TARGETS) $(F7X5XE_TARGETS) -1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_TARGETS) -2048K_TARGETS = $(F7X5XI_TARGETS) - -# Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already. -ifeq ($(FLASH_SIZE),) -ifeq ($(TARGET),$(filter $(TARGET),$(2048K_TARGETS))) -FLASH_SIZE = 2048 -else ifeq ($(TARGET),$(filter $(TARGET),$(1024K_TARGETS))) -FLASH_SIZE = 1024 -else ifeq ($(TARGET),$(filter $(TARGET),$(512K_TARGETS))) -FLASH_SIZE = 512 -else ifeq ($(TARGET),$(filter $(TARGET),$(256K_TARGETS))) -FLASH_SIZE = 256 -else ifeq ($(TARGET),$(filter $(TARGET),$(128K_TARGETS))) -FLASH_SIZE = 128 -else -$(error FLASH_SIZE not configured for target $(TARGET)) -endif -endif - -# note that there is no hardfault debugging startup file assembly handler for other platforms -ifeq ($(DEBUG_HARDFAULTS),F3) -CFLAGS += -DDEBUG_HARDFAULTS -STM32F30x_COMMON_SRC = startup_stm32f3_debug_hardfault_handler.S -else -STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S -endif - -ifeq ($(DEBUG_HARDFAULTS),F7) -CFLAGS += -DDEBUG_HARDFAULTS -endif - -REVISION := $(shell git log -1 --format="%h") - -FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' ) -FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' ) -FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' ) - -FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH) - -# Search path for sources -VPATH := $(SRC_DIR):$(SRC_DIR)/startup -USBFS_DIR = $(ROOT)/lib/main/STM32_USB-FS-Device_Driver -USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c)) -FATFS_DIR = $(ROOT)/lib/main/FatFS -FATFS_SRC = $(notdir $(wildcard $(FATFS_DIR)/*.c)) - -CSOURCES := $(shell find $(SRC_DIR) -name '*.c') - -ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) -# F3 TARGETS - -STDPERIPH_DIR = $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) -EXCLUDES = stm32f30x_crc.c \ - stm32f30x_can.c - -STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) -DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) - -VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c)) - -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(STDPERIPH_DIR)/inc \ - $(CMSIS_DIR)/CM1/CoreSupport \ - $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x - -ifneq ($(filter VCP, $(FEATURES)),) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(USBFS_DIR)/inc \ - $(ROOT)/src/main/vcp - -VPATH := $(VPATH):$(USBFS_DIR)/src - -DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC)\ - $(USBPERIPH_SRC) -endif - -ifneq ($(filter SDCARD, $(FEATURES)),) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(FATFS_DIR) \ - -VPATH := $(VPATH):$(FATFS_DIR) -endif - -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld - -ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion -DEVICE_FLAGS = -DSTM32F303xC -DSTM32F303 -# End F3 targets -# -# Start F4 targets -else ifeq ($(TARGET),$(filter $(TARGET), $(F4_TARGETS))) - -#STDPERIPH -STDPERIPH_DIR = $(ROOT)/lib/main/STM32F4xx_StdPeriph_Driver -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) -EXCLUDES = stm32f4xx_crc.c \ - stm32f4xx_can.c \ - stm32f4xx_fmc.c \ - stm32f4xx_sai.c \ - stm32f4xx_cec.c \ - stm32f4xx_dsi.c \ - stm32f4xx_flash_ramfunc.c \ - stm32f4xx_fmpi2c.c \ - stm32f4xx_lptim.c \ - stm32f4xx_qspi.c \ - stm32f4xx_spdifrx.c \ - stm32f4xx_cryp.c \ - stm32f4xx_cryp_aes.c \ - stm32f4xx_hash_md5.c \ - stm32f4xx_cryp_des.c \ - stm32f4xx_rtc.c \ - stm32f4xx_hash.c \ - stm32f4xx_dbgmcu.c \ - stm32f4xx_cryp_tdes.c \ - stm32f4xx_hash_sha1.c - - -ifeq ($(TARGET),$(filter $(TARGET), $(F411_TARGETS))) -EXCLUDES += stm32f4xx_fsmc.c -endif - -STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) - -#USB -USBCORE_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Core -USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/src/*.c)) -USBOTG_DIR = $(ROOT)/lib/main/STM32_USB_OTG_Driver -USBOTG_SRC = $(notdir $(wildcard $(USBOTG_DIR)/src/*.c)) -EXCLUDES = usb_bsp_template.c \ - usb_conf_template.c \ - usb_hcd_int.c \ - usb_hcd.c \ - usb_otg.c - -USBOTG_SRC := $(filter-out ${EXCLUDES}, $(USBOTG_SRC)) -USBCDC_DIR = $(ROOT)/lib/main/STM32_USB_Device_Library/Class/cdc -USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/src/*.c)) -EXCLUDES = usbd_cdc_if_template.c -USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) -VPATH := $(VPATH):$(USBOTG_DIR)/src:$(USBCORE_DIR)/src:$(USBCDC_DIR)/src - -DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \ - $(USBOTG_SRC) \ - $(USBCORE_SRC) \ - $(USBCDC_SRC) - -#CMSIS -VPATH := $(VPATH):$(CMSIS_DIR)/CM4/CoreSupport:$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM4/CoreSupport/*.c \ - $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx/*.c)) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(STDPERIPH_DIR)/inc \ - $(USBOTG_DIR)/inc \ - $(USBCORE_DIR)/inc \ - $(USBCDC_DIR)/inc \ - $(USBFS_DIR)/inc \ - $(CMSIS_DIR)/CM4/CoreSupport \ - $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx \ - $(ROOT)/src/main/vcpf4 - -ifneq ($(filter SDCARD,$(FEATURES)),) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(FATFS_DIR) -VPATH := $(VPATH):$(FATFS_DIR) -endif - -#Flags -ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion - -ifeq ($(TARGET),$(filter $(TARGET),$(F411_TARGETS))) -DEVICE_FLAGS = -DSTM32F411xE -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f411.ld -else ifeq ($(TARGET),$(filter $(TARGET),$(F405_TARGETS))) -DEVICE_FLAGS = -DSTM32F40_41xxx -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f405.ld -else -$(error Unknown MCU for F4 target) -endif -DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) - -# End F4 targets -# -# Start F7 targets -else ifeq ($(TARGET),$(filter $(TARGET), $(F7_TARGETS))) - -#STDPERIPH -STDPERIPH_DIR = $(ROOT)/lib/main/STM32F7xx_HAL_Driver -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Src/*.c)) -EXCLUDES = stm32f7xx_hal_timebase_rtc_wakeup_template.c \ - stm32f7xx_hal_timebase_rtc_alarm_template.c \ - stm32f7xx_hal_timebase_tim_template.c - -STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) - -#USB -USBCORE_DIR = $(ROOT)/lib/main/Middlewares/ST/STM32_USB_Device_Library/Core -USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/Src/*.c)) -EXCLUDES = usbd_conf_template.c -USBCORE_SRC := $(filter-out ${EXCLUDES}, $(USBCORE_SRC)) - -USBCDC_DIR = $(ROOT)/lib/main/Middlewares/ST/STM32_USB_Device_Library/Class/CDC -USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/Src/*.c)) -EXCLUDES = usbd_cdc_if_template.c -USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) - -VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src - -DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \ - $(USBCORE_SRC) \ - $(USBCDC_SRC) - -#CMSIS -VPATH := $(VPATH):$(CMSIS_DIR)/CM7/Include:$(CMSIS_DIR)/CM7/Device/ST/STM32F7xx -VPATH := $(VPATH):$(STDPERIPH_DIR)/Src -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM7/Include/*.c \ - $(CMSIS_DIR)/CM7/Device/ST/STM32F7xx/*.c)) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(STDPERIPH_DIR)/Inc \ - $(USBCORE_DIR)/Inc \ - $(USBCDC_DIR)/Inc \ - $(CMSIS_DIR)/CM7/Include \ - $(CMSIS_DIR)/CM7/Device/ST/STM32F7xx/Include \ - $(ROOT)/src/main/vcp_hal - -ifneq ($(filter SDCARD,$(FEATURES)),) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(FATFS_DIR) -VPATH := $(VPATH):$(FATFS_DIR) -endif - -#Flags -ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion - -ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS))) -DEVICE_FLAGS = -DSTM32F745xx -DUSE_HAL_DRIVER -D__FPU_PRESENT -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld -else ifeq ($(TARGET),$(filter $(TARGET),$(F7X6XG_TARGETS))) -DEVICE_FLAGS = -DSTM32F746xx -DUSE_HAL_DRIVER -D__FPU_PRESENT -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f746.ld -else -$(error Unknown MCU for F7 target) -endif -DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) - -TARGET_FLAGS = -D$(TARGET) - -# End F7 targets -# -# Start F1 targets -else - -STDPERIPH_DIR = $(ROOT)/lib/main/STM32F10x_StdPeriph_Driver -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) -EXCLUDES = stm32f10x_crc.c \ - stm32f10x_cec.c \ - stm32f10x_can.c - -STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) - -# Search path and source files for the CMSIS sources -VPATH := $(VPATH):$(CMSIS_DIR)/CM3/CoreSupport:$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM3/CoreSupport/*.c \ - $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x/*.c)) - -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(STDPERIPH_DIR)/inc \ - $(CMSIS_DIR)/CM3/CoreSupport \ - $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \ - -DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC) - -ifneq ($(filter VCP, $(FEATURES)),) -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(USBFS_DIR)/inc \ - $(ROOT)/src/main/vcp - -VPATH := $(VPATH):$(USBFS_DIR)/src - -DEVICE_STDPERIPH_SRC := $(DEVICE_STDPERIPH_SRC) \ - $(USBPERIPH_SRC) - -endif - -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_$(FLASH_SIZE)k.ld -ARCH_FLAGS = -mthumb -mcpu=cortex-m3 - -ifeq ($(DEVICE_FLAGS),) -DEVICE_FLAGS = -DSTM32F10X_MD -endif -DEVICE_FLAGS += -DSTM32F10X - -endif -# -# End F1 targets -# -ifneq ($(BASE_TARGET), $(TARGET)) -TARGET_FLAGS := $(TARGET_FLAGS) -D$(BASE_TARGET) -endif - -ifneq ($(FLASH_SIZE),) -DEVICE_FLAGS := $(DEVICE_FLAGS) -DFLASH_SIZE=$(FLASH_SIZE) -endif - -ifneq ($(HSE_VALUE),) -DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE) -endif - -TARGET_DIR = $(ROOT)/src/main/target/$(BASE_TARGET) -TARGET_DIR_SRC = $(notdir $(wildcard $(TARGET_DIR)/*.c)) - -ifeq ($(OPBL),yes) -TARGET_FLAGS := -DOPBL $(TARGET_FLAGS) -ifeq ($(TARGET), $(filter $(TARGET),$(F405_TARGETS))) -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f405_opbl.ld -else ifeq ($(TARGET), $(filter $(TARGET),$(F411_TARGETS))) -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f411_opbl.ld -else ifeq ($(TARGET), $(filter $(TARGET),$(F3_TARGETS))) -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k_opbl.ld -else ifeq ($(TARGET), $(filter $(TARGET),$(F1_TARGETS))) -LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_$(FLASH_SIZE)k_opbl.ld -endif -.DEFAULT_GOAL := binary -else -.DEFAULT_GOAL := hex -endif - -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(ROOT)/lib/main/MAVLink - -INCLUDE_DIRS := $(INCLUDE_DIRS) \ - $(TARGET_DIR) - -VPATH := $(VPATH):$(TARGET_DIR) - -COMMON_SRC = \ - build/build_config.c \ - build/debug.c \ - build/version.c \ - $(TARGET_DIR_SRC) \ - main.c \ - common/encoding.c \ - common/filter.c \ - common/maths.c \ - common/printf.c \ - common/streambuf.c \ - common/typeconversion.c \ - config/config_eeprom.c \ - config/feature.c \ - config/parameter_group.c \ - drivers/adc.c \ - drivers/buf_writer.c \ - drivers/bus_i2c_soft.c \ - drivers/bus_spi.c \ - drivers/bus_spi_soft.c \ - drivers/display.c \ - drivers/exti.c \ - drivers/gyro_sync.c \ - drivers/io.c \ - drivers/light_led.c \ - drivers/resource.c \ - drivers/rx_nrf24l01.c \ - drivers/rx_spi.c \ - drivers/rx_xn297.c \ - drivers/pwm_esc_detect.c \ - drivers/pwm_output.c \ - drivers/pwm_rx.c \ - drivers/rcc.c \ - drivers/serial.c \ - drivers/serial_uart.c \ - drivers/sound_beeper.c \ - drivers/stack_check.c \ - drivers/system.c \ - drivers/timer.c \ - fc/config.c \ - fc/fc_tasks.c \ - fc/fc_msp.c \ - fc/mw.c \ - fc/rc_controls.c \ - fc/rc_curves.c \ - fc/runtime_config.c \ - flight/altitudehold.c \ - flight/failsafe.c \ - flight/imu.c \ - flight/mixer.c \ - flight/pid.c \ - flight/servos.c \ - io/beeper.c \ - io/serial.c \ - io/serial_4way.c \ - io/serial_4way_avrootloader.c \ - io/serial_4way_stk500v2.c \ - io/serial_cli.c \ - io/statusindicator.c \ - msp/msp_serial.c \ - rx/ibus.c \ - rx/jetiexbus.c \ - rx/msp.c \ - rx/nrf24_cx10.c \ - rx/nrf24_inav.c \ - rx/nrf24_h8_3d.c \ - rx/nrf24_syma.c \ - rx/nrf24_v202.c \ - rx/pwm.c \ - rx/rx.c \ - rx/rx_spi.c \ - rx/crsf.c \ - rx/sbus.c \ - rx/spektrum.c \ - rx/sumd.c \ - rx/sumh.c \ - rx/xbus.c \ - scheduler/scheduler.c \ - sensors/acceleration.c \ - sensors/battery.c \ - sensors/boardalignment.c \ - sensors/compass.c \ - sensors/gyro.c \ - sensors/initialisation.c \ - $(CMSIS_SRC) \ - $(DEVICE_STDPERIPH_SRC) - -HIGHEND_SRC = \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ - cms/cms.c \ - cms/cms_menu_blackbox.c \ - cms/cms_menu_builtin.c \ - cms/cms_menu_imu.c \ - cms/cms_menu_ledstrip.c \ - cms/cms_menu_misc.c \ - cms/cms_menu_osd.c \ - cms/cms_menu_vtx.c \ - common/colorconversion.c \ - drivers/display_ug2864hsweg01.c \ - drivers/light_ws2811strip.c \ - drivers/serial_escserial.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c \ - flight/gtune.c \ - flight/navigation.c \ - flight/gps_conversion.c \ - io/dashboard.c \ - io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ - io/gps.c \ - io/ledstrip.c \ - io/osd.c \ - sensors/sonar.c \ - sensors/barometer.c \ - telemetry/telemetry.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/hott.c \ - telemetry/smartport.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - sensors/esc_sensor.c \ - -SPEED_OPTIMISED_SRC := "" -SIZE_OPTIMISED_SRC := "" - -ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) -SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ - common/encoding.c \ - common/filter.c \ - common/maths.c \ - common/typeconversion.c \ - drivers/adc.c \ - drivers/buf_writer.c \ - drivers/bus_i2c_soft.c \ - drivers/bus_spi.c \ - drivers/bus_spi_soft.c \ - drivers/exti.c \ - drivers/gyro_sync.c \ - drivers/io.c \ - drivers/light_led.c \ - drivers/resource.c \ - drivers/rx_nrf24l01.c \ - drivers/rx_spi.c \ - drivers/rx_xn297.c \ - drivers/pwm_output.c \ - drivers/pwm_rx.c \ - drivers/rcc.c \ - drivers/serial.c \ - drivers/serial_uart.c \ - drivers/sound_beeper.c \ - drivers/stack_check.c \ - drivers/system.c \ - drivers/timer.c \ - fc/fc_tasks.c \ - fc/mw.c \ - fc/rc_controls.c \ - fc/rc_curves.c \ - fc/runtime_config.c \ - flight/altitudehold.c \ - flight/failsafe.c \ - flight/imu.c \ - flight/mixer.c \ - flight/pid.c \ - flight/servos.c \ - io/beeper.c \ - io/serial.c \ - io/statusindicator.c \ - rx/ibus.c \ - rx/jetiexbus.c \ - rx/msp.c \ - rx/nrf24_cx10.c \ - rx/nrf24_inav.c \ - rx/nrf24_h8_3d.c \ - rx/nrf24_syma.c \ - rx/nrf24_v202.c \ - rx/pwm.c \ - rx/rx.c \ - rx/rx_spi.c \ - rx/crsf.c \ - rx/sbus.c \ - rx/spektrum.c \ - rx/sumd.c \ - rx/sumh.c \ - rx/xbus.c \ - scheduler/scheduler.c \ - sensors/acceleration.c \ - sensors/boardalignment.c \ - sensors/gyro.c \ - $(CMSIS_SRC) \ - $(DEVICE_STDPERIPH_SRC) \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ - drivers/display_ug2864hsweg01.c \ - drivers/light_ws2811strip.c \ - drivers/serial_softserial.c \ - io/dashboard.c \ - io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ - io/ledstrip.c \ - io/osd.c \ - telemetry/telemetry.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/hott.c \ - telemetry/smartport.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - telemetry/esc_telemetry.c \ - -SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ - drivers/serial_escserial.c \ - io/serial_cli.c \ - io/serial_4way.c \ - io/serial_4way_avrootloader.c \ - io/serial_4way_stk500v2.c \ - msp/msp_serial.c \ - cms/cms.c \ - cms/cms_menu_blackbox.c \ - cms/cms_menu_builtin.c \ - cms/cms_menu_imu.c \ - cms/cms_menu_ledstrip.c \ - cms/cms_menu_misc.c \ - cms/cms_menu_osd.c \ -<<<<<<< HEAD - cms/cms_menu_vtx.c \ - io/vtx_smartaudio.c -======= - cms/cms_menu_vtx.c -endif #F3 ->>>>>>> betaflight/master - -ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) -VCP_SRC = \ - vcpf4/stm32f4xx_it.c \ - vcpf4/usb_bsp.c \ - vcpf4/usbd_desc.c \ - vcpf4/usbd_usr.c \ - vcpf4/usbd_cdc_vcp.c \ - drivers/serial_usb_vcp.c -else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS))) -VCP_SRC = \ - vcp_hal/usbd_desc.c \ - vcp_hal/usbd_conf.c \ - vcp_hal/usbd_cdc_interface.c \ - drivers/serial_usb_vcp_hal.c -else -VCP_SRC = \ - vcp/hw_config.c \ - vcp/stm32_it.c \ - vcp/usb_desc.c \ - vcp/usb_endp.c \ - vcp/usb_istr.c \ - vcp/usb_prop.c \ - vcp/usb_pwr.c \ - drivers/serial_usb_vcp.c \ - drivers/usb_io.c -endif - -STM32F10x_COMMON_SRC = \ - startup_stm32f10x_md_gcc.S \ - drivers/adc_stm32f10x.c \ - drivers/bus_i2c_stm32f10x.c \ - drivers/dma.c \ - drivers/gpio_stm32f10x.c \ - drivers/inverter.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/serial_uart_stm32f10x.c \ - drivers/system_stm32f10x.c \ - drivers/timer_stm32f10x.c - -STM32F30x_COMMON_SRC = \ - startup_stm32f30x_md_gcc.S \ - target/system_stm32f30x.c \ - drivers/adc_stm32f30x.c \ - drivers/bus_i2c_stm32f30x.c \ - drivers/dma.c \ - drivers/gpio_stm32f30x.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/pwm_output_stm32f3xx.c \ - drivers/serial_uart_stm32f30x.c \ - drivers/system_stm32f30x.c \ - drivers/timer_stm32f30x.c - -STM32F4xx_COMMON_SRC = \ - startup_stm32f40xx.s \ - target/system_stm32f4xx.c \ - drivers/accgyro_mpu.c \ - drivers/adc_stm32f4xx.c \ - drivers/bus_i2c_stm32f10x.c \ - drivers/dma_stm32f4xx.c \ - drivers/gpio_stm32f4xx.c \ - drivers/inverter.c \ - drivers/light_ws2811strip_stm32f4xx.c \ - drivers/pwm_output_stm32f4xx.c \ - drivers/serial_uart_stm32f4xx.c \ - drivers/system_stm32f4xx.c \ - drivers/timer_stm32f4xx.c - -STM32F7xx_COMMON_SRC = \ - startup_stm32f745xx.s \ - target/system_stm32f7xx.c \ - drivers/accgyro_mpu.c \ - drivers/adc_stm32f7xx.c \ - drivers/bus_i2c_hal.c \ - drivers/dma_stm32f7xx.c \ - drivers/gpio_stm32f7xx.c \ - drivers/inverter.c \ - drivers/bus_spi_hal.c \ - drivers/pwm_output_stm32f7xx.c \ - drivers/timer_hal.c \ - drivers/timer_stm32f7xx.c \ - drivers/pwm_output_hal.c \ - drivers/system_stm32f7xx.c \ - drivers/serial_uart_stm32f7xx.c \ - drivers/serial_uart_hal.c - -F7EXCLUDES = drivers/bus_spi.c \ - drivers/bus_i2c.c \ - drivers/timer.c \ - drivers/pwm_output.c \ - drivers/serial_uart.c - -# check if target.mk supplied -ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) -TARGET_SRC := $(STM32F4xx_COMMON_SRC) $(TARGET_SRC) -else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS))) -TARGET_SRC := $(STM32F7xx_COMMON_SRC) $(TARGET_SRC) -else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) -TARGET_SRC := $(STM32F30x_COMMON_SRC) $(TARGET_SRC) -else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) -TARGET_SRC := $(STM32F10x_COMMON_SRC) $(TARGET_SRC) -endif - -ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) -TARGET_SRC += \ - drivers/flash_m25p16.c \ - io/flashfs.c -endif - -ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS) $(F4_TARGETS) $(F3_TARGETS))) -TARGET_SRC += $(HIGHEND_SRC) -else ifneq ($(filter HIGHEND,$(FEATURES)),) -TARGET_SRC += $(HIGHEND_SRC) -endif - -TARGET_SRC += $(COMMON_SRC) -#excludes -ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS))) -TARGET_SRC := $(filter-out ${F7EXCLUDES}, $(TARGET_SRC)) -endif - -ifneq ($(filter SDCARD,$(FEATURES)),) -TARGET_SRC += \ - drivers/sdcard.c \ - drivers/sdcard_standard.c \ - io/asyncfatfs/asyncfatfs.c \ - io/asyncfatfs/fat_standard.c -endif - -ifneq ($(filter VCP,$(FEATURES)),) -TARGET_SRC += $(VCP_SRC) -endif -# end target specific make file checks - - -# Search path and source files for the ST stdperiph library -VPATH := $(VPATH):$(STDPERIPH_DIR)/src - -############################################################################### -# Things that might need changing to use different tools -# - -# Find out if ccache is installed on the system -CCACHE := ccache -RESULT = $(shell (which $(CCACHE) > /dev/null 2>&1; echo $$?) ) -ifneq ($(RESULT),0) -CCACHE := -endif - -# Tool names -CROSS_CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc -CROSS_CXX := $(CCACHE) $(ARM_SDK_PREFIX)g++ -OBJCOPY := $(ARM_SDK_PREFIX)objcopy -SIZE := $(ARM_SDK_PREFIX)size - -# -# Tool options. -# - -ifneq ($(DEBUG),GDB) -OPTIMISATION_BASE := -flto -fuse-linker-plugin -ffast-math -OPTIMISE_SPEED := "" -OPTIMISE_SIZE := "" - -ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) -OPTIMISE_DEFAULT := -Os - -LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT) - -else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) -OPTIMISE_DEFAULT := -O2 -OPTIMISE_SPEED := -Ofast -OPTIMISE_SIZE := -Os - -LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) - -else -OPTIMISE_DEFAULT := -Ofast - -LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT) - -endif #TARGETS - -CC_DEFAULT_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT) -CC_SPEED_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) -CC_SIZE_OPTIMISATION := $(OPTIMISATION_BASE) $(OPTIMISE_SIZE) - -else #DEBUG -OPTIMISE_DEFAULT := -O0 - -CC_DEBUG_OPTIMISATION := $(OPTIMISE_DEFAULT) - -LTO_FLAGS := $(OPTIMISE_DEFAULT) -endif #DEBUG - -DEBUG_FLAGS = -ggdb3 -DDEBUG - -CFLAGS += $(ARCH_FLAGS) \ - $(addprefix -D,$(OPTIONS)) \ - $(addprefix -I,$(INCLUDE_DIRS)) \ - $(DEBUG_FLAGS) \ - -std=gnu99 \ - -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ - -ffunction-sections \ - -fdata-sections \ - -pedantic \ - $(DEVICE_FLAGS) \ - -DUSE_STDPERIPH_DRIVER \ - -D$(TARGET) \ - $(TARGET_FLAGS) \ - -D'__FORKNAME__="$(FORKNAME)"' \ - -D'__TARGET__="$(TARGET)"' \ - -D'__REVISION__="$(REVISION)"' \ - -save-temps=obj \ - -MMD -MP - -ASFLAGS = $(ARCH_FLAGS) \ - -x assembler-with-cpp \ - $(addprefix -I,$(INCLUDE_DIRS)) \ - -MMD -MP - -LDFLAGS = -lm \ - -nostartfiles \ - --specs=nano.specs \ - -lc \ - -lnosys \ - $(ARCH_FLAGS) \ - $(LTO_FLAGS) \ - $(DEBUG_FLAGS) \ - -static \ - -Wl,-gc-sections,-Map,$(TARGET_MAP) \ - -Wl,-L$(LINKER_DIR) \ - -Wl,--cref \ - -Wl,--no-wchar-size-warning \ - -T$(LD_SCRIPT) - -############################################################################### -# No user-serviceable parts below -############################################################################### - -CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ - --std=c99 --inline-suppr --quiet --force \ - $(addprefix -I,$(INCLUDE_DIRS)) \ - -I/usr/include -I/usr/include/linux - -# -# Things we will build -# -TARGET_BIN = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).bin -TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).hex -TARGET_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf -TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(TARGET_SRC)))) -TARGET_DEPS = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(TARGET_SRC)))) -TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map - - -CLEAN_ARTIFACTS := $(TARGET_BIN) -CLEAN_ARTIFACTS += $(TARGET_HEX) -CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) - -# Make sure build date and revision is updated on every incremental build -$(OBJECT_DIR)/$(TARGET)/build/version.o : $(TARGET_SRC) - -# List of buildable ELF files and their object dependencies. -# It would be nice to compute these lists, but that seems to be just beyond make. - -$(TARGET_HEX): $(TARGET_ELF) - $(V0) $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@ - -$(TARGET_BIN): $(TARGET_ELF) - $(V0) $(OBJCOPY) -O binary $< $@ - -$(TARGET_ELF): $(TARGET_OBJS) - $(V1) echo Linking $(TARGET) - $(V1) $(CROSS_CC) -o $@ $^ $(LDFLAGS) - $(V0) $(SIZE) $(TARGET_ELF) - -# Compile -ifneq ($(DEBUG),GDB) -$(OBJECT_DIR)/$(TARGET)/%.o: %.c - $(V1) mkdir -p $(dir $@) - $(V1) $(if $(findstring $(subst ./src/main/,,$<),$(SPEED_OPTIMISED_SRC)), \ - echo "%% (speed optimised) $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SPEED_OPTIMISATION) $<, \ - $(if $(findstring $(subst ./src/main/,,$<),$(SIZE_OPTIMISED_SRC)), \ - echo "%% (size optimised) $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_SIZE_OPTIMISATION) $<, \ - echo "%% $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_DEFAULT_OPTIMISATION) $<)) -else -$(OBJECT_DIR)/$(TARGET)/%.o: %.c - $(V1) mkdir -p $(dir $@) - $(V1) echo "%% $(notdir $<)" "$(STDOUT)" && \ - $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_DEBUG_OPTIMISATION) $< -endif - -# Assemble -$(OBJECT_DIR)/$(TARGET)/%.o: %.s - $(V1) mkdir -p $(dir $@) - $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< - -$(OBJECT_DIR)/$(TARGET)/%.o: %.S - $(V1) mkdir -p $(dir $@) - $(V1) echo "%% $(notdir $<)" "$(STDOUT)" - $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< - -## sample : Build all sample (travis) targets -sample: $(SAMPLE_TARGETS) - -## all : Build all valid targets -all: $(VALID_TARGETS) - -$(VALID_TARGETS): - $(V0) echo "" && \ - echo "Building $@" && \ - $(MAKE) binary hex TARGET=$@ && \ - echo "Building $@ succeeded." - - - -CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) ) -TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) - -## clean : clean up temporary / machine-generated files -clean: - $(V0) echo "Cleaning $(TARGET)" - $(V0) rm -f $(CLEAN_ARTIFACTS) - $(V0) rm -rf $(OBJECT_DIR)/$(TARGET) - $(V0) echo "Cleaning $(TARGET) succeeded." - -## clean_test : clean up temporary / machine-generated files (tests) -clean_test: - $(V0) cd src/test && $(MAKE) clean || true - -## clean_ : clean up one specific target -$(CLEAN_TARGETS) : - $(V0) $(MAKE) -j TARGET=$(subst clean_,,$@) clean - -## _clean : clean up one specific target (alias for above) -$(TARGETS_CLEAN) : - $(V0) $(MAKE) -j TARGET=$(subst _clean,,$@) clean - -## clean_all : clean all valid targets -clean_all:$(CLEAN_TARGETS) - -## all_clean : clean all valid targets (alias for above) -all_clean:$(TARGETS_CLEAN) - - -flash_$(TARGET): $(TARGET_HEX) - $(V0) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon - $(V0) echo -n 'R' >$(SERIAL_DEVICE) - $(V0) stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) - -## flash : flash firmware (.hex) onto flight controller -flash: flash_$(TARGET) - -st-flash_$(TARGET): $(TARGET_BIN) - $(V0) st-flash --reset write $< 0x08000000 - -## st-flash : flash firmware (.bin) onto flight controller -st-flash: st-flash_$(TARGET) - -binary: - $(V0) $(MAKE) -j $(TARGET_BIN) - -hex: - $(V0) $(MAKE) -j $(TARGET_HEX) - -unbrick_$(TARGET): $(TARGET_HEX) - $(V0) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon - $(V0) stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) - -## unbrick : unbrick flight controller -unbrick: unbrick_$(TARGET) - -## cppcheck : run static analysis on C source code -cppcheck: $(CSOURCES) - $(V0) $(CPPCHECK) - -cppcheck-result.xml: $(CSOURCES) - $(V0) $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml - -# mkdirs -$(DL_DIR): - mkdir -p $@ - -$(TOOLS_DIR): - mkdir -p $@ - -$(BUILD_DIR): - mkdir -p $@ - -## help : print this help message and exit -help: Makefile make/tools.mk - $(V0) @echo "" - $(V0) @echo "Makefile for the $(FORKNAME) firmware" - $(V0) @echo "" - $(V0) @echo "Usage:" - $(V0) @echo " make [V=] [TARGET=] [OPTIONS=\"\"]" - $(V0) @echo "Or:" - $(V0) @echo " make [V=] [OPTIONS=\"\"]" - $(V0) @echo "" - $(V0) @echo "Valid TARGET values are: $(VALID_TARGETS)" - $(V0) @echo "" - $(V0) @sed -n 's/^## //p' $? - -## targets : print a list of all valid target platforms (for consumption by scripts) -targets: - $(V0) @echo "Valid targets: $(VALID_TARGETS)" - $(V0) @echo "Target: $(TARGET)" - $(V0) @echo "Base target: $(BASE_TARGET)" - -## test : run the cleanflight test suite -## junittest : run the cleanflight test suite, producing Junit XML result files. -test junittest: - $(V0) cd src/test && $(MAKE) $@ - -# rebuild everything when makefile changes -$(TARGET_OBJS) : Makefile - -# include auto-generated dependencies --include $(TARGET_DEPS) From 9a4aaaab4cac5fe8c8cca2327348d53b04a9ac20 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Dec 2016 00:41:21 +0900 Subject: [PATCH 36/40] Delete bogus files --- src/main/io/cms_blackbox.c | 77 -------------- src/main/io/cms_blackbox.h | 4 - src/main/io/cms_imu.c | 206 ------------------------------------- src/main/io/cms_imu.h | 13 --- src/main/io/cms_ledstrip.c | 107 ------------------- src/main/io/cms_ledstrip.h | 6 -- src/main/io/cms_osd.h | 2 - src/main/io/cms_vtx.c | 96 ----------------- src/main/io/cms_vtx.h | 7 -- 9 files changed, 518 deletions(-) delete mode 100644 src/main/io/cms_blackbox.c delete mode 100644 src/main/io/cms_blackbox.h delete mode 100644 src/main/io/cms_imu.c delete mode 100644 src/main/io/cms_imu.h delete mode 100644 src/main/io/cms_ledstrip.c delete mode 100644 src/main/io/cms_ledstrip.h delete mode 100644 src/main/io/cms_osd.h delete mode 100644 src/main/io/cms_vtx.c delete mode 100644 src/main/io/cms_vtx.h diff --git a/src/main/io/cms_blackbox.c b/src/main/io/cms_blackbox.c deleted file mode 100644 index 3f118f87ee..0000000000 --- a/src/main/io/cms_blackbox.c +++ /dev/null @@ -1,77 +0,0 @@ -// -// CMS things for blackbox and flashfs. -// Should be part of blackbox.c (or new blackbox/blackbox_cms.c) and io/flashfs.c -// -#include -#include -#include -#include - -#include "platform.h" - -#include "build/version.h" - -#ifdef CMS - -#include "drivers/system.h" - -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_blackbox.h" - -#include "io/flashfs.h" - -#ifdef USE_FLASHFS -long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr) -{ - UNUSED(ptr); - - cmsScreenClear(pDisplay); - cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH..."); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing? - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - - cmsScreenClear(pDisplay); - cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? - - return 0; -} -#endif // USE_FLASHFS - -uint8_t cmsx_FeatureBlackbox; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry cmsx_menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -void cmsx_Blackbox_FeatureRead(void) -{ - cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; -} - -void cmsx_Blackbox_FeatureWriteback(void) -{ - if (cmsx_FeatureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); -} -#endif diff --git a/src/main/io/cms_blackbox.h b/src/main/io/cms_blackbox.h deleted file mode 100644 index 6e7d02eff2..0000000000 --- a/src/main/io/cms_blackbox.h +++ /dev/null @@ -1,4 +0,0 @@ -extern OSD_Entry cmsx_menuBlackbox[]; - -void cmsx_Blackbox_FeatureRead(void); -void cmsx_Blackbox_FeatureWriteback(void); diff --git a/src/main/io/cms_imu.c b/src/main/io/cms_imu.c deleted file mode 100644 index 9428d993d6..0000000000 --- a/src/main/io/cms_imu.c +++ /dev/null @@ -1,206 +0,0 @@ - -// Menu contents for PID, RATES, RC preview, misc -// Should be part of the relevant .c file. - -#include -#include -#include -#include - -#include "platform.h" - -#include "build/version.h" - -#ifdef CMS - -#include "drivers/system.h" - -//#include "common/typeconversion.h" - -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_imu.h" - -#include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" - -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -uint8_t tempPid[4][3]; - -static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; -static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; -static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; - -static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; -static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; -static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; - -static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; -static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; -static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; - -void cmsx_PidRead(void) -{ - uint8_t i; - - for (i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i]; - } - tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]; -} - -void cmsx_PidWriteback(void) -{ - uint8_t i; - - for (i = 0; i < 3; i++) { - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2]; - } - - masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2]; -} - -OSD_Entry cmsx_menuPid[] = -{ - {"--- PID ---", OME_Label, NULL, NULL, 0}, - {"ROLL P", OME_UINT8, NULL, &entryRollP, 0}, - {"ROLL I", OME_UINT8, NULL, &entryRollI, 0}, - {"ROLL D", OME_UINT8, NULL, &entryRollD, 0}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP, 0}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI, 0}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD, 0}, - - {"YAW P", OME_UINT8, NULL, &entryYawP, 0}, - {"YAW I", OME_UINT8, NULL, &entryYawI, 0}, - {"YAW D", OME_UINT8, NULL, &entryYawD, 0}, - - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Rate & Expo -// -controlRateConfig_t rateProfile; - -static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; -static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; -static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; -static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcYawRate = {&rateProfile.rcYawRate8, 0, 200, 1, 10}; -static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; -static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; -static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; -static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; -static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; - -void cmsx_RateExpoRead() -{ - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); -} - -void cmsx_RateExpoWriteback() -{ - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); -} - -OSD_Entry cmsx_menuRateExpo[] = -{ - {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0}, - {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0}, - {"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0}, - {"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0}, - {"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0}, - {"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0}, - {"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0}, - {"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0}, - {"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0}, - {"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// RC preview -// -static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; -static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; -static OSD_INT16_t entryRcThr = {&rcData[THROTTLE], 1, 2500, 0}; -static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; -static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; -static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; -static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; -static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; - -OSD_Entry cmsx_menuRc[] = -{ - {"--- RC PREV ---", OME_Label, NULL, NULL, 0}, - {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0}, - {"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0}, - {"THR", OME_Poll_INT16, NULL, &entryRcThr, 0}, - {"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0}, - {"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0}, - {"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0}, - {"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0}, - {"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -// -// Misc -// -OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10}; -OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; -OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; -OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; -OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; -OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; - -OSD_Entry menuImuMisc[]= -{ - {"--- MISC ---", OME_Label, NULL, NULL, 0}, - {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0}, - {"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0}, - {"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0}, - {"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -OSD_Entry cmsx_menuImu[] = -{ - {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0}, - {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0}, - {"PID", OME_Submenu, cmsMenuChange, cmsx_menuPid, 0}, - {"RATE&RXPO", OME_Submenu, cmsMenuChange, cmsx_menuRateExpo, 0}, - {"RC PREV", OME_Submenu, cmsMenuChange, cmsx_menuRc, 0}, - {"MISC", OME_Submenu, cmsMenuChange, menuImuMisc, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; -#endif // CMS diff --git a/src/main/io/cms_imu.h b/src/main/io/cms_imu.h deleted file mode 100644 index b78c6ddb4b..0000000000 --- a/src/main/io/cms_imu.h +++ /dev/null @@ -1,13 +0,0 @@ -extern OSD_Entry cmsx_menuImu[]; - -// All of below should be gone. - -extern OSD_Entry cmsx_menuPid[]; -extern OSD_Entry cmsx_menuRc[]; -extern OSD_Entry cmsx_menuRateExpo[]; - -void cmsx_PidRead(void); -void cmsx_PidWriteback(void); -void cmsx_RateExpoRead(void); -void cmsx_RateExpoWriteback(void); - diff --git a/src/main/io/cms_ledstrip.c b/src/main/io/cms_ledstrip.c deleted file mode 100644 index 63e15d74e6..0000000000 --- a/src/main/io/cms_ledstrip.c +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include -#include - -#include "platform.h" - -#include "build/version.h" - -#ifdef CMS - -#include "drivers/system.h" - -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_blackbox.h" - -#ifdef LED_STRIP - -//local variable to keep color value -uint8_t ledColor; - -static const char * const LED_COLOR_NAMES[] = { - "BLACK ", - "WHITE ", - "RED ", - "ORANGE ", - "YELLOW ", - "LIME GRN", - "GREEN ", - "MINT GRN", - "CYAN ", - "LT BLUE ", - "BLUE ", - "DK VIOLT", - "MAGENTA ", - "DEEP PNK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -void cmsx_GetLedColor(void) -{ - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - int fn = ledGetFunction(ledConfig); - - if (fn == LED_FUNCTION_COLOR) { - ledColor = ledGetColor(ledConfig); - break; - } - } -} - -//udate all leds with flag color -static long applyLedColor(displayPort_t *pDisplay, void *ptr) -{ - UNUSED(ptr); - UNUSED(pDisplay); // Arrgh - - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) - *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); - } - - return 0; -} - -uint8_t cmsx_FeatureLedstrip; - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -OSD_Entry cmsx_menuLedstrip[] = -{ - {"--- LED STRIP ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0}, - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -void cmsx_Ledstrip_FeatureRead(void) -{ - cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0; -} - -void cmsx_Ledstrip_FeatureWriteback(void) -{ - if (cmsx_FeatureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); -} - -void cmsx_Ledstrip_ConfigRead(void) -{ - cmsx_GetLedColor(); -} - -#endif // LED_STRIP -#endif // CMS diff --git a/src/main/io/cms_ledstrip.h b/src/main/io/cms_ledstrip.h deleted file mode 100644 index 1ce1d5009f..0000000000 --- a/src/main/io/cms_ledstrip.h +++ /dev/null @@ -1,6 +0,0 @@ -extern OSD_Entry cmsx_menuLedstrip[]; - -void cmsx_Ledstrip_FeatureRead(void); -void cmsx_Ledstrip_FeatureWriteback(void); - -void cmsx_Ledstrip_ConfigRead(void); diff --git a/src/main/io/cms_osd.h b/src/main/io/cms_osd.h deleted file mode 100644 index 5c93a65215..0000000000 --- a/src/main/io/cms_osd.h +++ /dev/null @@ -1,2 +0,0 @@ -extern OSD_Entry cmsx_menuAlarms[]; -extern OSD_Entry cmsx_menuOsdLayout[]; diff --git a/src/main/io/cms_vtx.c b/src/main/io/cms_vtx.c deleted file mode 100644 index f133f4e7f1..0000000000 --- a/src/main/io/cms_vtx.c +++ /dev/null @@ -1,96 +0,0 @@ -#include -#include -#include - -#include "platform.h" - -#include "build/version.h" - -#include "io/cms.h" -#include "io/cms_types.h" -#include "io/cms_vtx.h" - -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#ifdef CMS - -#if defined(VTX) || defined(USE_RTC6705) - -uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; - -static const char * const vtxBandNames[] = { - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; - -OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; -OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; - -#ifdef VTX -OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; -OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; -#endif // VTX - -OSD_Entry cmsx_menuVtx[] = -{ - {"--- VTX ---", OME_Label, NULL, NULL, 0}, - {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL, 0}, - {NULL, OME_END, NULL, NULL, 0} -}; - -void cmsx_Vtx_FeatureRead(void) -{ - cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; -} - -void cmsx_Vtx_FeatureWriteback(void) -{ - if (cmsx_featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); -} - -void cmsx_Vtx_ConfigRead(void) -{ -#ifdef VTX - cmsx_vtxBand = masterConfig.vtxBand; - cmsx_vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX - -#ifdef USE_RTC6705 - cmsx_vtxBand = masterConfig.vtx_channel / 8; - cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 -} - -void cmsx_Vtx_ConfigWriteback(void) -{ -#ifdef VTX - masterConfig.vtxBand = cmsx_vtxBand; - masterConfig.vtx_channel = cmsx_vtxChannel - 1; -#endif // VTX - -#ifdef USE_RTC6705 - masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; -#endif // USE_RTC6705 -} - -#endif // VTX || USE_RTC6705 - -#endif // CMS diff --git a/src/main/io/cms_vtx.h b/src/main/io/cms_vtx.h deleted file mode 100644 index 412a9e4f73..0000000000 --- a/src/main/io/cms_vtx.h +++ /dev/null @@ -1,7 +0,0 @@ -extern OSD_Entry cmsx_menuVtx[]; - -void cmsx_Vtx_FeatureRead(void); -void cmsx_Vtx_FeatureWriteback(void); - -void cmsx_Vtx_ConfigRead(void); -void cmsx_Vtx_ConfigWriteback(void); From 9d6fb10da45a587f4a6a2d6862cf142cb9de574e Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Dec 2016 00:41:59 +0900 Subject: [PATCH 37/40] Add copyright notice --- src/main/io/vtx_smartaudio.c | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index ede9e58ca7..2fbe947435 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1,3 +1,22 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/* Created by jflyper */ + #include #include #include @@ -11,6 +30,7 @@ #include "string.h" #include "common/printf.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/serial.h" #include "io/serial.h" @@ -20,11 +40,6 @@ #include "fc/runtime_config.h" #include "flight/pid.h" -#if 0 -#include "config/config.h" -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#endif #include "config/config_master.h" #include "build/build_config.h" From e3826c17573990a3e9eca0d9aedc004f6dc6bcf6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Dec 2016 00:42:36 +0900 Subject: [PATCH 38/40] SmartAudio support to be included for >128K targets --- src/main/target/OMNIBUS/target.mk | 3 +-- src/main/target/OMNIBUSF4/target.mk | 4 +--- src/main/target/common.h | 1 + 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index dd7c8d7b5a..5878246a44 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -13,5 +13,4 @@ TARGET_SRC = \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c \ - io/vtx_smartaudio.c + io/transponder_ir.c diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 37ad7af0c6..db1c65b899 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -7,6 +7,4 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/barometer_spi_bmp280.c \ drivers/compass_hmc5883l.c \ - drivers/max7456.c \ - io/vtx_smartaudio.c - + drivers/max7456.c diff --git a/src/main/target/common.h b/src/main/target/common.h index ed8d709428..4f66ce3cb4 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -101,6 +101,7 @@ #define TELEMETRY_MAVLINK #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS +#define VTX_SMARTAUDIO #else #define SKIP_CLI_COMMAND_HELP #endif From c8a8ee065502cf7fb18a41391ce84daad0122ef6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Dec 2016 01:18:25 +0900 Subject: [PATCH 39/40] Delete VTX_SMARTAUDIO from config_master.h SmartAudio remembers everything by itself. --- src/main/config/config_master.h | 2 +- src/main/io/serial_cli.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0dd0a4a348..cbe9a26c05 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -198,7 +198,7 @@ typedef struct master_s { uint8_t transponderData[6]; #endif -#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +#if defined(USE_RTC6705) uint8_t vtx_channel; uint8_t vtx_power; #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6dd072e4e2..b9130106df 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -963,7 +963,7 @@ const clivalue_t valueTable[] = { #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif -#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +#if defined(USE_RTC6705) { "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 From 352d38500a4f3fe7e671e774d29d5e0835769219 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 31 Dec 2016 14:30:45 +0900 Subject: [PATCH 40/40] Created target/common_post.h to touch up configuration created by common.h and target.h --- src/main/platform.h | 2 +- src/main/target/OMNIBUS/target.h | 5 ----- src/main/target/OMNIBUSF4/target.h | 5 ----- src/main/target/common.h | 1 + src/main/target/common_post.h | 26 ++++++++++++++++++++++++++ 5 files changed, 28 insertions(+), 11 deletions(-) create mode 100644 src/main/target/common_post.h diff --git a/src/main/platform.h b/src/main/platform.h index 0f6439cbcb..9e2c68e0de 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -73,4 +73,4 @@ #include "target/common.h" #include "target.h" - +#include "target/common_post.h" diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index a4f5d57560..e0407debbb 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -106,11 +106,6 @@ //#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 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0789986057..409854ae2d 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -131,11 +131,6 @@ #define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 -// VTX monitor task -#define VTX_CONTROL -// VTX device type -#define VTX_SMARTAUDIO - #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/common.h b/src/main/target/common.h index 4f66ce3cb4..66d17911a4 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -101,6 +101,7 @@ #define TELEMETRY_MAVLINK #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS +#define VTX_CONTROL #define VTX_SMARTAUDIO #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h new file mode 100644 index 0000000000..193124c071 --- /dev/null +++ b/src/main/target/common_post.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +// Touch up configuration + +#pragma once + +// Targets with built-in vtx do not need external vtx +#if defined(VTX) || defined(USE_RTC6705) +# undef VTX_CONTROL +# undef VTX_SMARTAUDIO +#endif