diff --git a/Makefile b/Makefile index af64ce945..fc5be64c6 100644 --- a/Makefile +++ b/Makefile @@ -614,7 +614,9 @@ HIGHEND_SRC = \ telemetry/mavlink.c \ telemetry/ibus.c \ sensors/esc_sensor.c \ - io/vtx_smartaudio.c + io/vtx_common.c \ + io/vtx_smartaudio.c \ + io/vtx_tramp.c SPEED_OPTIMISED_SRC := "" SIZE_OPTIMISED_SRC := "" @@ -721,7 +723,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ cms/cms_menu_misc.c \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ - io/vtx_smartaudio.c + io/vtx_smartaudio.c \ + io/vtx_tramp.c endif #F3 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index a0dc6b19e..182765526 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -48,6 +48,7 @@ // User supplied menus #include "io/vtx_smartaudio_cms.h" +#include "io/vtx_tramp.h" // Info @@ -96,9 +97,14 @@ static OSD_Entry menuFeaturesEntries[] = #if defined(VTX) || defined(USE_RTC6705) {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0}, #endif // VTX || USE_RTC6705 +#if defined(VTX_CONTROL) #if defined(VTX_SMARTAUDIO) - {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, + {"VTX SA", OME_Submenu, cmsMenuChange, &cmsx_menuVtxSmartAudio, 0}, #endif +#if defined(VTX_TRAMP) + {"VTX TR", OME_Submenu, cmsMenuChange, &cmsx_menuVtxTramp, 0}, +#endif +#endif // VTX_CONTROL #ifdef LED_STRIP {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0}, #endif // LED_STRIP diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 1cd9806ef..2d8b88f0a 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -81,6 +81,9 @@ OSD_Entry menuOsdActiveElemsEntries[] = #endif // GPS {"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0}, {"POWER", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_POWER], 0}, + {"ROLL PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ROLL_PIDS], 0}, + {"PITCH PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_PITCH_PIDS], 0}, + {"YAW PID", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_YAW_PIDS], 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 3a4b96d19..b5774c625 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -20,31 +20,98 @@ #include "platform.h" -#ifdef INVERTER - #include "io.h" #include "io_impl.h" #include "inverter.h" -/* - TODO: move this to support multiple inverters on different UARTs etc - possibly move to put it in the UART driver itself. -*/ -static IO_t pin = IO_NONE; - -void initInverter(void) -{ - pin = IOGetByTag(IO_TAG(INVERTER)); - IOInit(pin, OWNER_INVERTER, 1); - IOConfigGPIO(pin, IOCFG_OUT_PP); - - inverterSet(false); -} - -void inverterSet(bool on) +#ifdef USE_INVERTER +static void inverterSet(IO_t pin, bool on) { IOWrite(pin, on); } +static void initInverter(ioTag_t ioTag) +{ + IO_t pin = IOGetByTag(ioTag); + IOInit(pin, OWNER_INVERTER, 1); + IOConfigGPIO(pin, IOCFG_OUT_PP); + + inverterSet(pin, false); +} #endif + +void initInverters(void) +{ +#ifdef INVERTER_PIN_USART1 + initInverter(IO_TAG(INVERTER_PIN_USART1)); +#endif + +#ifdef INVERTER_PIN_USART2 + initInverter(IO_TAG(INVERTER_PIN_USART2)); +#endif + +#ifdef INVERTER_PIN_USART3 + initInverter(IO_TAG(INVERTER_PIN_USART3)); +#endif + +#ifdef INVERTER_PIN_USART4 + initInverter(IO_TAG(INVERTER_PIN_USART4)); +#endif + +#ifdef INVERTER_PIN_USART5 + initInverter(IO_TAG(INVERTER_PIN_USART5)); +#endif + +#ifdef INVERTER_PIN_USART6 + initInverter(IO_TAG(INVERTER_PIN_USART6)); +#endif +} + +void enableInverter(USART_TypeDef *USARTx, bool on) +{ +#ifdef USE_INVERTER + IO_t pin = IO_NONE; + +#ifdef INVERTER_PIN_USART1 + if (USARTx == USART1) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART1)); + } +#endif + +#ifdef INVERTER_PIN_USART2 + if (USARTx == USART2) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART2)); + } +#endif + +#ifdef INVERTER_PIN_USART3 + if (USARTx == USART3) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART3)); + } +#endif + +#ifdef INVERTER_PIN_USART4 + if (USARTx == USART4) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART4)); + } +#endif + +#ifdef INVERTER_PIN_USART5 + if (USARTx == USART5) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART5)); + } +#endif + +#ifdef INVERTER_PIN_USART6 + if (USARTx == USART6) { + pin = IOGetByTag(IO_TAG(INVERTER_PIN_USART6)); + } +#endif + + inverterSet(pin, on); +#else + UNUSED(USARTx); + UNUSED(on); +#endif +} diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h index 4b9c3fe27..f7d747d5a 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/inverter.h @@ -17,14 +17,10 @@ #pragma once -#ifdef INVERTER -void inverterSet(bool on); -#define INVERTER_OFF inverterSet(false) -#define INVERTER_ON inverterSet(true) -#else -#define INVERTER_OFF do {} while(0) -#define INVERTER_ON do {} while(0) +#if defined(INVERTER_PIN_USART1) || defined(INVERTER_PIN_USART2) || defined(INVERTER_PIN_USART3) || defined(INVERTER_PIN_USART4) || defined(INVERTER_PIN_USART5) || defined(INVERTER_PIN_USART6) +#define USE_INVERTER #endif -void initInverter(void); +void initInverters(void); +void enableInverter(USART_TypeDef *USARTx, bool on); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 9b083349d..9dd53046d 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -36,15 +36,15 @@ #include "serial_uart_impl.h" static void usartConfigurePinInversion(uartPort_t *uartPort) { -#if !defined(INVERTER) && !defined(STM32F303xC) +#if !defined(USE_INVERTER) && !defined(STM32F303xC) UNUSED(uartPort); #else bool inverted = uartPort->port.options & SERIAL_INVERTED; -#ifdef INVERTER - if (inverted && uartPort->USARTx == INVERTER_USART) { +#ifdef USE_INVERTER + if (inverted) { // Enable hardware inverter if available. - INVERTER_ON; + enableInverter(uartPort->USARTx, true); } #endif diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 9df19cfe6..6014553cb 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -204,8 +204,8 @@ serialPort_t *usbVcpOpen(void) #else Set_System(); Set_USBClock(); - USB_Interrupts_Config(); USB_Init(); + USB_Interrupts_Config(); #endif s = &vcpPort; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 83cb6719e..4439f07c8 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2765,9 +2765,8 @@ static void cliReboot(void) static void cliDfu(char *cmdLine) { UNUSED(cmdLine); -#ifndef CLI_MINIMAL_VERBOSITY - cliPrint("\r\nRestarting in DFU mode"); -#endif + + cliPrintHashLine("restarting in DFU mode"); cliRebootEx(true); } @@ -2775,9 +2774,7 @@ static void cliExit(char *cmdline) { UNUSED(cmdline); -#ifndef CLI_MINIMAL_VERBOSITY - cliPrint("\r\nLeaving CLI mode, unsaved changes lost.\r\n"); -#endif + cliPrintHashLine("leaving CLI mode, unsaved changes lost"); bufWriterFlush(cliWriter); *cliBuffer = '\0'; @@ -3043,7 +3040,7 @@ static void cliSave(char *cmdline) { UNUSED(cmdline); - cliPrint("Saving"); + cliPrintHashLine("saving"); writeEEPROM(); cliReboot(); } @@ -3052,7 +3049,7 @@ static void cliDefaults(char *cmdline) { UNUSED(cmdline); - cliPrint("Resetting to defaults"); + cliPrintHashLine("resetting to defaults"); resetEEPROM(); cliReboot(); } @@ -3561,13 +3558,12 @@ static void printConfig(char *cmdline, bool doDiff) cliPrintHashLine("version"); cliVersion(NULL); -#ifndef CLI_MINIMAL_VERBOSITY if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { - cliPrintHashLine("reset configuration to default settings\r\ndefaults"); + cliPrintHashLine("reset configuration to default settings"); + cliPrint("defaults\r\n"); } cliPrintHashLine("name"); -#endif printName(dumpMask); #ifdef USE_RESOURCE_MGMT @@ -3653,19 +3649,16 @@ static void printConfig(char *cmdline, bool doDiff) } changeControlRateProfile(currentRateIndex); -#ifndef CLI_MINIMAL_VERBOSITY cliPrintHashLine("restore original rateprofile selection"); cliRateProfile(""); -#endif } changeProfile(activeProfile); -#ifndef CLI_MINIMAL_VERBOSITY cliPrintHashLine("restore original profile selection"); cliProfile(""); - cliPrintHashLine("save configuration\r\nsave"); -#endif + cliPrintHashLine("save configuration"); + cliPrint("save"); } else { cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0abd62285..dd22a3b08 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -209,9 +209,10 @@ void processRcCommand(void) static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t factor, rcInterpolationFactor; static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 1; + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; uint16_t rxRefreshRate; bool readyToCalculateRate = false; + uint8_t readyToCalculateRateAxisCnt = 0; if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); @@ -241,7 +242,7 @@ void processRcCommand(void) debug[3] = rxRefreshRate; } - for (int channel=ROLL; channel <= interpolationChannels; channel++) { + for (int channel=ROLL; channel < interpolationChannels; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; } @@ -252,24 +253,29 @@ void processRcCommand(void) } // Interpolate steps of rcCommand + int channel; if (factor > 0) { - for (int channel=ROLL; channel <= interpolationChannels; channel++) + for (channel=ROLL; channel < interpolationChannels; channel++) rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; } else { factor = 0; } - + readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation readyToCalculateRate = true; } else { factor = 0; // reset factor in case of level modes flip flopping } if (readyToCalculateRate || isRXDataNew) { + if (isRXDataNew) + readyToCalculateRateAxisCnt = FD_YAW; + // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); - for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) + calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2cf275de3..00c947581 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -92,6 +92,7 @@ #include "io/displayport_msp.h" #include "io/vtx.h" #include "io/vtx_smartaudio.h" +#include "io/vtx_tramp.h" #include "scheduler/scheduler.h" @@ -308,8 +309,8 @@ void init(void) beeperInit(beeperConfig()); #endif /* temp until PGs are implemented. */ -#ifdef INVERTER - initInverter(); +#ifdef USE_INVERTER + initInverters(); #endif #ifdef USE_BST @@ -523,10 +524,18 @@ void init(void) baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif +#ifdef VTX_CONTROL + #ifdef VTX_SMARTAUDIO smartAudioInit(); #endif +#ifdef VTX_TRAMP + trampInit(); +#endif + +#endif // VTX_CONTROL + // start all timers // TODO - not implemented yet timerStart(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a3069cabb..88ea65d1f 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1237,7 +1237,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { uint32_t i; - uint16_t tmp; uint8_t value; const unsigned int dataSize = sbufBytesRemaining(src); #ifdef GPS @@ -1375,10 +1374,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_MISC: - tmp = sbufReadU16(src); - if (tmp < 1600 && tmp > 1400) - rxConfig()->midrc = tmp; - + rxConfig()->midrc = sbufReadU16(src); motorConfig()->minthrottle = sbufReadU16(src); motorConfig()->maxthrottle = sbufReadU16(src); motorConfig()->mincommand = sbufReadU16(src); @@ -1639,7 +1635,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_RTC6705 case MSP_SET_VTX_CONFIG: - tmp = sbufReadU16(src); + ; + uint16_t tmp = sbufReadU16(src); if (tmp < 40) masterConfig.vtx_channel = tmp; if (current_vtx_channel != masterConfig.vtx_channel) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a13b5114d..2210ca66a 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -215,6 +215,9 @@ void taskVtxControl(uint32_t currentTime) #ifdef VTX_COMMON vtxCommonProcess(currentTime); #endif +#ifdef VTX_TRAMP + trampProcess(currentTime); +#endif } #endif @@ -300,7 +303,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_STACK_CHECK, true); #endif #ifdef VTX_CONTROL -#ifdef VTX_SMARTAUDIO +#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif diff --git a/src/main/fc/fc_tasks.c.orig b/src/main/fc/fc_tasks.c.orig new file mode 100644 index 000000000..8e9e5bf31 --- /dev/null +++ b/src/main/fc/fc_tasks.c.orig @@ -0,0 +1,517 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include + +#include "cms/cms.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/utils.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/serial.h" +#include "drivers/stack_check.h" +#include "drivers/vtx_common.h" + +#include "fc/config.h" +#include "fc/fc_msp.h" +#include "fc/fc_tasks.h" +#include "fc/fc_core.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" +#include "fc/cli.h" +#include "fc/fc_dispatch.h" + +#include "flight/pid.h" +#include "flight/altitudehold.h" + +#include "io/beeper.h" +#include "io/dashboard.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/osd.h" +#include "io/serial.h" +#include "io/transponder_ir.h" +<<<<<<< HEAD +======= +#include "io/vtx_smartaudio.h" +#include "io/vtx_tramp.h" +>>>>>>> betaflight/master + +#include "msp/msp_serial.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sonar.h" +#include "sensors/esc_sensor.h" + +#include "scheduler/scheduler.h" + +#include "telemetry/telemetry.h" + +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#ifdef USE_BST +void taskBstMasterProcess(timeUs_t currentTimeUs); +#endif + +#define TASK_PERIOD_HZ(hz) (1000000 / (hz)) +#define TASK_PERIOD_MS(ms) ((ms) * 1000) +#define TASK_PERIOD_US(us) (us) + +/* VBAT monitoring interval (in microseconds) - 1s*/ +#define VBATINTERVAL (6 * 3500) +/* IBat monitoring interval (in microseconds) - 6 default looptimes */ +#define IBATINTERVAL (6 * 3500) + + +static void taskUpdateAccelerometer(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + accUpdate(&accelerometerConfig()->accelerometerTrims); +} + +static void taskHandleSerial(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); +#ifdef USE_CLI + // in cli mode, all serial stuff goes to here. enter cli mode by sending # + if (cliMode) { + cliProcess(); + return; + } +#endif + mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); +} + +static void taskUpdateBattery(timeUs_t currentTimeUs) +{ +#if defined(USE_ADC) || defined(USE_ESC_SENSOR) + if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) { + static uint32_t vbatLastServiced = 0; + if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) { + vbatLastServiced = currentTimeUs; + updateBattery(); + } + } +#endif + + if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) { + static uint32_t ibatLastServiced = 0; + const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced); + + if (ibatTimeSinceLastServiced >= IBATINTERVAL) { + ibatLastServiced = currentTimeUs; + updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + } + } +} + +static void taskUpdateRxMain(timeUs_t currentTimeUs) +{ + processRx(currentTimeUs); + isRXDataNew = true; + +#if !defined(BARO) && !defined(SONAR) + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); +#endif + updateLEDs(); + +#ifdef BARO + if (sensors(SENSOR_BARO)) { + updateAltHoldState(); + } +#endif + +#ifdef SONAR + if (sensors(SENSOR_SONAR)) { + updateSonarAltHoldState(); + } +#endif +} + +#ifdef MAG +static void taskUpdateCompass(timeUs_t currentTimeUs) +{ + if (sensors(SENSOR_MAG)) { + compassUpdate(currentTimeUs, &compassConfig()->magZero); + } +} +#endif + +#ifdef BARO +static void taskUpdateBaro(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (sensors(SENSOR_BARO)) { + const uint32_t newDeadline = baroUpdate(); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } + } +} +#endif + +#if defined(BARO) || defined(SONAR) +static void taskCalculateAltitude(timeUs_t currentTimeUs) +{ + if (false +#if defined(BARO) + || (sensors(SENSOR_BARO) && isBaroReady()) +#endif +#if defined(SONAR) + || sensors(SENSOR_SONAR) +#endif + ) { + calculateEstimatedAltitude(currentTimeUs); + }} +#endif + +#ifdef TELEMETRY +static void taskTelemetry(timeUs_t currentTimeUs) +{ + telemetryCheckState(); + + if (!cliMode && feature(FEATURE_TELEMETRY)) { + telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + } +} +#endif + +#ifdef VTX_CONTROL +// Everything that listens to VTX devices +void taskVtxControl(uint32_t currentTime) +{ + if (ARMING_FLAG(ARMED)) + return; + +#ifdef VTX_COMMON + vtxCommonProcess(currentTime); +#endif +#ifdef VTX_TRAMP + trampProcess(currentTime); +#endif +} +#endif + +void fcTasksInit(void) +{ + schedulerInit(); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + setTaskEnabled(TASK_GYROPID, true); + + if (sensors(SENSOR_ACC)) { + setTaskEnabled(TASK_ACCEL, true); + rescheduleTask(TASK_ACCEL, acc.accSamplingInterval); + } + + setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); + setTaskEnabled(TASK_SERIAL, true); + rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz)); + setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); + setTaskEnabled(TASK_RX, true); + + setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled()); + +#ifdef BEEPER + setTaskEnabled(TASK_BEEPER, true); +#endif +#ifdef GPS + setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); +#endif +#ifdef MAG + setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); +#if defined(USE_SPI) && defined(USE_MAG_AK8963) + // fixme temporary solution for AK6983 via slave I2C on MPU9250 + rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); +#endif +#endif +#ifdef BARO + setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); +#endif +#ifdef SONAR + setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); +#endif +#if defined(BARO) || defined(SONAR) + setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); +#endif +#ifdef USE_DASHBOARD + setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); +#endif +#ifdef TELEMETRY + setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); + if (feature(FEATURE_TELEMETRY)) { + if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) { + // Reschedule telemetry to 500hz for Jeti Exbus + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); + } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) { + // Reschedule telemetry to 500hz, 2ms for CRSF + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); + } + } +#endif +#ifdef LED_STRIP + setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); +#endif +#ifdef TRANSPONDER + setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); +#endif +#ifdef OSD + setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); +#endif +#ifdef USE_BST + setTaskEnabled(TASK_BST_MASTER_PROCESS, true); +#endif +#ifdef USE_ESC_SENSOR + setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR)); +#endif +#ifdef CMS +#ifdef USE_MSP_DISPLAYPORT + setTaskEnabled(TASK_CMS, true); +#else + setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); +#endif +#endif +#ifdef STACK_CHECK + setTaskEnabled(TASK_STACK_CHECK, true); +#endif +#ifdef VTX_CONTROL +#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) + setTaskEnabled(TASK_VTXCTRL, true); +#endif +#endif +} + +cfTask_t cfTasks[TASK_COUNT] = { + [TASK_SYSTEM] = { + .taskName = "SYSTEM", + .taskFunc = taskSystem, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms + .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, + }, + + [TASK_GYROPID] = { + .taskName = "PID", + .subTaskName = "GYRO", + .taskFunc = taskMainPidLoop, + .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD, + .staticPriority = TASK_PRIORITY_REALTIME, + }, + + [TASK_ACCEL] = { + .taskName = "ACCEL", + .taskFunc = taskUpdateAccelerometer, + .desiredPeriod = TASK_PERIOD_HZ(1000), // 1000Hz, every 1ms + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_ATTITUDE] = { + .taskName = "ATTITUDE", + .taskFunc = imuUpdateAttitude, + .desiredPeriod = TASK_PERIOD_HZ(100), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_RX] = { + .taskName = "RX", + .checkFunc = rxUpdateCheck, + .taskFunc = taskUpdateRxMain, + .desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling + .staticPriority = TASK_PRIORITY_HIGH, + }, + + [TASK_SERIAL] = { + .taskName = "SERIAL", + .taskFunc = taskHandleSerial, + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud + .staticPriority = TASK_PRIORITY_LOW, + }, + + [TASK_DISPATCH] = { + .taskName = "DISPATCH", + .taskFunc = dispatchProcess, + .desiredPeriod = TASK_PERIOD_HZ(1000), + .staticPriority = TASK_PRIORITY_HIGH, + }, + + [TASK_BATTERY] = { + .taskName = "BATTERY", + .taskFunc = taskUpdateBattery, + .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + +#ifdef BEEPER + [TASK_BEEPER] = { + .taskName = "BEEPER", + .taskFunc = beeperUpdate, + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef GPS + [TASK_GPS] = { + .taskName = "GPS", + .taskFunc = gpsUpdate, + .desiredPeriod = TASK_PERIOD_HZ(10), // GPS usually don't go raster than 10Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef MAG + [TASK_COMPASS] = { + .taskName = "COMPASS", + .taskFunc = taskUpdateCompass, + .desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef BARO + [TASK_BARO] = { + .taskName = "BARO", + .taskFunc = taskUpdateBaro, + .desiredPeriod = TASK_PERIOD_HZ(20), + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef SONAR + [TASK_SONAR] = { + .taskName = "SONAR", + .taskFunc = sonarUpdate, + .desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfer with each other + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#if defined(BARO) || defined(SONAR) + [TASK_ALTITUDE] = { + .taskName = "ALTITUDE", + .taskFunc = taskCalculateAltitude, + .desiredPeriod = TASK_PERIOD_HZ(40), + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef TRANSPONDER + [TASK_TRANSPONDER] = { + .taskName = "TRANSPONDER", + .taskFunc = transponderUpdate, + .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef USE_DASHBOARD + [TASK_DASHBOARD] = { + .taskName = "DASHBOARD", + .taskFunc = dashboardUpdate, + .desiredPeriod = TASK_PERIOD_HZ(10), + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif +#ifdef OSD + [TASK_OSD] = { + .taskName = "OSD", + .taskFunc = osdUpdate, + .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif +#ifdef TELEMETRY + [TASK_TELEMETRY] = { + .taskName = "TELEMETRY", + .taskFunc = taskTelemetry, + .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef LED_STRIP + [TASK_LEDSTRIP] = { + .taskName = "LEDSTRIP", + .taskFunc = ledStripUpdate, + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef USE_BST + [TASK_BST_MASTER_PROCESS] = { + .taskName = "BST_MASTER_PROCESS", + .taskFunc = taskBstMasterProcess, + .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz, 20ms + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + +#ifdef USE_ESC_SENSOR + [TASK_ESC_SENSOR] = { + .taskName = "ESC_SENSOR", + .taskFunc = escSensorProcess, + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz every 10ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef CMS + [TASK_CMS] = { + .taskName = "CMS", + .taskFunc = cmsHandler, + .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef STACK_CHECK + [TASK_STACK_CHECK] = { + .taskName = "STACKCHECK", + .taskFunc = taskStackCheck, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + +#ifdef VTX_CONTROL + [TASK_VTXCTRL] = { + .taskName = "VTXCTRL", + .taskFunc = taskVtxControl, + .desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif +}; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fde344147..5ebe5cad3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -21,6 +21,7 @@ #define PID_CONTROLLER_BETAFLIGHT 1 #define PID_MIXER_SCALING 100.0f +#define PID_SERVO_MIXER_SCALING 7.0f #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter #define PIDSUM_LIMIT 0.5f diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 314857d42..2ba83eb02 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -341,9 +341,9 @@ STATIC_UNIT_TESTED void servoMixer(void) input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; } else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL]; - input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPIDf[YAW]; + input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL] * PID_SERVO_MIXER_SCALING; + input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH] * PID_SERVO_MIXER_SCALING; + input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 6bbfb0420..5a68a1804 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -37,8 +37,9 @@ typedef enum { FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 FUNCTION_ESC_SENSOR = (1 << 10), // 1024 - FUNCTION_VTX_CONTROL = (1 << 11), // 2048 - FUNCTION_TELEMETRY_IBUS = (1 << 12) // 4096 + FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 + FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096 + FUNCTION_VTX_TRAMP = (1 << 13), // 8192 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/vtx_common.c b/src/main/io/vtx_common.c new file mode 100644 index 000000000..5f89be8dd --- /dev/null +++ b/src/main/io/vtx_common.c @@ -0,0 +1,75 @@ +/* + * 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 +#include + +#include "platform.h" +#include "build/debug.h" + +#if defined(VTX_CONTROL) + +const uint16_t vtx58FreqTable[5][8] = +{ + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam 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 +}; + +const char * const vtx58BandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; + +const char vtx58BandLetter[] = "-ABEFR"; + +const char * const vtx58ChannelNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan) +{ + uint8_t band; + uint8_t chan; + + for (band = 0 ; band < 5 ; band++) { + for (chan = 0 ; chan < 8 ; chan++) { + if (vtx58FreqTable[band][chan] == freq) { + *pBand = band + 1; + *pChan = chan + 1; + return true; + } + } + } + + *pBand = 0; + *pChan = 0; + + return false; +} + +#endif diff --git a/src/main/io/vtx_common.h b/src/main/io/vtx_common.h new file mode 100644 index 000000000..cd593048b --- /dev/null +++ b/src/main/io/vtx_common.h @@ -0,0 +1,14 @@ +#pragma once + +#include + +#if defined(VTX_CONTROL) + +extern const uint16_t vtx58FreqTable[5][8]; +extern const char * const vtx58BandNames[]; +extern const char * const vtx58ChannelNames[]; +extern const char vtx58BandLetter[]; + +bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChan); + +#endif diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index e756c3809..fa198b0e5 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -23,7 +23,7 @@ #include "platform.h" -#ifdef VTX_SMARTAUDIO +#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) #include "cms/cms.h" #include "cms/cms_types.h" @@ -37,6 +37,7 @@ #include "drivers/vtx_common.h" #include "io/serial.h" #include "io/vtx_smartaudio.h" +#include "io/vtx_common.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -151,18 +152,6 @@ static smartAudioStat_t saStat = { .badcode = 0, }; -// 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 -}; -// XXX Should frequencies not usable in locked state be detected? - typedef struct saPowerTable_s { int rfpower; int16_t valueV1; @@ -688,9 +677,9 @@ bool smartAudioInit() } #endif - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_CONTROL); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_CONTROL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); } if (!smartAudioSerialPort) { @@ -907,9 +896,9 @@ void saCmsUpdate(void) saCmsBand = (saDevice.chan / 8) + 1; saCmsChan = (saDevice.chan % 8) + 1; - saCmsFreqRef = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsFreqRef = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; - saCmsDeviceFreq = saFreqTable[saDevice.chan / 8][saDevice.chan % 8]; + saCmsDeviceFreq = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { saCmsRFState = SACMS_TXMODE_ACTIVE; @@ -968,7 +957,7 @@ else tfp_sprintf(&saCmsStatusString[5], "%4d", saDevice.freq); else tfp_sprintf(&saCmsStatusString[5], "%4d", - saFreqTable[saDevice.chan / 8][saDevice.chan % 8]); + vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]); saCmsStatusString[9] = ' '; @@ -1006,7 +995,7 @@ static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred) saSetBandChan(saCmsBand - 1, saCmsChan - 1); - saCmsFreqRef = saFreqTable[saCmsBand - 1][saCmsChan - 1]; + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; return 0; } @@ -1031,7 +1020,7 @@ 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]; + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; return 0; } @@ -1137,7 +1126,15 @@ static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL }; static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChanNames, NULL }; -static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saPowerNames}; +static const char * const saCmsPowerNames[] = { + "---", + "25 ", + "200", + "500", + "800", +}; + +static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 }; diff --git a/src/main/io/vtx_smartaudio.c.orig b/src/main/io/vtx_smartaudio.c.orig new file mode 100644 index 000000000..575c7dede --- /dev/null +++ b/src/main/io/vtx_smartaudio.c.orig @@ -0,0 +1,1422 @@ +/* + * 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 + +#include "platform.h" + +#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) + +#include "cms/cms.h" +#include "cms/cms_types.h" + +#include "string.h" +#include "common/printf.h" +#include "common/utils.h" +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/vtx_var.h" +#include "drivers/vtx_common.h" +#include "io/serial.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx_common.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" +#include "config/config_master.h" + +#include "build/build_config.h" + +//#define SMARTAUDIO_DPRINTF +//#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 +#define dprintf(x) +#endif + +#include "build/debug.h" + +#ifdef CMS +static void saUpdateStatusString(void); // Forward +#endif + +static serialPort_t *smartAudioSerialPort = NULL; + +#if defined(CMS) || defined(VTX_COMMON) +static const char * const saPowerNames[] = { + "---", "25 ", "200", "500", "800", +}; +#endif + +#ifdef VTX_COMMON +static vtxVTable_t saVTable; // Forward +static vtxDevice_t vtxSmartAudio = { + .vTable = &saVTable, + .numBand = 5, + .numChan = 8, + .numPower = 4, + .bandNames = (char **)vtx58BandNames, + .chanNames = (char **)vtx58ChanNames, + .powerNames = (char **)saPowerNames, +}; +#endif + +// 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; + +// This is not a good design; can't distinguish command from response this way. +#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 +#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 // 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)) + +// 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; + uint16_t ooopresp; + uint16_t badcode; +} smartAudioStat_t; + +static smartAudioStat_t saStat = { + .pktsent = 0, + .pktrcvd = 0, + .badpre = 0, + .badlen = 0, + .crc = 0, + .ooopresp = 0, + .badcode = 0, +}; + +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 }, +}; + +// Last received device ('hard') states + +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 smartAudioDevice_t saDevice = { + .version = 0, + .chan = -1, + .power = -1, + .mode = 0, + .freq = 0, + .orfreq = 0, +}; + +static smartAudioDevice_t saDevicePrev = { + .version = 0, +}; + +// XXX Possible compliance problem here. Need LOCK/UNLOCK menu? +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 +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 void saPrintSettings(void) +{ +#ifdef SMARTAUDIO_DPRINTF + 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 +} + +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 +#define SMARTBAUD_MAX 4950 +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 + +static void saAutobaud(void) +{ + if (saStat.pktsent < 10) + // Not enough samples collected + return; + +#if 0 + dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", + sa_smartbaud, saStat.pktrcvd, saStat.pktsent, ((saStat.pktrcvd * 100) / saStat.pktsent))); +#endif + + if (((saStat.pktrcvd * 100) / saStat.pktsent) >= 70) { + // This is okay + saStat.pktsent = 0; // Should be more moderate? + saStat.pktrcvd = 0; + return; + } + + dprintf(("autobaud: adjusting\r\n")); + + if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) { + sa_adjdir = -1; + dprintf(("autobaud: now going down\r\n")); + } else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) { + sa_adjdir = 1; + dprintf(("autobaud: now going up\r\n")); + } + + sa_smartbaud += sa_baudstep * sa_adjdir; + + dprintf(("autobaud: %d\r\n", sa_smartbaud)); + + smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud); + + saStat.pktsent = 0; + saStat.pktrcvd = 0; +} + +// 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 saCmsUpdate(void); +#endif + +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 { + saStat.ooopresp++; + dprintf(("processResponse: outstanding %d got %d\r\n", sa_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; + + 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 SMARTAUDIO_DEBUG_MONITOR + debug[0] = saDevice.version * 100 + saDevice.mode; + debug[1] = saDevice.chan; + debug[2] = saDevice.freq; + debug[3] = saDevice.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; + + uint16_t freq = (buf[2] << 8)|buf[3]; + + if (freq & SA_FREQ_GETPIT) { + saDevice.orfreq = freq & SA_FREQ_MASK; + dprintf(("saProcessResponse: GETPIT freq %d\r\n", saDevice.orfreq)); + } else if (freq & SA_FREQ_SETPIT) { + saDevice.orfreq = freq & SA_FREQ_MASK; + dprintf(("saProcessResponse: SETPIT freq %d\r\n", saDevice.orfreq)); + } else { + saDevice.freq = freq; + dprintf(("saProcessResponse: SETFREQ freq %d\r\n", freq)); + } + break; + + case SA_CMD_SET_MODE: // Set Mode + dprintf(("saProcessResponse: SET_MODE 0x%x\r\n", buf[2])); + break; + + default: + saStat.badcode++; + return; + } + + // Debug + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + saPrintSettings(); + saDevicePrev = saDevice; + +#ifdef VTX_COMMON + // Todo: Update states in saVtxDevice? +#endif + +#ifdef CMS + // Export current device status for CMS + saCmsUpdate(); + saUpdateStatusString(); +#endif +} + +// +// Datalink +// + +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; // Don't need this (no change) + } + break; + + case S_WAITPRE2: + if (c == 0x55) { + state = S_WAITRESP; + } else { + saStat.badpre++; + 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 > SA_MAX_RCVLEN - 2) { + saStat.badlen++; + state = S_WAITPRE1; + } else if (len == 0) { + state = S_WAITCRC; + } else { + dlen = 0; + state = S_DATA; + } + break; + + case S_DATA: + // XXX Should check buffer overflow (-> saerr_overflow) + sa_rbuf[2 + dlen] = c; + if (++dlen == len) { + state = S_WAITCRC; + } + break; + + case S_WAITCRC: + if (CRC8(sa_rbuf, 2 + len) == c) { + // Got a response + saProcessResponse(sa_rbuf, len + 2); + saStat.pktrcvd++; + } else if (sa_rbuf[0] & 1) { + // Command echo + // XXX There is an exceptional case (V2 response) + // XXX Should check crc in the command format? + } else { + saStat.crc++; + } + state = S_WAITPRE1; + break; + } +} + +static void saSendFrame(uint8_t *buf, int len) +{ + int i; + + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + + for (i = 0 ; i < len ; i++) + serialWrite(smartAudioSerialPort, buf[i]); + + serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this + + sa_lastTransmission = millis(); + saStat.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. + */ + +// Retransmission + +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 queue management + +typedef struct saCmdQueue_s { + uint8_t *buf; + int len; +} saCmdQueue_t; + +#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; + +#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 + +static void saGetSettings(void) +{ + static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F}; + + saQueueCmd(bufGetSettings, 5); +} + +static void saSetFreq(uint16_t freq) +{ + static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 }; + + 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; + buf[6] = CRC8(buf, 6); + + saQueueCmd(buf, 7); +} + +#if 0 +static void saSetPitFreq(uint16_t freq) +{ + saSetFreq(freq | SA_FREQ_SETPIT); +} + +static void saGetPitFreq(void) +{ + saSetFreq(SA_FREQ_GETPIT); +} +#endif + +void saSetBandChan(uint8_t band, uint8_t 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); +} + +static void saSetMode(int mode) +{ + static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; + + buf[4] = (mode & 0x3f)|saLockMode; + buf[5] = CRC8(buf, 5); + + saQueueCmd(buf, 6); +} + +void saSetPowerByIndex(uint8_t index) +{ + static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; + + dprintf(("saSetPowerByIndex: index %d\r\n", index)); + + if (saDevice.version == 0) { + // Unknown or yet unknown version. + return; + } + + if (index > 3) + return; + + buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; + buf[5] = CRC8(buf, 5); + saQueueCmd(buf, 6); +} + +bool smartAudioInit() +{ +#ifdef SMARTAUDIO_DPRINTF + // Setup debugSerialPort + + debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0); + if (debugSerialPort) { + setPrintfSerialPort(debugSerialPort); + dprintf(("smartAudioInit: OK\r\n")); + } +#endif + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); + if (portConfig) { + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + } + + if (!smartAudioSerialPort) { + return false; + } + + vtxSmartAudio.vTable = &saVTable; + vtxCommonRegisterDevice(&vtxSmartAudio); + + return true; +} + +void vtxSAProcess(uint32_t now) +{ + static bool initialSent = false; + + if (smartAudioSerialPort == NULL) + return; + + while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { + uint8_t c = serialRead(smartAudioSerialPort); + saReceiveFramer((uint16_t)c); + } + + // Re-evaluate baudrate after each frame reception + saAutobaud(); + + if (!initialSent) { + saGetSettings(); + saSetFreq(SA_FREQ_GETPIT); + saSendQueue(); + initialSent = true; + return; + } + + 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)); + // XXX Todo: Resend termination and possible offline transition + 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 + //dprintf(("process: sending heartbeat\r\n")); + saGetSettings(); + saSendQueue(); + } + +#ifdef SMARTAUDIO_TEST_VTX_COMMON + // Testing VTX_COMMON API + { + static uint32_t lastMonitorUs = 0; + if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) + return; + + static uint8_t monBand; + static uint8_t monChan; + static uint8_t monPower; + + vtxCommonGetBandChan(&monBand, &monChan); + vtxCommonGetPowerIndex(&monPower); + debug[0] = monBand; + debug[1] = monChan; + debug[2] = monPower; + } +#endif +} + +#ifdef VTX_COMMON +// Interface to common VTX API + +vtxDevType_e vtxSAGetDeviceType(void) +{ + return VTXDEV_SMARTAUDIO; +} + +bool vtxSAIsReady(void) +{ + return !(saDevice.version == 0); +} + +void vtxSASetBandChan(uint8_t band, uint8_t chan) +{ + if (band && chan) + saSetBandChan(band - 1, chan - 1); +} + +void vtxSASetPowerByIndex(uint8_t index) +{ + if (index == 0) { + // SmartAudio doesn't support power off. + return; + } + + saSetPowerByIndex(index - 1); +} + +void vtxSASetPitmode(uint8_t onoff) +{ + if (!(vtxSAIsReady() && (saDevice.version == 2))) + return; + + if (onoff) { + // SmartAudio can not turn pit mode on by software. + return; + } + + uint8_t newmode = SA_MODE_CLR_PITMODE; + + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) + newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + + saSetMode(newmode); + + return true; +} + +bool vtxSAGetBandChan(uint8_t *pBand, uint8_t *pChan) +{ + if (!vtxSAIsReady()) + return false; + + *pBand = (saDevice.chan / 8) + 1; + *pChan = (saDevice.chan % 8) + 1; + return true; +} + +bool vtxSAGetPowerIndex(uint8_t *pIndex) +{ + if (!vtxSAIsReady()) + return false; + + *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; + return true; +} + +bool vtxSAGetPitmode(uint8_t *pOnoff) +{ + if (!(vtxSAIsReady() && (saDevice.version == 2))) + return false; + + *pOnoff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; + return true; +} + +static vtxVTable_t saVTable = { + .process = vtxSAProcess, + .getDeviceType = vtxSAGetDeviceType, + .isReady = vtxSAIsReady, + .setBandChan = vtxSASetBandChan, + .setPowerByIndex = vtxSASetPowerByIndex, + .setPitmode = vtxSASetPitmode, + .getBandChan = vtxSAGetBandChan, + .getPowerIndex = vtxSAGetPowerIndex, + .getPitmode = vtxSAGetPitmode, +}; +#endif // VTX_COMMON + +#ifdef CMS + +// Interface to CMS + +// Operational Model and RF modes (CMS) + +#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 + +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 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; +uint8_t saCmsPower; +uint8_t saCmsPitFMode; // In-Range or Out-Range +uint8_t saCmsFselMode; // Channel(0) or User defined(1) + +uint16_t saCmsORFreq = 0; // POR frequency +uint16_t saCmsORFreqNew; // POR frequency + +uint16_t saCmsUserFreq = 0; // User defined frequency +uint16_t saCmsUserFreqNew; // User defined frequency + +void saCmsUpdate(void) +{ +// XXX Take care of pit mode update somewhere??? + + if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { + // 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 = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; + + saCmsDeviceFreq = vtx58FreqTable[saDevice.chan / 8][saDevice.chan % 8]; + + if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { + saCmsRFState = SACMS_TXMODE_ACTIVE; + } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + saCmsRFState = SACMS_TXMODE_PIT_INRANGE; + } else { + saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; + } + + if (saDevice.version == 2) { + saCmsPower = saDevice.power + 1; // XXX Take care V1 + } else { + saCmsPower = saDacToPowerIndex(saDevice.power) + 1; + } + } + + saUpdateStatusString(); +} + +char saCmsStatusString[31] = "- -- ---- ---"; +// m bc ffff ppp +// 0123456789012 + +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) +{ + if (saDevice.version == 0) + return; + +// XXX These should be done somewhere else +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) + saCmsPitFMode = 1; +else + saCmsPitFMode = 0; + + saCmsStatusString[0] = "-FR"[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", + vtx58FreqTable[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 { + saCmsStatusString[11] = 'O'; + } + saCmsStatusString[12] = 'R'; + saCmsStatusString[13] = 0; + } else { + tfp_sprintf(&saCmsStatusString[10], "%3d", (saDevice.version == 2) ? saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower); + } +} + +static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (saDevice.version == 0) { + // Bounce back; not online yet + saCmsBand = 0; + return 0; + } + + if (saCmsBand == 0) { + // Bouce back, no going back to undef state + saCmsBand = 1; + return 0; + } + + if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; + + return 0; +} + +static long saCmsConfigChanByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (saDevice.version == 0) { + // Bounce back; not online yet + saCmsChan = 0; + return 0; + } + + if (saCmsChan == 0) { + // Bounce back; no going back to undef state + saCmsChan = 1; + return 0; + } + + if ((saCmsOpmodel == SACMS_OPMODEL_FREE) && !saDeferred) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + + saCmsFreqRef = vtx58FreqTable[saCmsBand - 1][saCmsChan - 1]; + + return 0; +} + +static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (saDevice.version == 0) { + // Bounce back; not online yet + saCmsPower = 0; + return 0; + } + + if (saCmsPower == 0) { + // Bouce back; no going back to undef state + saCmsPower = 1; + return 0; + } + + if (saCmsOpmodel == SACMS_OPMODEL_FREE) + saSetPowerByIndex(saCmsPower - 1); + + return 0; +} + +static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode)); + + if (saCmsPitFMode == 0) { + saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); + } else { + saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); + } + + return 0; +} + +static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + uint8_t opmodel = saCmsOpmodel; + + dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel)); + + if (opmodel == SACMS_OPMODEL_FREE) { + // VTX should power up transmitting. + // Turn off In-Range and Out-Range bits + saSetMode(0); + } 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. + saCmsPitFMode = 0; + saCmsConfigPitFModeByGvar(pDisp, self); + } else { + // Trying to go back to unknown state; bounce back + saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1; + } + + return 0; +} + +static const char * const saCmsDeviceStatusNames[] = { + "OFFL", + "ONL V1", + "ONL V2", +}; + +static OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames }; + +static OSD_Entry saCmsMenuStatsEntries[] = { + { "- SA STATS -", OME_Label, 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 saCmsMenuStats = { + .GUARD_text = "XSAST", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuStatsEntries +}; + +static OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames, NULL }; + +<<<<<<< HEAD +static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChanNames, NULL }; +======= +static OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, &vtx58ChannelNames[0], NULL }; + +static const char * const saCmsPowerNames[] = { + "---", + "25 ", + "200", + "500", + "800", +}; +>>>>>>> betaflight/master + +static OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saPowerNames}; + +static OSD_UINT16_t saCmsEntFreqRef = { &saCmsFreqRef, 5600, 5900, 0 }; + +static const char * const saCmsOpmodelNames[] = { + "----", + "FREE", + "RACE", +}; + +static const char * const saCmsFselModeNames[] = { + "CHAN", + "USER" +}; + +static const char * const saCmsPitFModeNames[] = { + "PIR", + "POR" +}; + +static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames }; + +static long sacms_SetupTopMenu(void); // Forward + +static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (saCmsFselMode == 0) { + // CHAN + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + } else { + // USER: User frequency mode is only available in FREE opmodel. + if (saCmsOpmodel == SACMS_OPMODEL_FREE) { + saSetFreq(saCmsUserFreq); + } else { + // Bounce back + saCmsFselMode = 0; + } + } + + sacms_SetupTopMenu(); + + return 0; +} + +static long saCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (saCmsOpmodel == SACMS_OPMODEL_RACE) { + // Race model + // Setup band, freq and power. + + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + saSetPowerByIndex(saCmsPower - 1); + + // If in pit mode, cancel it. + + 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 { + // Freestyle model + // Setup band and freq / user freq + if (saCmsFselMode == 0) + saSetBandChan(saCmsBand - 1, saCmsChan - 1); + else + saSetFreq(saCmsUserFreq); + } + + return MENU_CHAIN_BACK; +} + +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 char *saCmsORFreqGetString(void) +{ + static char pbuf[5]; + + tfp_sprintf(pbuf, "%4d", saCmsORFreq); + + return pbuf; +} + +static char *saCmsUserFreqGetString(void) +{ + static char pbuf[5]; + + tfp_sprintf(pbuf, "%4d", saCmsUserFreq); + + return pbuf; +} + +static long saCmsSetUserFreqOnEnter(void) +{ + saCmsUserFreqNew = saCmsUserFreq; + + return 0; +} + +static long saCmsSetUserFreq(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + saCmsUserFreq = saCmsUserFreqNew; + saSetFreq(saCmsUserFreq); + + return 0; +} + +static OSD_Entry saCmsMenuPORFreqEntries[] = { + { "- 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 = +{ + .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, &(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 = +{ + .GUARD_text = "XSAUFQ", + .GUARD_type = OME_MENU, + .onEnter = saCmsSetUserFreqOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuUserFreqEntries, +}; + +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 }, + { "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 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu saCmsMenuConfig = { + .GUARD_text = "XSACFG", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .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 } +}; + +static CMS_Menu saCmsMenuCommence = { + .GUARD_text = "XVTXCOM", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuCommenceEntries, +}; + +static OSD_Entry saCmsMenuFreqModeEntries[] = { + { "- SMARTAUDIO -", OME_Label, NULL, NULL, 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 } +}; + +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, &saCmsEntFreqRef, 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, &saCmsMenuStats, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuVtxSmartAudio; // Forward + +static long sacms_SetupTopMenu(void) +{ + if (saCmsDeviceStatus) { + if (saCmsFselMode == 0) + cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries; + else + cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries; + } else { + cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries; + } + + return 0; +} + +CMS_Menu cmsx_menuVtxSmartAudio = { + .GUARD_text = "XVTXSA", + .GUARD_type = OME_MENU, + .onEnter = sacms_SetupTopMenu, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = saCmsMenuOfflineEntries, +}; + +#endif // CMS + +#endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c new file mode 100644 index 000000000..4e4716836 --- /dev/null +++ b/src/main/io/vtx_tramp.c @@ -0,0 +1,597 @@ +/* + * 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 +#include + +#include "platform.h" + +#if defined(VTX_TRAMP) && defined(VTX_CONTROL) + +#include "build/debug.h" + +#include "common/utils.h" +#include "common/printf.h" + +#include "io/serial.h" +#include "drivers/serial.h" +#include "drivers/system.h" +#include "io/vtx_tramp.h" +#include "io/vtx_common.h" + +static serialPort_t *trampSerialPort = NULL; + +static uint8_t trampReqBuffer[16]; +static uint8_t trampRespBuffer[16]; + +typedef enum { + TRAMP_STATUS_BAD_DEVICE = -1, + TRAMP_STATUS_OFFLINE = 0, + TRAMP_STATUS_ONLINE, + TRAMP_STATUS_SET_FREQ_PW, + TRAMP_STATUS_CHECK_FREQ_PW +} trampStatus_e; + +trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; + +uint32_t trampRFFreqMin; +uint32_t trampRFFreqMax; +uint32_t trampRFPowerMax; + +uint32_t trampCurFreq = 0; +uint8_t trampCurBand = 0; +uint8_t trampCurChan = 0; +uint16_t trampCurPower = 0; // Actual transmitting power +uint16_t trampCurConfigPower = 0; // Configured transmitting power +int16_t trampCurTemp = 0; +uint8_t trampCurPitmode = 0; + +uint32_t trampConfFreq = 0; +uint16_t trampConfPower = 0; + +#ifdef CMS +static void trampCmsUpdateStatusString(void); // Forward +#endif + +static void trampWriteBuf(uint8_t *buf) +{ + serialWriteBuf(trampSerialPort, buf, 16); +} + +static uint8_t trampChecksum(uint8_t *trampBuf) +{ + uint8_t cksum = 0; + + for (int i = 1 ; i < 14 ; i++) + cksum += trampBuf[i]; + + return cksum; +} + +void trampCmdU16(uint8_t cmd, uint16_t param) +{ + if (!trampSerialPort) + return; + + memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); + trampReqBuffer[0] = 15; + trampReqBuffer[1] = cmd; + trampReqBuffer[2] = param & 0xff; + trampReqBuffer[3] = (param >> 8) & 0xff; + trampReqBuffer[14] = trampChecksum(trampReqBuffer); + trampWriteBuf(trampReqBuffer); +} + +void trampSetFreq(uint16_t freq) +{ + trampConfFreq = freq; +} + +void trampSendFreq(uint16_t freq) +{ + trampCmdU16('F', freq); +} + +void trampSetBandChan(uint8_t band, uint8_t chan) +{ + trampSetFreq(vtx58FreqTable[band - 1][chan - 1]); +} + +void trampSetRFPower(uint16_t level) +{ + trampConfPower = level; +} + +void trampSendRFPower(uint16_t level) +{ + trampCmdU16('P', level); +} + +// return false if error +bool trampCommitChanges() +{ + if(trampStatus != TRAMP_STATUS_ONLINE) + return false; + + trampStatus = TRAMP_STATUS_SET_FREQ_PW; + return true; +} + +void trampSetPitmode(uint8_t onoff) +{ + trampCmdU16('I', onoff ? 0 : 1); +} + +// returns completed response code +char trampHandleResponse(void) +{ + uint8_t respCode = trampRespBuffer[1]; + + switch (respCode) { + case 'r': + { + uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if(min_freq != 0) { + trampRFFreqMin = min_freq; + trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); + return 'r'; + } + + // throw bytes echoed from tx to rx in bidirectional mode away + } + break; + + case 'v': + { + uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if(freq != 0) { + trampCurFreq = freq; + trampCurConfigPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampCurPitmode = trampRespBuffer[7]; + trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); + vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + return 'v'; + } + + // throw bytes echoed from tx to rx in bidirectional mode away + } + break; + + case 's': + { + uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + if(temp != 0) { + trampCurTemp = temp; + return 's'; + } + } + break; + } + + return 0; +} + +typedef enum { + S_WAIT_LEN = 0, // Waiting for a packet len + S_WAIT_CODE, // Waiting for a response code + S_DATA, // Waiting for rest of the packet. +} trampReceiveState_e; + +static trampReceiveState_e trampReceiveState = S_WAIT_LEN; +static int trampReceivePos = 0; + +static void trampResetReceiver() +{ + trampReceiveState = S_WAIT_LEN; + trampReceivePos = 0; +} + +static bool trampIsValidResponseCode(uint8_t code) +{ + if (code == 'r' || code == 'v' || code == 's') + return true; + else + return false; +} + +// returns completed response code or 0 +static char trampReceive(uint32_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (!trampSerialPort) + return 0; + + while (serialRxBytesWaiting(trampSerialPort)) { + uint8_t c = serialRead(trampSerialPort); + trampRespBuffer[trampReceivePos++] = c; + + switch(trampReceiveState) { + case S_WAIT_LEN: + if (c == 0x0F) { + trampReceiveState = S_WAIT_CODE; + } else { + trampReceivePos = 0; + } + break; + + case S_WAIT_CODE: + if (trampIsValidResponseCode(c)) { + trampReceiveState = S_DATA; + } else { + trampResetReceiver(); + } + break; + + case S_DATA: + if (trampReceivePos == 16) { + uint8_t cksum = trampChecksum(trampRespBuffer); + + trampResetReceiver(); + + if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { + return trampHandleResponse(); + } + } + break; + + default: + trampResetReceiver(); + } + } + + return 0; +} + +void trampQuery(uint8_t cmd) +{ + trampResetReceiver(); + trampCmdU16(cmd, 0); +} + +void trampQueryR(void) +{ + trampQuery('r'); +} + +void trampQueryV(void) +{ + trampQuery('v'); +} + +void trampQueryS(void) +{ + trampQuery('s'); +} + +#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) + +bool trampInit() +{ + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); + + if (portConfig) { + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + } + + if (!trampSerialPort) { + return false; + } + + return true; +} + +void trampProcess(uint32_t currentTimeUs) +{ + static uint32_t lastQueryTimeUs = 0; + +#ifdef TRAMP_DEBUG + static uint16_t debugFreqReqCounter = 0; + static uint16_t debugPowReqCounter = 0; +#endif + + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + return; + + char replyCode = trampReceive(currentTimeUs); + +#ifdef TRAMP_DEBUG + debug[0] = trampStatus; +#endif + + switch(replyCode) { + case 'r': + if (trampStatus <= TRAMP_STATUS_OFFLINE) + trampStatus = TRAMP_STATUS_ONLINE; + break; + + case 'v': + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + trampStatus = TRAMP_STATUS_SET_FREQ_PW; + break; + } + + switch(trampStatus) { + + case TRAMP_STATUS_OFFLINE: + case TRAMP_STATUS_ONLINE: + if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s + + if (trampStatus == TRAMP_STATUS_OFFLINE) + trampQueryR(); + else { + static unsigned int cnt = 0; + if(((cnt++) & 1) == 0) + trampQueryV(); + else + trampQueryS(); + } + + lastQueryTimeUs = currentTimeUs; + } + break; + + case TRAMP_STATUS_SET_FREQ_PW: + { + bool done = true; + if (trampConfFreq != trampCurFreq) { + trampSendFreq(trampConfFreq); +#ifdef TRAMP_DEBUG + debugFreqReqCounter++; +#endif + done = false; + } + else if (trampConfPower != trampCurConfigPower) { + trampSendRFPower(trampConfPower); +#ifdef TRAMP_DEBUG + debugPowReqCounter++; +#endif + done = false; + } + + if(!done) { + trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; + + // delay next status query by 300ms + lastQueryTimeUs = currentTimeUs + 300 * 1000; + } + else { + // everything has been done, let's return to original state + trampStatus = TRAMP_STATUS_ONLINE; + } + } + break; + + case TRAMP_STATUS_CHECK_FREQ_PW: + if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) { + trampQueryV(); + lastQueryTimeUs = currentTimeUs; + } + break; + + default: + break; + } + +#ifdef TRAMP_DEBUG + debug[1] = debugFreqReqCounter; + debug[2] = debugPowReqCounter; + debug[3] = 0; +#endif + +#ifdef CMS + trampCmsUpdateStatusString(); +#endif +} + +#ifdef CMS +#include "cms/cms.h" +#include "cms/cms_types.h" + + +char trampCmsStatusString[31] = "- -- ---- ----"; +// m bc ffff tppp +// 01234567890123 + +static void trampCmsUpdateStatusString(void) +{ + trampCmsStatusString[0] = '*'; + trampCmsStatusString[1] = ' '; + trampCmsStatusString[2] = vtx58BandLetter[trampCurBand]; + trampCmsStatusString[3] = vtx58ChannelNames[trampCurChan][0]; + trampCmsStatusString[4] = ' '; + + if (trampCurFreq) + tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq); + else + tfp_sprintf(&trampCmsStatusString[5], "----"); + + if (trampCurPower) { + tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampCurPower == trampCurConfigPower) ? ' ' : '*', trampCurPower); + } + else + tfp_sprintf(&trampCmsStatusString[9], " ----"); +} + +uint8_t trampCmsPitmode = 0; +uint8_t trampCmsBand = 1; +uint8_t trampCmsChan = 1; +uint16_t trampCmsFreqRef; + +static OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames, NULL }; + +static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }; + +static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; + +static const char * const trampCmsPowerNames[] = { + "25 ", "100", "200", "400", "600" +}; + +static const uint16_t trampCmsPowerTable[] = { + 25, 100, 200, 400, 600 +}; + +static uint8_t trampCmsPower = 0; + +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampCmsPowerNames, NULL }; + +static void trampCmsUpdateFreqRef(void) +{ + if (trampCmsBand > 0 && trampCmsChan > 0) + trampCmsFreqRef = vtx58FreqTable[trampCmsBand - 1][trampCmsChan - 1]; +} + +static long trampCmsConfigBand(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (trampCmsBand == 0) + // Bounce back + trampCmsBand = 1; + else + trampCmsUpdateFreqRef(); + + return 0; +} + +static long trampCmsConfigChan(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (trampCmsChan == 0) + // Bounce back + trampCmsChan = 1; + else + trampCmsUpdateFreqRef(); + + return 0; +} + +static OSD_INT16_t trampCmsEntTemp = { &trampCurTemp, -100, 300, 0 }; + +static const char * const trampCmsPitmodeNames[] = { + "---", "OFF", "ON " +}; + +static OSD_TAB_t trampCmsEntPitmode = { &trampCmsPitmode, 2, trampCmsPitmodeNames, NULL }; + +static long trampCmsSetPitmode(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (trampCmsPitmode == 0) { + // Bouce back + trampCmsPitmode = 1; + } else { + trampSetPitmode(trampCmsPitmode - 1); + } + + return 0; +} + +static long trampCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + trampSetBandChan(trampCmsBand, trampCmsChan); + trampSetRFPower(trampCmsPowerTable[trampCmsPower]); + + // If it fails, the user should retry later + trampCommitChanges(); + + + return MENU_CHAIN_BACK; +} + +static void trampCmsInitSettings() +{ + if(trampCurBand > 0) trampCmsBand = trampCurBand; + if(trampCurChan > 0) trampCmsChan = trampCurChan; + + trampCmsUpdateFreqRef(); + trampCmsPitmode = trampCurPitmode + 1; + + if (trampCurConfigPower > 0) { + for (uint8_t i = 0; i < sizeof(trampCmsPowerTable); i++) { + if (trampCurConfigPower <= trampCmsPowerTable[i]) { + trampCmsPower = i; + break; + } + } + } +} + +static long trampCmsOnEnter() +{ + trampCmsInitSettings(); + return 0; +} + +static OSD_Entry trampCmsMenuCommenceEntries[] = { + { "CONFIRM", OME_Label, NULL, NULL, 0 }, + { "YES", OME_Funcall, trampCmsCommence, NULL, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static CMS_Menu trampCmsMenuCommence = { + .GUARD_text = "XVTXTRC", + .GUARD_type = OME_MENU, + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = trampCmsMenuCommenceEntries, +}; + +static OSD_Entry trampMenuEntries[] = +{ + { "- TRAMP -", OME_Label, NULL, NULL, 0 }, + + { "", OME_Label, NULL, trampCmsStatusString, DYNAMIC }, + { "PIT", OME_TAB, trampCmsSetPitmode, &trampCmsEntPitmode, 0 }, + { "BAND", OME_TAB, trampCmsConfigBand, &trampCmsEntBand, 0 }, + { "CHAN", OME_TAB, trampCmsConfigChan, &trampCmsEntChan, 0 }, + { "(FREQ)", OME_UINT16, NULL, &trampCmsEntFreqRef, DYNAMIC }, + { "POWER", OME_TAB, NULL, &trampCmsEntPower, 0 }, + { "T(C)", OME_INT16, NULL, &trampCmsEntTemp, DYNAMIC }, + { "SET", OME_Submenu, cmsMenuChange, &trampCmsMenuCommence, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuVtxTramp = { + .GUARD_text = "XVTXTR", + .GUARD_type = OME_MENU, + .onEnter = trampCmsOnEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = trampMenuEntries, +}; +#endif + +#endif // VTX_TRAMP diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h new file mode 100644 index 000000000..1864cf0a9 --- /dev/null +++ b/src/main/io/vtx_tramp.h @@ -0,0 +1,14 @@ +#pragma once + +#if defined(VTX_TRAMP) && defined(VTX_CONTROL) + +bool trampInit(); +void trampProcess(uint32_t currentTimeUs); + +#ifdef CMS +#include "cms/cms.h" +#include "cms/cms_types.h" +extern CMS_Menu cmsx_menuVtxTramp; +#endif + +#endif diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index a2d1c74c4..1c234b3a2 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -33,8 +33,7 @@ #define BEEPER PC13 #define BEEPER_INVERTED -#define INVERTER PC15 -#define INVERTER_USART USART2 +#define INVERTER_PIN_USART2 PC15 // MPU interrupt #define USE_EXTI diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index ac0e53428..34d4a9804 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -38,8 +38,8 @@ #define BEEPER_OPT PB7 #define BEEPER_INVERTED -#define INVERTER PB15 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PB15 +//#define INVERTER_PIN_USART1 PC9 #define UART1_INVERTER PC9 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 77a24a129..9871d0cf4 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -19,8 +19,7 @@ #define LED0 PB3 -#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER_PIN_USART1 PB2 // PB2 (BOOT1) used as inverter select GPIO #define BEEPER PA15 #define BEEPER_OPT PA2 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index b48f412fd..cfd8ef165 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -33,8 +33,7 @@ #define BEEPER PC5 -#define INVERTER PB2 // PB2 used as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER_PIN_USART2 PB2 // PB2 used as inverter select GPIO #define MPU6000_CS_PIN PC4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 390f8f93a..2a148a5a0 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -28,8 +28,7 @@ #define BEEPER PE5 -#define INVERTER PD3 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PD3 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index da9c69ec0..c83dde617 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -28,8 +28,7 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define INVERTER PC8 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PC8 #define USE_EXTI #define MPU_INT_EXTI PC4 diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index a343c48b3..22f7aea92 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -28,8 +28,7 @@ #define BEEPER PA8 #define BEEPER_INVERTED -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER_PIN_USART1 PC0 // PC0 used as inverter select GPIO // MPU6000 interrupts #define USE_EXTI diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index d3b02f787..6007db44c 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -28,8 +28,7 @@ #define BEEPER PA8 -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER_PIN_USART1 PC0 // PC0 used as inverter select GPIO // MPU6000 interrupts #define USE_EXTI diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index e29948be6..8b6cc28d9 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -27,8 +27,7 @@ #define BARO_XCLR_PIN PC13 #define BARO_EOC_PIN PC14 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER_PIN_USART2 PB2 // PB2 (BOOT1) abused as inverter select GPIO #define USE_EXTI #define MAG_INT_EXTI PC14 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 1a047c40d..b9cc396a3 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -44,8 +44,7 @@ //#define BARO_XCLR_PIN PC13 //#define BARO_EOC_PIN PC14 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER_PIN_USART2 PB2 // PB2 (BOOT1) abused as inverter select GPIO #define USE_EXTI #define MAG_INT_EXTI PC14 @@ -122,16 +121,16 @@ #define USE_UART2 /* only 2 uarts available on the NAZE, add ifdef here if present on other boards */ //#define USE_UART3 -//#define USE_SOFTSERIAL1 -//#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 2 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 4 -//#define SOFTSERIAL_1_TIMER TIM3 -//#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 -//#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -//#define SOFTSERIAL_2_TIMER TIM3 -//#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 -//#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 409854ae2..6d4289da5 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -35,8 +35,7 @@ #define BEEPER PB4 #define BEEPER_INVERTED -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER_PIN_USART1 PC0 // PC0 used as inverter select GPIO #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 7483128e6..fe7dc33dc 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -69,8 +69,7 @@ #endif // PC0 used as inverter select GPIO -#define INVERTER PC0 -#define INVERTER_USART USART1 +#define INVERTER_PIN_USART1 PC0 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index e4dd143fa..cfed437f8 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -30,8 +30,7 @@ #define BEEPER PC13 -#define INVERTER PC15 -#define INVERTER_USART USART2 //Sbus on USART 2 of nano. +#define INVERTER_PIN_USART2 PC15 //Sbus on USART 2 of nano. #define MPU9250_CS_PIN PB12 #define MPU9250_SPI_INSTANCE SPI2 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 1dc708384..7ac23dcf0 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -32,8 +32,7 @@ #define BEEPER PC9 #define BEEPER_INVERTED -#define INVERTER PC6 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PC6 #define USE_ESC_SENSOR diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index ec7a8925f..fa27f4785 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -27,8 +27,7 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define INVERTER PD7 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PD7 #define MPU6500_CS_PIN PE10 #define MPU6500_SPI_INSTANCE SPI2 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 40387f877..28e9d6887 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -29,8 +29,7 @@ #define BEEPER PC9 //#define BEEPER_INVERTED -#define INVERTER PB15 -#define INVERTER_USART USART6 +#define INVERTER_PIN_USART6 PB15 // Gyro interrupt diff --git a/src/main/target/common.h b/src/main/target/common.h index 13a422034..a6db3d021 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -106,6 +106,7 @@ #define VTX_COMMON #define VTX_CONTROL #define VTX_SMARTAUDIO +#define VTX_TRAMP #define USE_SENSOR_NAMES #else #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 193124c07..d881516c1 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -22,5 +22,4 @@ // Targets with built-in vtx do not need external vtx #if defined(VTX) || defined(USE_RTC6705) # undef VTX_CONTROL -# undef VTX_SMARTAUDIO #endif diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5d0281f4d..b089714cc 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -322,14 +322,14 @@ void freeSmartPortTelemetryPort(void) void configureSmartPortTelemetryPort(void) { - portOptions_t portOptions; - if (!portConfig) { return; } + portOptions_t portOptions = 0; + if (telemetryConfig->sportHalfDuplex) { - portOptions = SERIAL_BIDIR; + portOptions |= SERIAL_BIDIR; } if (telemetryConfig->telemetry_inversion) {