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

Rebase + touch-up 

Includes fix for SmartAudio CMS behavior
This commit is contained in:
jflyper 2017-01-16 22:32:59 +09:00
parent b1e1c7ef7b b60bc0a2e1
commit 1d8018f547
41 changed files with 2841 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Makefile
View File

@ -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)))

8
src/main/cms/cms_menu_builtin.c
View File

@ -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

3
src/main/cms/cms_menu_osd.c
View File

@ -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}
};

103
src/main/drivers/inverter.c
View File

@ -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
}

12
src/main/drivers/inverter.h
View File

@ -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);

8
src/main/drivers/serial_uart.c
View File

@ -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

2
src/main/drivers/serial_usb_vcp.c
View File

@ -204,8 +204,8 @@ serialPort_t *usbVcpOpen(void)
#else
Set_System();
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
USB_Interrupts_Config();
#endif
s = &vcpPort;

25
src/main/fc/cli.c
View File

@ -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);

16
src/main/fc/fc_core.c
View File

@ -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;
}

13
src/main/fc/fc_init.c
View File

@ -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();

9
src/main/fc/fc_msp.c
View File

@ -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) {

5
src/main/fc/fc_tasks.c
View File

@ -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

517
src/main/fc/fc_tasks.c.orig Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <platform.h>
#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
};

1
src/main/flight/pid.h
View File

@ -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

6
src/main/flight/servos.c
View File

@ -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)) {

5
src/main/io/serial.h
View File

@ -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 {

75
src/main/io/vtx_common.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/* Created by jflyper */
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#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

14
src/main/io/vtx_common.h Normal file
View File

@ -0,0 +1,14 @@
#pragma once
#include <stdint.h>
#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

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

@ -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 };

1422
src/main/io/vtx_smartaudio.c.orig Normal file
View File

File diff suppressed because it is too large Load Diff

597
src/main/io/vtx_tramp.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/* Created by jflyper */
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#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

14
src/main/io/vtx_tramp.h Normal file
View File

@ -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

3
src/main/target/ALIENFLIGHTF4/target.h
View File

@ -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

4
src/main/target/BLUEJAYF4/target.h
View File

@ -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

3
src/main/target/CC3D/target.h
View File

@ -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

3
src/main/target/COLIBRI/target.h
View File

@ -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

3
src/main/target/F4BY/target.h
View File

@ -28,8 +28,7 @@
#define BEEPER PE5
#define INVERTER PD3
#define INVERTER_USART USART6
#define INVERTER_PIN_USART6 PD3

3
src/main/target/FISHDRONEF4/target.h
View File

@ -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

3
src/main/target/FURYF4/target.h
View File

@ -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

3
src/main/target/KIWIF4/target.h
View File

@ -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

3
src/main/target/MICROSCISKY/target.h
View File

@ -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

21
src/main/target/NAZE/target.h
View File

@ -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

3
src/main/target/OMNIBUSF4/target.h
View File

@ -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

3
src/main/target/REVO/target.h
View File

@ -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

3
src/main/target/REVONANO/target.h
View File

@ -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

3
src/main/target/SPARKY2/target.h
View File

@ -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

3
src/main/target/VRRACE/target.h
View File

@ -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

3
src/main/target/YUPIF4/target.h
View File

@ -29,8 +29,7 @@
#define BEEPER PC9
//#define BEEPER_INVERTED
#define INVERTER PB15
#define INVERTER_USART USART6
#define INVERTER_PIN_USART6 PB15
// Gyro interrupt

1
src/main/target/common.h
View File

@ -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

1
src/main/target/common_post.h
View File

@ -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

6
src/main/telemetry/smartport.c
View File

@ -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) {