Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge branch 'development' into dzikuvx-correct-3d-throttle-handling

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-07 13:22:21 +01:00
commit fab7de6a48
71 changed files with 1065 additions and 2468 deletions
Download patch file Download diff file Expand all files Collapse all files

8
docs/Cli.md
View file

@ -421,10 +421,10 @@ A shorter form is also supported to enable and disable functions using `serial <
| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |
| dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
| dyn_notch_width_percent | 8 | Distance in % of the attenuated frequency for double dynamic filter notched. When set to `0` single dynamic notch filter is used | | dynamic_gyro_notch_enabled | `OFF` | Enable/disable dynamic gyro notch also known as Matrix Filter |
| dyn_notch_range | MEDIUM | Dynamic gyro filter range. Possible values `LOW` `MEDIUM` `HIGH`. `MEDIUM` should work best for 5-6" multirotors. `LOW` should work best with 7" and bigger. `HIGH` should work with everything below 4" | | dynamic_gyro_notch_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers |
| dyn_notch_q | 120 | Q factor for dynamic notches | | dynamic_gyro_notch_q | 120 | Q factor for dynamic notches |
| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | | dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) |
| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | | rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV |

9
docs/Logic Conditions.md
View file

@ -70,6 +70,15 @@ Logic conditions can be edited using INAV Configurator user interface, of via CL
| 14 | TROTTLE_POS | in `%` | | 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` | | 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` | | 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_WP | boolean `0`/`1` |
| 24 | IS_LANDING | boolean `0`/`1` |
| 25 | IS_FAILSAFE | boolean `0`/`1` |
#### FLIGHT_MODE #### FLIGHT_MODE

4
docs/Rx.md
View file

@ -154,11 +154,9 @@ The receiver type can be set from the configurator or CLI.
``` ```
# get receiver_type # get receiver_type
receiver_type = NONE receiver_type = NONE
Allowed values: NONE, PWM, PPM, SERIAL, MSP, SPI, UIB Allowed values: NONE, PPM, SERIAL, MSP, SPI, UIB
``` ```
Note that `PWM` is a synonym for `NONE`.
### RX signal-loss detection ### RX signal-loss detection
The software has signal loss detection which is always enabled. Signal loss detection is used for safety and failsafe reasons. The software has signal loss detection which is always enabled. Signal loss detection is used for safety and failsafe reasons.

5
make/source.mk
View file

@ -98,6 +98,7 @@ COMMON_SRC = \
flight/wind_estimator.c \ flight/wind_estimator.c \
flight/gyroanalyse.c \ flight/gyroanalyse.c \
flight/rpm_filter.c \ flight/rpm_filter.c \
flight/dynamic_gyro_notch.c \
io/beeper.c \ io/beeper.c \
io/esc_serialshot.c \ io/esc_serialshot.c \
io/frsky_osd.c \ io/frsky_osd.c \
@ -161,9 +162,7 @@ COMMON_SRC = \
cms/cms_menu_navigation.c \ cms/cms_menu_navigation.c \
cms/cms_menu_osd.c \ cms/cms_menu_osd.c \
cms/cms_menu_saveexit.c \ cms/cms_menu_saveexit.c \
cms/cms_menu_vtx_smartaudio.c \ cms/cms_menu_vtx.c \
cms/cms_menu_vtx_tramp.c \
cms/cms_menu_vtx_ffpv.c \
drivers/display_ug2864hsweg01.c \ drivers/display_ug2864hsweg01.c \
drivers/rangefinder/rangefinder_hcsr04.c \ drivers/rangefinder/rangefinder_hcsr04.c \
drivers/rangefinder/rangefinder_hcsr04_i2c.c \ drivers/rangefinder/rangefinder_hcsr04_i2c.c \

7
src/main/blackbox/blackbox.c
View file

@ -1693,10 +1693,9 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2); gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,

3
src/main/build/debug.h
View file

@ -67,5 +67,8 @@ typedef enum {
DEBUG_ERPM, DEBUG_ERPM,
DEBUG_RPM_FILTER, DEBUG_RPM_FILTER,
DEBUG_RPM_FREQ, DEBUG_RPM_FREQ,
DEBUG_NAV_YAW,
DEBUG_DYNAMIC_FILTER,
DEBUG_DYNAMIC_FILTER_FREQUENCY,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

20
src/main/cms/cms_menu_builtin.c
View file

@ -50,13 +50,6 @@
#include "cms/cms_menu_battery.h" #include "cms/cms_menu_battery.h"
#include "cms/cms_menu_misc.h" #include "cms/cms_menu_misc.h"
// VTX supplied menus
#include "cms/cms_menu_vtx_smartaudio.h"
#include "cms/cms_menu_vtx_tramp.h"
#include "cms/cms_menu_vtx_ffpv.h"
// Info // Info
static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1]; static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
@ -111,19 +104,8 @@ static const OSD_Entry menuFeaturesEntries[] =
#if defined(USE_NAV) #if defined(USE_NAV)
OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation), OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation),
#endif #endif
#if defined(VTX) || defined(USE_RTC6705)
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtx),
#endif // VTX || USE_RTC6705
#if defined(USE_VTX_CONTROL) #if defined(USE_VTX_CONTROL)
#if defined(USE_VTX_SMARTAUDIO) OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl),
OSD_SUBMENU_ENTRY("VTX SA", &cmsx_menuVtxSmartAudio),
#endif
#if defined(USE_VTX_TRAMP)
OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp),
#endif
#if defined(USE_VTX_FFPV)
OSD_SUBMENU_ENTRY("VTX FFPV", &cmsx_menuVtxFFPV),
#endif
#endif // VTX_CONTROL #endif // VTX_CONTROL
#ifdef USE_LED_STRIP #ifdef USE_LED_STRIP
OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip), OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip),

297
src/main/cms/cms_menu_vtx.c
View file

@ -15,132 +15,247 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <ctype.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <string.h>
#include "platform.h" #include "platform.h"
#include "build/version.h" #include "build/version.h"
#if defined(USE_CMS) && defined(USE_VTX_CONTROL)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h" #include "cms/cms_types.h"
#include "cms/cms_menu_vtx.h" #include "cms/cms_menu_vtx.h"
#include "config/feature.h" #include "drivers/vtx_common.h"
#ifdef USE_CMS #include "fc/config.h"
#if defined(VTX) || defined(USE_RTC6705) #include "io/vtx_string.h"
#include "io/vtx.h"
static bool featureRead = false;
static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;
static long cmsx_Vtx_FeatureRead(void) // Config-time settings
{ static uint8_t vtxBand = 0;
if (!featureRead) { static uint8_t vtxChan = 0;
cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0; static uint8_t vtxPower = 0;
featureRead = true; static uint8_t vtxPitMode = 0;
}
return 0; static const char * const vtxCmsPitModeNames[] = {
} "---", "OFF", "ON "
static long cmsx_Vtx_FeatureWriteback(void)
{
if (cmsx_featureVtx)
featureSet(FEATURE_VTX);
else
featureClear(FEATURE_VTX);
return 0;
}
static const char * const vtxBandNames[] = {
"A",
"B",
"E",
"F",
"R",
}; };
static const OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; // Menus (these are not const because we update them at run-time )
static const OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; static OSD_TAB_t cms_Vtx_EntBand = { &vtxBand, VTX_SETTINGS_BAND_COUNT, vtx58BandNames };
static OSD_TAB_t cms_Vtx_EntChan = { &vtxChan, VTX_SETTINGS_CHANNEL_COUNT, vtx58ChannelNames };
static OSD_TAB_t cms_Vtx_EntPower = { &vtxPower, VTX_SETTINGS_POWER_COUNT, vtx58DefaultPowerNames };
static const OSD_TAB_t cms_Vtx_EntPitMode = { &vtxPitMode, 2, vtxCmsPitModeNames };
static void cmsx_Vtx_ConfigRead(void) static long cms_Vtx_configPitMode(displayPort_t *pDisp, const void *self)
{
#ifdef VTX
cmsx_vtxBand = masterConfig.vtx_band;
cmsx_vtxChannel = masterConfig.vtx_channel + 1;
#endif // VTX
#ifdef USE_RTC6705
cmsx_vtxBand = masterConfig.vtx_channel / 8;
cmsx_vtxChannel = masterConfig.vtx_channel % 8 + 1;
#endif // USE_RTC6705
}
static void cmsx_Vtx_ConfigWriteback(void)
{
#ifdef VTX
masterConfig.vtx_band = cmsx_vtxBand;
masterConfig.vtx_channel = cmsx_vtxChannel - 1;
#endif // VTX
#ifdef USE_RTC6705
masterConfig.vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1;
#endif // USE_RTC6705
}
static long cmsx_Vtx_onEnter(void)
{
cmsx_Vtx_FeatureRead();
cmsx_Vtx_ConfigRead();
return 0;
}
static long cmsx_Vtx_onExit(const OSD_Entry *self)
{ {
UNUSED(pDisp);
UNUSED(self); UNUSED(self);
cmsx_Vtx_ConfigWriteback(); if (vtxPitMode == 0) {
vtxPitMode = 1;
}
// Pit mode changes are immediate, without saving
vtxCommonSetPitMode(vtxCommonDevice(), vtxPitMode >= 2 ? 1 : 0);
return 0; return 0;
} }
#ifdef VTX static long cms_Vtx_configBand(displayPort_t *pDisp, const void *self)
static const OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1};
static const OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1};
#endif // VTX
static const OSD_Entry cmsx_menuVtxEntries[] =
{ {
OSD_LABEL_ENTRY("--- VTX ---"), UNUSED(pDisp);
OSD_BOOL_ENTRY("ENABLED", &cmsx_featureVtx), UNUSED(self);
#ifdef VTX
OSD_UINT8_ENTRY("VTX MODE", &entryVtxMode), if (vtxBand == 0) {
OSD_UINT16_ENTRY("VTX MHZ", &entryVtxMhz), vtxBand = 1;
#endif // VTX }
OSD_TAB_ENTRY("BAND", &entryVtxBand), return 0;
OSD_UINT8_ENTRY("CHANNEL", &entryVtxChannel), }
#ifdef USE_RTC6705
OSD_BOOL_ENTRY("LOW POWER", &masterConfig.vtx_power), static long cms_Vtx_configChan(displayPort_t *pDisp, const void *self)
#endif // USE_RTC6705 {
UNUSED(pDisp);
UNUSED(self);
if (vtxChan == 0) {
vtxChan = 1;
}
return 0;
}
static long cms_Vtx_configPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (vtxPower == 0) {
vtxPower = 1;
}
return 0;
}
static void cms_Vtx_initSettings(void)
{
vtxDevice_t * vtxDevice = vtxCommonDevice();
vtxDeviceCapability_t vtxDeviceCapability;
if (vtxCommonGetDeviceCapability(vtxDevice, &vtxDeviceCapability)) {
cms_Vtx_EntBand.max = vtxDeviceCapability.bandCount;
cms_Vtx_EntBand.names = (const char * const *)vtxDeviceCapability.bandNames;
cms_Vtx_EntChan.max = vtxDeviceCapability.channelCount;
cms_Vtx_EntChan.names = (const char * const *)vtxDeviceCapability.channelNames;
cms_Vtx_EntPower.max = vtxDeviceCapability.powerCount;
cms_Vtx_EntPower.names = (const char * const *)vtxDeviceCapability.powerNames;
}
else {
cms_Vtx_EntBand.max = VTX_SETTINGS_BAND_COUNT;
cms_Vtx_EntBand.names = vtx58BandNames;
cms_Vtx_EntChan.max = VTX_SETTINGS_CHANNEL_COUNT;
cms_Vtx_EntChan.names = vtx58ChannelNames;
cms_Vtx_EntPower.max = VTX_SETTINGS_POWER_COUNT;
cms_Vtx_EntPower.names = vtx58DefaultPowerNames;
}
vtxBand = vtxSettingsConfig()->band;
vtxChan = vtxSettingsConfig()->channel;
vtxPower = vtxSettingsConfig()->power;
// If device is ready - read actual PIT mode
if (vtxCommonDeviceIsReady(vtxDevice)) {
uint8_t onoff;
vtxCommonGetPitMode(vtxDevice, &onoff);
vtxPitMode = onoff ? 2 : 1;
}
else {
vtxPitMode = vtxSettingsConfig()->lowPowerDisarm ? 2 : 1;
}
}
static long cms_Vtx_onEnter(const OSD_Entry *self)
{
UNUSED(self);
cms_Vtx_initSettings();
return 0;
}
static long cms_Vtx_Commence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxBand, vtxChan);
vtxCommonSetPowerByIndex(vtxCommonDevice(), vtxPower);
vtxCommonSetPitMode(vtxCommonDevice(), vtxPitMode == 2 ? 1 : 0);
vtxSettingsConfigMutable()->band = vtxBand;
vtxSettingsConfigMutable()->channel = vtxChan;
vtxSettingsConfigMutable()->power = vtxPower;
vtxSettingsConfigMutable()->lowPowerDisarm = (vtxPitMode == 2 ? 1 : 0);
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static bool cms_Vtx_drawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "-- ---- ----";
// bc ffff pppp
// 012345678901
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t * vtxDevice = vtxCommonDevice();
vtxDeviceOsdInfo_t osdInfo;
if (!vtxDevice || !vtxCommonGetOsdInfo(vtxDevice, &osdInfo) || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = osdInfo.bandLetter;
buf[1] = osdInfo.channelName[0];
buf[2] = ' ';
if (osdInfo.frequency)
tfp_sprintf(&buf[3], "%4d", osdInfo.frequency);
else
tfp_sprintf(&buf[3], "----");
if (osdInfo.powerIndex) {
// If OSD driver provides power in milliwatt - display MW, otherwise - power level
if (osdInfo.powerMilliwatt) {
tfp_sprintf(&buf[7], " %4d", osdInfo.powerMilliwatt);
}
else {
tfp_sprintf(&buf[7], " PL=%c", osdInfo.powerIndex);
}
} else {
tfp_sprintf(&buf[7], " ----");
}
return true;
}
static const OSD_Entry cms_menuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", cms_Vtx_Commence),
OSD_BACK_AND_END_ENTRY, OSD_BACK_AND_END_ENTRY,
}; };
const CMS_Menu cmsx_menuVtx = { static const CMS_Menu cms_menuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = cms_menuCommenceEntries,
};
static const OSD_Entry cms_menuVtxEntries[] =
{
OSD_LABEL_ENTRY("--- VTX ---"),
OSD_LABEL_FUNC_DYN_ENTRY("", cms_Vtx_drawStatusString),
OSD_TAB_CALLBACK_ENTRY("PIT", cms_Vtx_configPitMode, &cms_Vtx_EntPitMode),
OSD_TAB_CALLBACK_ENTRY("BAND", cms_Vtx_configBand, &cms_Vtx_EntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", cms_Vtx_configChan, &cms_Vtx_EntChan),
OSD_TAB_CALLBACK_ENTRY("POWER", cms_Vtx_configPower, &cms_Vtx_EntPower),
OSD_SUBMENU_ENTRY("SET", &cms_menuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxControl = {
#ifdef CMS_MENU_DEBUG #ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUVTX", .GUARD_text = "MENUVTX",
.GUARD_type = OME_MENU, .GUARD_type = OME_MENU,
#endif #endif
.onEnter = cmsx_Vtx_onEnter, .onEnter = cms_Vtx_onEnter,
.onExit= cmsx_Vtx_onExit, .onExit = NULL,
.onGlobalExit = cmsx_Vtx_FeatureWriteback, .onGlobalExit = NULL,
.entries = cmsx_menuVtxEntries .entries = cms_menuVtxEntries
}; };
#endif // VTX || USE_RTC6705
#endif // CMS #endif // CMS

2
src/main/cms/cms_menu_vtx.h
View file

@ -17,4 +17,4 @@
#pragma once #pragma once
extern const CMS_Menu cmsx_menuVtx; extern const CMS_Menu cmsx_menuVtxControl;

214
src/main/cms/cms_menu_vtx_ffpv.c
View file

@ -1,214 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_FFPV)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_ffpv24g.h"
#include "io/vtx.h"
static bool ffpvCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ---";
// m bc ffff ppp
// 01234567890123
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_FFPV || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = '*';
buf[1] = ' ';
buf[2] = ffpvBandLetters[ffpvGetRuntimeState()->band];
buf[3] = ffpvChannelNames[ffpvGetRuntimeState()->channel][0];
buf[4] = ' ';
tfp_sprintf(&buf[5], "%4d", ffpvGetRuntimeState()->frequency);
tfp_sprintf(&buf[9], " %3d", ffpvGetRuntimeState()->powerMilliwatt);
return true;
}
uint8_t ffpvCmsBand = 1;
uint8_t ffpvCmsChan = 1;
uint16_t ffpvCmsFreqRef;
static uint8_t ffpvCmsPower = 1;
static const OSD_TAB_t ffpvCmsEntBand = { &ffpvCmsBand, VTX_FFPV_BAND_COUNT, ffpvBandNames };
static const OSD_TAB_t ffpvCmsEntChan = { &ffpvCmsChan, VTX_FFPV_CHANNEL_COUNT, ffpvChannelNames };
static const OSD_TAB_t ffpvCmsEntPower = { &ffpvCmsPower, VTX_FFPV_POWER_COUNT, ffpvPowerNames };
static void ffpvCmsUpdateFreqRef(void)
{
if (ffpvCmsBand > 0 && ffpvCmsChan > 0) {
ffpvCmsFreqRef = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
}
}
static long ffpvCmsConfigBand(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsBand == 0) {
// Bounce back
ffpvCmsBand = 1;
}
else {
ffpvCmsUpdateFreqRef();
}
return 0;
}
static long ffpvCmsConfigChan(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsChan == 0) {
// Bounce back
ffpvCmsChan = 1;
}
else {
ffpvCmsUpdateFreqRef();
}
return 0;
}
static long ffpvCmsConfigPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (ffpvCmsPower == 0) {
// Bounce back
ffpvCmsPower = 1;
}
return 0;
}
static long ffpvCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
// call driver directly
ffpvSetBandAndChannel(ffpvCmsBand, ffpvCmsChan);
ffpvSetRFPowerByIndex(ffpvCmsPower);
// update'vtx_' settings
vtxSettingsConfigMutable()->band = ffpvCmsBand;
vtxSettingsConfigMutable()->channel = ffpvCmsChan;
vtxSettingsConfigMutable()->power = ffpvCmsPower;
vtxSettingsConfigMutable()->freq = ffpvFrequencyTable[ffpvCmsBand - 1][ffpvCmsChan - 1];
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static void ffpvCmsInitSettings(void)
{
ffpvCmsBand = ffpvGetRuntimeState()->band;
ffpvCmsChan = ffpvGetRuntimeState()->channel;
ffpvCmsPower = ffpvGetRuntimeState()->powerIndex;
ffpvCmsUpdateFreqRef();
}
static long ffpvCmsOnEnter(const OSD_Entry *from)
{
UNUSED(from);
ffpvCmsInitSettings();
return 0;
}
static const OSD_Entry ffpvCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu ffpvCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = ffpvCmsMenuCommenceEntries,
};
static const OSD_Entry ffpvMenuEntries[] =
{
OSD_LABEL_ENTRY("- TRAMP -"),
OSD_LABEL_FUNC_DYN_ENTRY("", ffpvCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("BAND", ffpvCmsConfigBand, &ffpvCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", ffpvCmsConfigChan, &ffpvCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &ffpvCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxFFPV = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTR",
.GUARD_type = OME_MENU,
#endif
.onEnter = ffpvCmsOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = ffpvMenuEntries,
};
#endif

23
src/main/cms/cms_menu_vtx_ffpv.h
View file

@ -1,23 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cms/cms.h"
#include "cms/cms_types.h"
extern const CMS_Menu cmsx_menuVtxFFPV;

710
src/main/cms/cms_menu_vtx_smartaudio.c
View file

@ -1,710 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_SMARTAUDIO)
#include "common/log.h"
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_vtx_smartaudio.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx.h"
// 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/channel mode)
uint16_t saCmsFreqRef = 0;
uint16_t saCmsDeviceFreq = 0;
uint8_t saCmsDeviceStatus = 0;
uint8_t saCmsPower;
uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2)
uint8_t saCmsFselMode; // Channel(0) or User defined(1)
uint8_t saCmsFselModeNew; // 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
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 bool saCmsUpdateCopiedState(void)
{
if (saDevice.version == 0)
return false;
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.version == 2) {
if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE)
saCmsPitFMode = 1;
else
saCmsPitFMode = 0;
}
return true;
}
static bool saCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ---";
// m bc ffff ppp
// 0123456789012
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
if (!saCmsUpdateCopiedState()) {
// VTX is not ready
return true;
}
buf[0] = "-FR"[saCmsOpmodel];
if (saCmsFselMode == 0) {
buf[2] = "ABEFR"[saDevice.channel / 8];
buf[3] = '1' + (saDevice.channel % 8);
} else {
buf[2] = 'U';
buf[3] = 'F';
}
if ((saDevice.mode & SA_MODE_GET_PITMODE)
&& (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE))
tfp_sprintf(&buf[5], "%4d", saDevice.orfreq);
else if (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ)
tfp_sprintf(&buf[5], "%4d", saDevice.freq);
else
tfp_sprintf(&buf[5], "%4d",
vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]);
buf[9] = ' ';
if (saDevice.mode & SA_MODE_GET_PITMODE) {
buf[10] = 'P';
if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
buf[11] = 'I';
} else {
buf[11] = 'O';
}
buf[12] = 'R';
buf[13] = 0;
} else {
tfp_sprintf(&buf[10], "%3d", (saDevice.version == 2) ? saPowerTable[saDevice.power].rfpower : saPowerTable[saDacToPowerIndex(saDevice.power)].rfpower);
}
return true;
}
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 = vtxSettingsConfig()->band;
saCmsChan = vtxSettingsConfig()->channel;
saCmsFreqRef = vtxSettingsConfig()->freq;
saCmsDeviceFreq = saCmsFreqRef;
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;
}
saCmsPower = vtxSettingsConfig()->power;
// if user-freq mode then track possible change
if (saCmsFselMode && vtxSettingsConfig()->freq) {
saCmsUserFreq = vtxSettingsConfig()->freq;
}
saCmsFselModeNew = saCmsFselMode; //init mode for menu
}
saCmsUpdateCopiedState();
}
void saCmsResetOpmodel()
{
// trigger data refresh in 'saCmsUpdate()'
saCmsOpmodel = SACMS_OPMODEL_UNDEF;
}
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)
saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58frequencyTable[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)
saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1);
saCmsFreqRef = vtx58frequencyTable[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 && !saDeferred) {
vtxSettingsConfigMutable()->power = saCmsPower;
}
return 0;
}
static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 1) {
// V1 device doesn't support PIT mode; bounce back.
saCmsPitFMode = 0;
return 0;
}
LOG_D(VTX, "saCmsConfigPitFmodeByGbar: saCmsPitFMode %d", saCmsPitFMode);
if (saCmsPitFMode == 0) {
// Bounce back
saCmsPitFMode = 1;
return 0;
}
if (saCmsPitFMode == 1) {
saSetMode(SA_MODE_SET_IN_RANGE_PITMODE);
} else {
saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE);
}
return 0;
}
static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self); // Forward
static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (saDevice.version == 1) {
if (saCmsOpmodel != SACMS_OPMODEL_FREE)
saCmsOpmodel = SACMS_OPMODEL_FREE;
return 0;
}
uint8_t opmodel = saCmsOpmodel;
LOG_D(VTX, "saCmsConfigOpmodelByGvar: opmodel %d", 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);
// Direct frequency mode is not available in RACE opmodel
saCmsFselModeNew = 0;
saCmsConfigFreqModeByGvar(pDisp, self);
} else {
// Trying to go back to unknown state; bounce back
saCmsOpmodel = SACMS_OPMODEL_UNDEF + 1;
}
return 0;
}
#ifdef USE_EXTENDED_CMS_MENUS
static const char * const saCmsDeviceStatusNames[] = {
"OFFL",
"ONL V1",
"ONL V2",
};
static const OSD_TAB_t saCmsEntOnline = { &saCmsDeviceStatus, 2, saCmsDeviceStatusNames };
static const OSD_Entry saCmsMenuStatsEntries[] = {
OSD_LABEL_ENTRY("- SA STATS -"),
OSD_TAB_DYN_ENTRY("STATUS", &saCmsEntOnline),
OSD_UINT16_RO_ENTRY("BAUDRATE", &sa_smartbaud),
OSD_UINT16_RO_ENTRY("SENT", &saStat.pktsent),
OSD_UINT16_RO_ENTRY("RCVD", &saStat.pktrcvd),
OSD_UINT16_RO_ENTRY("BADPRE", &saStat.badpre),
OSD_UINT16_RO_ENTRY("BADLEN", &saStat.badlen),
OSD_UINT16_RO_ENTRY("CRCERR", &saStat.crc),
OSD_UINT16_RO_ENTRY("OOOERR", &saStat.ooopresp),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuStats = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAST",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuStatsEntries
};
#endif /* USE_EXTENDED_CMS_MENUS */
static const OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames };
static const OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames };
static const OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saPowerNames};
static const char * const saCmsOpmodelNames[] = {
"----",
"FREE",
"RACE",
};
static const char * const saCmsFselModeNames[] = {
"CHAN",
"USER"
};
static const char * const saCmsPitFModeNames[] = {
"---",
"PIR",
"POR"
};
static const OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
static long sacms_SetupTopMenu(const OSD_Entry *from); // Forward
static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
// if trying to do user frequency mode in RACE opmodel then
// revert because user-freq only available in FREE opmodel
if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) {
saCmsFselModeNew = 0;
}
// don't call 'saSetBandAndChannel()' / 'saSetFreq()' here,
// wait until SET / 'saCmsCommence()' is activated
sacms_SetupTopMenu(NULL);
return 0;
}
static long saCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
const vtxSettingsConfig_t prevSettings = {
.band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel,
.freq = vtxSettingsConfig()->freq,
.power = vtxSettingsConfig()->power,
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
};
vtxSettingsConfig_t newSettings = prevSettings;
if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
// Race model
// Setup band, freq and power.
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
// 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 (saCmsFselModeNew == 0) {
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
} else {
saSetMode(0); //make sure FREE mode is setup
newSettings.band = 0;
newSettings.freq = saCmsUserFreq;
}
}
newSettings.power = saCmsPower;
if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) {
vtxSettingsConfigMutable()->band = newSettings.band;
vtxSettingsConfigMutable()->channel = newSettings.channel;
vtxSettingsConfigMutable()->power = newSettings.power;
vtxSettingsConfigMutable()->freq = newSettings.freq;
saveConfigAndNotify();
}
return MENU_CHAIN_BACK;
}
static long saCmsSetPORFreqOnEnter(const OSD_Entry *from)
{
UNUSED(from);
if (saDevice.version == 1)
return MENU_CHAIN_BACK;
saCmsORFreqNew = saCmsORFreq;
return 0;
}
static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
saSetPitFreq(saCmsORFreqNew);
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(const OSD_Entry *from)
{
UNUSED(from);
saCmsUserFreqNew = saCmsUserFreq;
return 0;
}
static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
saCmsUserFreq = saCmsUserFreqNew;
return MENU_CHAIN_BACK;
}
static const OSD_Entry saCmsMenuPORFreqEntries[] =
{
OSD_LABEL_ENTRY("- POR FREQ -"),
OSD_UINT16_RO_ENTRY("CUR FREQ", &saCmsORFreq),
OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 })),
OSD_FUNC_CALL_ENTRY("SET", saCmsSetPORFreq),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuPORFreq =
{
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAPOR",
.GUARD_type = OME_MENU,
#endif
.onEnter = saCmsSetPORFreqOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuPORFreqEntries,
};
static const OSD_Entry saCmsMenuUserFreqEntries[] =
{
OSD_LABEL_ENTRY("- USER FREQ -"),
OSD_UINT16_RO_ENTRY("CUR FREQ", &saCmsUserFreq),
OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 })),
OSD_FUNC_CALL_ENTRY("SET", saCmsConfigUserFreq),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuUserFreq =
{
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSAUFQ",
.GUARD_type = OME_MENU,
#endif
.onEnter = saCmsSetUserFreqOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuUserFreqEntries,
};
static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames };
static const OSD_Entry saCmsMenuConfigEntries[] =
{
OSD_LABEL_ENTRY("- SA CONFIG -"),
{ "OP MODEL", {.func = saCmsConfigOpmodelByGvar}, &(const OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, OME_TAB, DYNAMIC },
{ "FSEL MODE", {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, OME_TAB, DYNAMIC },
OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode),
{ "POR FREQ", {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OME_Submenu, OPTSTRING },
#ifdef USE_EXTENDED_CMS_MENUS
OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
#endif /* USE_EXTENDED_CMS_MENUS */
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuConfig = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XSACFG",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuConfigEntries
};
static const OSD_Entry saCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", saCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu saCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXCOM",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuCommenceEntries,
};
#pragma GCC diagnostic push
#if (__GNUC__ > 7)
// This is safe on 32bit platforms, suppress warning for saCmsUserFreqGetString
#pragma GCC diagnostic ignored "-Wcast-function-type"
#endif
static const OSD_Entry saCmsMenuFreqModeEntries[] =
{
OSD_LABEL_ENTRY("- SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
{ "FREQ", {.menufunc = saCmsUserFreqGetString}, &saCmsMenuUserFreq, OME_Submenu, OPTSTRING },
OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence),
OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig),
OSD_BACK_AND_END_ENTRY,
};
#pragma GCC diagnostic pop
static const OSD_Entry saCmsMenuChanModeEntries[] =
{
OSD_LABEL_ENTRY("- SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("BAND", saCmsConfigBandByGvar, &saCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", saCmsConfigChanByGvar, &saCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &saCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower),
OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence),
OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig),
OSD_BACK_AND_END_ENTRY,
};
static const OSD_Entry saCmsMenuOfflineEntries[] =
{
OSD_LABEL_ENTRY("- VTX SMARTAUDIO -"),
OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString),
#ifdef USE_EXTENDED_CMS_MENUS
OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats),
#endif /* USE_EXTENDED_CMS_MENUS */
OSD_BACK_AND_END_ENTRY,
};
CMS_Menu cmsx_menuVtxSmartAudio; // Forward
static long sacms_SetupTopMenu(const OSD_Entry *from)
{
UNUSED(from);
if (saCmsDeviceStatus) {
if (saCmsFselModeNew == 0)
cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
else
cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries;
} else {
cmsx_menuVtxSmartAudio.entries = saCmsMenuOfflineEntries;
}
return 0;
}
CMS_Menu cmsx_menuVtxSmartAudio = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXSA",
.GUARD_type = OME_MENU,
#endif
.onEnter = sacms_SetupTopMenu,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = saCmsMenuOfflineEntries,
};
#endif // CMS

29
src/main/cms/cms_menu_vtx_smartaudio.h
View file

@ -1,29 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cms/cms.h"
#include "cms/cms_types.h"
extern CMS_Menu cmsx_menuVtxSmartAudio;
void saCmsUpdate(void);
void saCmsResetOpmodel(void);

254
src/main/cms/cms_menu_vtx_tramp.c
View file

@ -1,254 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(USE_CMS) && defined(USE_VTX_TRAMP)
#include "common/printf.h"
#include "common/utils.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "io/vtx_string.h"
#include "io/vtx_tramp.h"
#include "io/vtx.h"
static bool trampCmsDrawStatusString(char *buf, unsigned bufsize)
{
const char *defaultString = "- -- ---- ----";
// m bc ffff tppp
// 01234567890123
if (bufsize < strlen(defaultString) + 1) {
return false;
}
strcpy(buf, defaultString);
vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_TRAMP || !vtxCommonDeviceIsReady(vtxDevice)) {
return true;
}
buf[0] = '*';
buf[1] = ' ';
buf[2] = vtx58BandLetter[trampData.band];
buf[3] = vtx58ChannelNames[trampData.channel][0];
buf[4] = ' ';
if (trampData.curFreq)
tfp_sprintf(&buf[5], "%4d", trampData.curFreq);
else
tfp_sprintf(&buf[5], "----");
if (trampData.power) {
tfp_sprintf(&buf[9], " %c%3d", (trampData.power == trampData.configuredPower) ? ' ' : '*', trampData.power);
} else {
tfp_sprintf(&buf[9], " ----");
}
return true;
}
uint8_t trampCmsPitMode = 0;
uint8_t trampCmsBand = 1;
uint8_t trampCmsChan = 1;
uint16_t trampCmsFreqRef;
static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames };
static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames };
static uint8_t trampCmsPower = 1;
static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, VTX_TRAMP_POWER_COUNT, trampPowerNames };
static void trampCmsUpdateFreqRef(void)
{
if (trampCmsBand > 0 && trampCmsChan > 0)
trampCmsFreqRef = vtx58frequencyTable[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 long trampCmsConfigPower(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
if (trampCmsPower == 0)
// Bounce back
trampCmsPower = 1;
return 0;
}
static const char * const trampCmsPitModeNames[] = {
"---", "OFF", "ON "
};
static const OSD_TAB_t trampCmsEntPitMode = { &trampCmsPitMode, 2, trampCmsPitModeNames };
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);
trampSetBandAndChannel(trampCmsBand, trampCmsChan);
trampSetRFPower(trampPowerTable[trampCmsPower-1]);
// If it fails, the user should retry later
trampCommitChanges();
// update'vtx_' settings
vtxSettingsConfigMutable()->band = trampCmsBand;
vtxSettingsConfigMutable()->channel = trampCmsChan;
vtxSettingsConfigMutable()->power = trampCmsPower;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(trampCmsBand, trampCmsChan);
saveConfigAndNotify();
return MENU_CHAIN_BACK;
}
static void trampCmsInitSettings(void)
{
if (trampData.band > 0) trampCmsBand = trampData.band;
if (trampData.channel > 0) trampCmsChan = trampData.channel;
trampCmsUpdateFreqRef();
trampCmsPitMode = trampData.pitMode + 1;
if (trampData.configuredPower > 0) {
for (uint8_t i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
if (trampData.configuredPower <= trampPowerTable[i]) {
trampCmsPower = i + 1;
break;
}
}
}
}
static long trampCmsOnEnter(const OSD_Entry *from)
{
UNUSED(from);
trampCmsInitSettings();
return 0;
}
static const OSD_Entry trampCmsMenuCommenceEntries[] =
{
OSD_LABEL_ENTRY("CONFIRM"),
OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence),
OSD_BACK_AND_END_ENTRY,
};
static const CMS_Menu trampCmsMenuCommence = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTRC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = trampCmsMenuCommenceEntries,
};
static const OSD_Entry trampMenuEntries[] =
{
OSD_LABEL_ENTRY("- TRAMP -"),
OSD_LABEL_FUNC_DYN_ENTRY("", trampCmsDrawStatusString),
OSD_TAB_CALLBACK_ENTRY("PIT", trampCmsSetPitMode, &trampCmsEntPitMode),
OSD_TAB_CALLBACK_ENTRY("BAND", trampCmsConfigBand, &trampCmsEntBand),
OSD_TAB_CALLBACK_ENTRY("CHAN", trampCmsConfigChan, &trampCmsEntChan),
OSD_UINT16_RO_ENTRY("(FREQ)", &trampCmsFreqRef),
OSD_TAB_CALLBACK_ENTRY("POWER", trampCmsConfigPower, &trampCmsEntPower),
OSD_INT16_RO_ENTRY("T(C)", &trampData.temperature),
OSD_SUBMENU_ENTRY("SET", &trampCmsMenuCommence),
OSD_BACK_AND_END_ENTRY,
};
const CMS_Menu cmsx_menuVtxTramp = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XVTXTR",
.GUARD_type = OME_MENU,
#endif
.onEnter = trampCmsOnEnter,
.onExit = NULL,
.onGlobalExit = NULL,
.entries = trampMenuEntries,
};
#endif

23
src/main/cms/cms_menu_vtx_tramp.h
View file

@ -1,23 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cms/cms.h"
#include "cms/cms_types.h"
extern const CMS_Menu cmsx_menuVtxTramp;

39
src/main/common/logic_condition.c
View file

@ -40,6 +40,9 @@
#include "sensors/pitotmeter.h" #include "sensors/pitotmeter.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0); PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS]; logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
@ -232,6 +235,42 @@ static int logicConditionGetFlightOperandValue(int operand) {
return constrain(attitude.values.pitch / 10, -180, 180); return constrain(attitude.values.pitch / 10, -180, 180);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
return ARMING_FLAG(ARMED) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
return (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) ? 1 : 0;
break;
default: default:
return 0; return 0;
break; break;

43
src/main/common/logic_condition.h
View file

@ -56,23 +56,32 @@ typedef enum logicOperandType_s {
} logicOperandType_e; } logicOperandType_e;
typedef enum { typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s // 0
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m // 1
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m // 2
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI, LOGIC_CONDITION_OPERAND_FLIGHT_RSSI, // 3
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10 LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10 // 4
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10 LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10 // 5
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100 LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100 // 6
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh // 7
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS, LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS, // 8
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s // 9
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s // 10
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s // 11
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm // 12
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s // 13
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // % LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // % // 14
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg // 15
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg // 16
LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED, // 0/1 // 17
LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH, // 0/1 // 18
LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL, // 0/1 // 19
LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1 // 20
LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1 // 21
LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1 // 22
LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1 // 23
LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 24
LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 25
} logicFlightOperands_e; } logicFlightOperands_e;
typedef enum { typedef enum {

6
src/main/drivers/system.c
View file

@ -49,9 +49,9 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
} }
// cycles per microsecond // cycles per microsecond
STATIC_UNIT_TESTED timeUs_t usTicks = 0; STATIC_UNIT_TESTED EXTENDED_FASTRAM timeUs_t usTicks = 0;
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0; STATIC_UNIT_TESTED EXTENDED_FASTRAM volatile timeMs_t sysTickUptime = 0;
STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0; STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
// cached value of RCC->CSR // cached value of RCC->CSR
uint32_t cachedRccCsrValue; uint32_t cachedRccCsrValue;
@ -76,7 +76,7 @@ void cycleCounterInit(void)
// SysTick // SysTick
static volatile int sysTickPending = 0; static EXTENDED_FASTRAM volatile int sysTickPending = 0;
void SysTick_Handler(void) void SysTick_Handler(void)
{ {

39
src/main/drivers/vtx_common.c
View file

@ -103,13 +103,6 @@ void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
} }
} }
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
{
if (vtxDevice && vtxDevice->vTable->setFrequency) {
vtxDevice->vTable->setFrequency(vtxDevice, frequency);
}
}
bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (vtxDevice && vtxDevice->vTable->getBandAndChannel) { if (vtxDevice && vtxDevice->vTable->getBandAndChannel) {
@ -150,3 +143,35 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t
} }
return false; return false;
} }
bool vtxCommonGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
if (vtxDevice && vtxDevice->vTable->getPower) {
return vtxDevice->vTable->getPower(vtxDevice, pIndex, pPowerMw);
}
return false;
}
bool vtxCommonGetOsdInfo(vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
bool ret = false;
if (vtxDevice && vtxDevice->vTable->getOsdInfo) {
ret = vtxDevice->vTable->getOsdInfo(vtxDevice, pOsdInfo);
}
// Make sure we provide sane results even in case API fails
if (!ret) {
pOsdInfo->band = 0;
pOsdInfo->channel = 0;
pOsdInfo->frequency = 0;
pOsdInfo->powerIndex = 0;
pOsdInfo->powerMilliwatt = 0;
pOsdInfo->bandLetter = '-';
pOsdInfo->bandName = "-";
pOsdInfo->channelName = "-";
pOsdInfo->powerIndexLetter = '0';
}
return ret;
}

46
src/main/drivers/vtx_common.h
View file

@ -30,19 +30,12 @@
#define VTX_SETTINGS_DEFAULT_BAND 4 #define VTX_SETTINGS_DEFAULT_BAND 4
#define VTX_SETTINGS_DEFAULT_CHANNEL 1 #define VTX_SETTINGS_DEFAULT_CHANNEL 1
#define VTX_SETTINGS_DEFAULT_FREQ 5740 #define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1
#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0
#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 #define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0
#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting #define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting
#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting
#if defined(USE_VTX_RTC6705)
#include "drivers/vtx_rtc6705.h"
#endif
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
#define VTX_SETTINGS_POWER_COUNT 5 #define VTX_SETTINGS_POWER_COUNT 5
@ -53,13 +46,6 @@
#define VTX_SETTINGS_FREQCMD #define VTX_SETTINGS_FREQCMD
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) #define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
#elif defined(USE_VTX_RTC6705)
#define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT
#define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER
#define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1)
#endif #endif
// check value for MSP_SET_VTX_CONFIG to determine if value is encoded // check value for MSP_SET_VTX_CONFIG to determine if value is encoded
@ -68,7 +54,7 @@
typedef enum { typedef enum {
VTXDEV_UNSUPPORTED = 0, // reserved for MSP VTXDEV_UNSUPPORTED = 0, // reserved for MSP
VTXDEV_RTC6705 = 1, VTXDEV_RTC6705 = 1, // deprecated
// 2 reserved // 2 reserved
VTXDEV_SMARTAUDIO = 3, VTXDEV_SMARTAUDIO = 3,
VTXDEV_TRAMP = 4, VTXDEV_TRAMP = 4,
@ -82,23 +68,32 @@ typedef struct vtxDeviceCapability_s {
uint8_t bandCount; uint8_t bandCount;
uint8_t channelCount; uint8_t channelCount;
uint8_t powerCount; uint8_t powerCount;
char **bandNames;
char **channelNames;
char **powerNames;
} vtxDeviceCapability_t; } vtxDeviceCapability_t;
typedef struct vtxDeviceOsdInfo_s {
int band;
int channel;
int frequency;
int powerIndex;
int powerMilliwatt;
char bandLetter;
const char * bandName;
const char * channelName;
char powerIndexLetter;
} vtxDeviceOsdInfo_t;
typedef struct vtxDevice_s { typedef struct vtxDevice_s {
const struct vtxVTable_s * const vTable; const struct vtxVTable_s * const vTable;
vtxDeviceCapability_t capability; vtxDeviceCapability_t capability;
uint16_t *frequencyTable; // Array of [bandCount][channelCount]
char **bandNames; // char *bandNames[bandCount]
char **channelNames; // char *channelNames[channelCount]
char **powerNames; // char *powerNames[powerCount]
uint8_t band; // Band = 1, 1-based uint8_t band; // Band = 1, 1-based
uint8_t channel; // CH1 = 1, 1-based uint8_t channel; // CH1 = 1, 1-based
uint8_t powerIndex; // Lowest/Off = 0 uint8_t powerIndex; // Lowest/Off = 0
uint8_t pitMode; // 0 = non-PIT, 1 = PIT uint8_t pitMode; // 0 = non-PIT, 1 = PIT
} vtxDevice_t; } vtxDevice_t;
// {set,get}BandAndChannel: band and channel are 1 origin // {set,get}BandAndChannel: band and channel are 1 origin
@ -113,12 +108,14 @@ typedef struct vtxVTable_s {
void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level); void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level);
void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff); void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff);
void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq);
bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex); bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool (*getPower)(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw);
bool (*getOsdInfo)(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo);
} vtxVTable_t; } vtxVTable_t;
// 3.1.0 // 3.1.0
@ -137,9 +134,10 @@ bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice);
void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index); void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index);
void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency);
bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex); bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff); bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability); bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability);
bool vtxCommonGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw);
bool vtxCommonGetOsdInfo(vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo);

264
src/main/drivers/vtx_rtc6705.c
View file

@ -1,264 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI)
#include "common/maths.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/vtx_rtc6705.h"
#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
#define RTC6705_SET_A1 0x8F3031 //5865
#define RTC6705_SET_A2 0x8EB1B1 //5845
#define RTC6705_SET_A3 0x8E3331 //5825
#define RTC6705_SET_A4 0x8DB4B1 //5805
#define RTC6705_SET_A5 0x8D3631 //5785
#define RTC6705_SET_A6 0x8CB7B1 //5765
#define RTC6705_SET_A7 0x8C4131 //5745
#define RTC6705_SET_A8 0x8BC2B1 //5725
#define RTC6705_SET_B1 0x8BF3B1 //5733
#define RTC6705_SET_B2 0x8C6711 //5752
#define RTC6705_SET_B3 0x8CE271 //5771
#define RTC6705_SET_B4 0x8D55D1 //5790
#define RTC6705_SET_B5 0x8DD131 //5809
#define RTC6705_SET_B6 0x8E4491 //5828
#define RTC6705_SET_B7 0x8EB7F1 //5847
#define RTC6705_SET_B8 0x8F3351 //5866
#define RTC6705_SET_E1 0x8B4431 //5705
#define RTC6705_SET_E2 0x8AC5B1 //5685
#define RTC6705_SET_E3 0x8A4731 //5665
#define RTC6705_SET_E4 0x89D0B1 //5645
#define RTC6705_SET_E5 0x8FA6B1 //5885
#define RTC6705_SET_E6 0x902531 //5905
#define RTC6705_SET_E7 0x90A3B1 //5925
#define RTC6705_SET_E8 0x912231 //5945
#define RTC6705_SET_F1 0x8C2191 //5740
#define RTC6705_SET_F2 0x8CA011 //5760
#define RTC6705_SET_F3 0x8D1691 //5780
#define RTC6705_SET_F4 0x8D9511 //5800
#define RTC6705_SET_F5 0x8E1391 //5820
#define RTC6705_SET_F6 0x8E9211 //5840
#define RTC6705_SET_F7 0x8F1091 //5860
#define RTC6705_SET_F8 0x8F8711 //5880
#define RTC6705_SET_R1 0x8A2151 //5658
#define RTC6705_SET_R2 0x8B04F1 //5695
#define RTC6705_SET_R3 0x8BF091 //5732
#define RTC6705_SET_R4 0x8CD431 //5769
#define RTC6705_SET_R5 0x8DB7D1 //5806
#define RTC6705_SET_R6 0x8EA371 //5843
#define RTC6705_SET_R7 0x8F8711 //5880
#define RTC6705_SET_R8 0x9072B1 //5917
#define RTC6705_SET_R 400 //Reference clock
#define RTC6705_SET_FDIV 1024 //128*(fosc/1000000)
#define RTC6705_SET_NDIV 16 //Remainder divider to get 'A' part of equation
#define RTC6705_SET_WRITE 0x11 //10001b to write to register
#define RTC6705_SET_DIVMULT 1000000 //Division value (to fit into a uint32_t) (Hz to MHz)
#ifdef RTC6705_POWER_PIN
static IO_t vtxPowerPin = IO_NONE;
#endif
static IO_t vtxCSPin = IO_NONE;
#define DISABLE_RTC6705() IOHi(vtxCSPin)
#ifdef USE_RTC6705_CLK_HACK
static IO_t vtxCLKPin = IO_NONE;
// HACK for missing pull up on CLK line - drive the CLK high *before* enabling the CS pin.
#define ENABLE_RTC6705() {IOHi(vtxCLKPin); delayMicroseconds(5); IOLo(vtxCSPin); }
#else
#define ENABLE_RTC6705() IOLo(vtxCSPin)
#endif
#define DP_5G_MASK 0x7000 // b111000000000000
#define PA5G_BS_MASK 0x0E00 // b000111000000000
#define PA5G_PW_MASK 0x0180 // b000000110000000
#define PD_Q5G_MASK 0x0040 // b000000001000000
#define QI_5G_MASK 0x0038 // b000000000111000
#define PA_BS_MASK 0x0007 // b000000000000111
#define PA_CONTROL_DEFAULT 0x4FBD
#define RTC6705_RW_CONTROL_BIT (1 << 4)
#define RTC6705_ADDRESS (0x07)
#define ENABLE_VTX_POWER() IOLo(vtxPowerPin)
#define DISABLE_VTX_POWER() IOHi(vtxPowerPin)
// Define variables
static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = {
{ RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
{ RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
{ RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
{ RTC6705_SET_F1, RTC6705_SET_F2, RTC6705_SET_F3, RTC6705_SET_F4, RTC6705_SET_F5, RTC6705_SET_F6, RTC6705_SET_F7, RTC6705_SET_F8 },
{ RTC6705_SET_R1, RTC6705_SET_R2, RTC6705_SET_R3, RTC6705_SET_R4, RTC6705_SET_R5, RTC6705_SET_R6, RTC6705_SET_R7, RTC6705_SET_R8 },
};
/**
* Reverse a uint32_t (LSB to MSB)
* This is easier for when generating the frequency to then
* reverse the bits afterwards
*/
static uint32_t reverse32(uint32_t in)
{
uint32_t out = 0;
for (uint8_t i = 0 ; i < 32 ; i++)
{
out |= ((in>>i) & 1)<<(31-i);
}
return out;
}
/**
* Start chip if available
*/
void rtc6705IOInit(void)
{
#ifdef RTC6705_POWER_PIN
vtxPowerPin = IOGetByTag(IO_TAG(RTC6705_POWER_PIN));
IOInit(vtxPowerPin, OWNER_VTX, RESOURCE_OUTPUT, 0);
DISABLE_VTX_POWER();
IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
#endif
#ifdef USE_RTC6705_CLK_HACK
vtxCLKPin = IOGetByTag(IO_TAG(RTC6705_CLK_PIN));
// we assume the CLK pin will have been initialised by the SPI code.
#endif
vtxCSPin = IOGetByTag(IO_TAG(RTC6705_CS_PIN));
IOInit(vtxCSPin, OWNER_VTX, RESOURCE_OUTPUT, 0);
DISABLE_RTC6705();
// GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode.
// Note: It's critical to ensure that incorrect signals are not sent to the VTX.
IOConfigGPIO(vtxCSPin, IOCFG_OUT_PP);
}
/**
* Transfer a 25bit packet to RTC6705
* This will just send it as a 32bit packet LSB meaning
* extra 0's get truncated on RTC6705 end
*/
static void rtc6705Transfer(uint32_t command)
{
command = reverse32(command);
ENABLE_RTC6705();
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 24) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 16) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 8) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 0) & 0xFF);
delayMicroseconds(2);
DISABLE_RTC6705();
delayMicroseconds(2);
}
/**
* Set a band and channel
*/
void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
{
band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1);
channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
rtc6705Transfer(RTC6705_SET_HEAD);
rtc6705Transfer(channelArray[band][channel]);
}
/**
* Set a freq in mhz
* Formula derived from datasheet
*/
void rtc6705SetFreq(uint16_t frequency)
{
frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);
uint32_t val_hex = 0;
uint32_t val_a = ((((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
uint32_t val_n = (((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
val_hex |= RTC6705_SET_WRITE;
val_hex |= (val_a << 5);
val_hex |= (val_n << 12);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
rtc6705Transfer(RTC6705_SET_HEAD);
delayMicroseconds(10);
rtc6705Transfer(val_hex);
}
void rtc6705SetRFPower(uint8_t rf_power)
{
rf_power = constrain(rf_power, 0, VTX_RTC6705_POWER_COUNT - 1);
spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);
uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
val_hex |= RTC6705_ADDRESS; // address
uint32_t data = rf_power == 0 ? (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK)) : PA_CONTROL_DEFAULT;
val_hex |= data << 5; // 4 address bits and 1 rw bit.
rtc6705Transfer(val_hex);
}
void rtc6705Disable(void)
{
#ifdef RTC6705_POWER_PIN
DISABLE_VTX_POWER();
#endif
}
void rtc6705Enable(void)
{
#ifdef RTC6705_POWER_PIN
ENABLE_VTX_POWER();
#endif
}
#endif

50
src/main/drivers/vtx_rtc6705.h
View file

@ -1,50 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#pragma once
#include <stdint.h>
#define VTX_RTC6705_BAND_COUNT 5
#define VTX_RTC6705_CHANNEL_COUNT 8
#define VTX_RTC6705_POWER_COUNT 3
#define VTX_RTC6705_DEFAULT_POWER 1
#if defined(RTC6705_POWER_PIN)
#define VTX_RTC6705_MIN_POWER 0
#else
#define VTX_RTC6705_MIN_POWER 1
#endif
#define VTX_RTC6705_FREQ_MIN 5600
#define VTX_RTC6705_FREQ_MAX 5950
#define VTX_RTC6705_BOOT_DELAY 350 // milliseconds
void rtc6705IOInit(void);
void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel);
void rtc6705SetFreq(const uint16_t freq);
void rtc6705SetRFPower(const uint8_t rf_power);
void rtc6705Disable(void);
void rtc6705Enable(void);

156
src/main/drivers/vtx_rtc6705_soft_spi.c
View file

@ -1,156 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI)
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "light_led.h"
#include "vtx_rtc6705.h"
#define DP_5G_MASK 0x7000
#define PA5G_BS_MASK 0x0E00
#define PA5G_PW_MASK 0x0180
#define PD_Q5G_MASK 0x0040
#define QI_5G_MASK 0x0038
#define PA_BS_MASK 0x0007
#define PA_CONTROL_DEFAULT 0x4FBD
#define RTC6705_SPICLK_ON IOHi(rtc6705ClkPin)
#define RTC6705_SPICLK_OFF IOLo(rtc6705ClkPin)
#define RTC6705_SPIDATA_ON IOHi(rtc6705DataPin)
#define RTC6705_SPIDATA_OFF IOLo(rtc6705DataPin)
#define RTC6705_SPILE_ON IOHi(rtc6705LePin)
#define RTC6705_SPILE_OFF IOLo(rtc6705LePin)
const uint16_t vtx_freq[] =
{
5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725, // Boacam A
5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866, // Boscam B
5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945, // Boscam E
5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880, // FatShark
5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917, // RaceBand
};
static IO_t rtc6705DataPin = IO_NONE;
static IO_t rtc6705LePin = IO_NONE;
static IO_t rtc6705ClkPin = IO_NONE;
void rtc6705IOInit(void)
{
rtc6705DataPin = IOGetByTag(IO_TAG(RTC6705_SPIDATA_PIN));
rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN));
rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN));
IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP);
IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP);
IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET);
IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP);
}
static void rtc6705_write_register(uint8_t addr, uint32_t data)
{
uint8_t i;
RTC6705_SPILE_OFF;
delay(1);
// send address
for (i=0; i<4; i++) {
if ((addr >> i) & 1)
RTC6705_SPIDATA_ON;
else
RTC6705_SPIDATA_OFF;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
}
// Write bit
RTC6705_SPIDATA_ON;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
for (i=0; i<20; i++) {
if ((data >> i) & 1)
RTC6705_SPIDATA_ON;
else
RTC6705_SPIDATA_OFF;
RTC6705_SPICLK_ON;
delay(1);
RTC6705_SPICLK_OFF;
delay(1);
}
RTC6705_SPILE_ON;
}
void rtc6705SetFreq(uint16_t channel_freq)
{
uint32_t freq = (uint32_t)channel_freq * 1000;
uint32_t N, A;
freq /= 40;
N = freq / 64;
A = freq % 64;
rtc6705_write_register(0, 400);
rtc6705_write_register(1, (N << 7) | A);
}
void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel)
{
// band and channel are 1-based, not 0-based
// example for raceband/ch8:
// (5 - 1) * 8 + (8 - 1)
// 4 * 8 + 7
// 32 + 7 = 39
uint8_t freqIndex = ((band - 1) * RTC6705_BAND_COUNT) + (channel - 1);
uint16_t freq = vtx_freq[freqIndex];
rtc6705SetFreq(freq);
}
void rtc6705SetRFPower(const uint8_t rf_power)
{
rtc6705_write_register(7, (rf_power ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK))));
}
void rtc6705Disable(void)
{
}
void rtc6705Enable(void)
{
}
#endif

34
src/main/fc/cli.c
View file

@ -67,6 +67,7 @@ extern uint8_t __config_end;
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/usb_msc.h" #include "drivers/usb_msc.h"
#include "drivers/vtx_common.h"
#include "fc/fc_core.h" #include "fc/fc_core.h"
#include "fc/cli.h" #include "fc/cli.h"
@ -145,7 +146,7 @@ static bool commandBatchError = false;
// sync this with features_e // sync this with features_e
static const char * const featureNames[] = { static const char * const featureNames[] = {
"THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
"DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "SOFTSERIAL", "GPS", "RPM_FILTERS",
"", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
"BLACKBOX", "", "TRANSPONDER", "AIRMODE", "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
@ -1321,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
} else if (sl_strcasecmp(cmdline, "save") == 0) { } else if (sl_strcasecmp(cmdline, "save") == 0) {
posControl.waypointListValid = false; posControl.waypointListValid = false;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break; if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) { if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.waypointCount = i + 1; posControl.waypointCount = i + 1;
posControl.waypointListValid = true; posControl.waypointListValid = true;
@ -1907,7 +1908,7 @@ static void cliGlobalFunctions(char *cmdline) {
if ( if (
i >= 0 && i < MAX_GLOBAL_FUNCTIONS && i >= 0 && i < MAX_GLOBAL_FUNCTIONS &&
args[ENABLED] >= 0 && args[ENABLED] <= 1 && args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
args[CONDITION_ID] >= 0 && args[CONDITION_ID] < MAX_LOGIC_CONDITIONS && args[CONDITION_ID] >= -1 && args[CONDITION_ID] < MAX_LOGIC_CONDITIONS &&
args[ACTION] >= 0 && args[ACTION] < GLOBAL_FUNCTION_ACTION_LAST && args[ACTION] >= 0 && args[ACTION] < GLOBAL_FUNCTION_ACTION_LAST &&
args[VALUE_TYPE] >= 0 && args[VALUE_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST && args[VALUE_TYPE] >= 0 && args[VALUE_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[VALUE_VALUE] >= -1000000 && args[VALUE_VALUE] <= 1000000 && args[VALUE_VALUE] >= -1000000 && args[VALUE_VALUE] <= 1000000 &&
@ -2975,6 +2976,29 @@ static void cliStatus(char *cmdline)
cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS); cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
#endif #endif
#if defined(USE_VTX_CONTROL) && !defined(CLI_MINIMAL_VERBOSITY)
cliPrint("VTX: ");
if (vtxCommonDeviceIsReady(vtxCommonDevice())) {
vtxDeviceOsdInfo_t osdInfo;
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
cliPrintf("band: %c, chan: %s, power: %c", osdInfo.bandLetter, osdInfo.channelName, osdInfo.powerIndexLetter);
if (osdInfo.powerMilliwatt) {
cliPrintf(" (%d mW)", osdInfo.powerMilliwatt);
}
if (osdInfo.frequency) {
cliPrintf(", freq: %d MHz", osdInfo.frequency);
}
}
else {
cliPrint("not detected");
}
cliPrintLinefeed();
#endif
// If we are blocked by PWM init - provide more information // If we are blocked by PWM init - provide more information
if (getPwmInitError() != PWM_INIT_ERROR_NONE) { if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage()); cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());
@ -3146,13 +3170,13 @@ static void printConfig(const char *cmdline, bool doDiff)
//printResource(dumpMask, &defaultConfig); //printResource(dumpMask, &defaultConfig);
cliPrintHashLine("mixer"); cliPrintHashLine("mixer");
cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n");
printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0));
// print custom servo mixer if exists // print custom servo mixer if exists
cliPrintHashLine("servo mix"); cliPrintHashLine("servo mix");
cliDumpPrintLinef(dumpMask, customServoMixers(0)->rate == 0, "smix reset\r\n"); cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n");
printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0));
// print servo parameters // print servo parameters

18
src/main/fc/config.c
View file

@ -181,13 +181,7 @@ void validateAndFixConfig(void)
} }
// Disable unused features // Disable unused features
featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10); featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);
#if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM)
if (rxConfig()->receiverType == RX_TYPE_PWM) {
rxConfigMutable()->receiverType = RX_TYPE_NONE;
}
#endif
#if !defined(USE_RX_PPM) #if !defined(USE_RX_PPM)
if (rxConfig()->receiverType == RX_TYPE_PPM) { if (rxConfig()->receiverType == RX_TYPE_PPM) {
@ -196,16 +190,6 @@ void validateAndFixConfig(void)
#endif #endif
if (rxConfig()->receiverType == RX_TYPE_PWM) {
#if defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
// led strip needs the same ports
featureClear(FEATURE_LED_STRIP);
#endif
// software serial needs free PWM ports
featureClear(FEATURE_SOFTSERIAL);
}
#if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) { if (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);

2
src/main/fc/config.h
View file

@ -41,7 +41,7 @@ typedef enum {
FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command
FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4, FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_DYNAMIC_FILTERS = 1 << 5, // was FEATURE_SERVO_TILT FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS
FEATURE_SOFTSERIAL = 1 << 6, FEATURE_SOFTSERIAL = 1 << 6,
FEATURE_GPS = 1 << 7, FEATURE_GPS = 1 << 7,
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE

36
src/main/fc/fc_init.c
View file

@ -74,7 +74,6 @@
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/io_pca9685.h" #include "drivers/io_pca9685.h"
#include "drivers/vtx_rtc6705.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#ifdef USE_USB_MSC #ifdef USE_USB_MSC
#include "drivers/usb_msc.h" #include "drivers/usb_msc.h"
@ -253,10 +252,10 @@ void init(void)
#if defined(AVOID_UART2_FOR_PWM_PPM) #if defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(feature(FEATURE_SOFTSERIAL), serialInit(feature(FEATURE_SOFTSERIAL),
(rxConfig()->receiverType == RX_TYPE_PWM) || (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM) #elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(feature(FEATURE_SOFTSERIAL), serialInit(feature(FEATURE_SOFTSERIAL),
(rxConfig()->receiverType == RX_TYPE_PWM) || (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); (rxConfig()->receiverType == RX_TYPE_PPM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else #else
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif #endif
@ -299,37 +298,6 @@ void init(void)
ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR); ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR);
} }
/*
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
pwm_params.flyingPlatformType = mixerConfig()->platformType;
pwm_params.useParallelPWM = (rxConfig()->receiverType == RX_TYPE_PWM);
pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM);
pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL);
pwm_params.useServoOutputs = isMixerUsingServos();
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE);
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
pwmRxInit(systemConfig()->pwmRxInputFilteringMode);
#endif
#ifdef USE_PWM_SERVO_DRIVER
// If external PWM driver is enabled, for example PCA9685, disable internal
// servo handling mechanism, since external device will do that
if (feature(FEATURE_PWM_SERVO_DRIVER)) {
pwm_params.useServoOutputs = false;
}
#endif
*/
systemState |= SYSTEM_STATE_MOTORS_READY; systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER #ifdef BEEPER

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

@ -1419,7 +1419,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP2_INAV_MIXER: case MSP2_INAV_MIXER:
sbufWriteU8(dst, mixerConfig()->yaw_motor_direction); sbufWriteU8(dst, mixerConfig()->motorDirectionInverted);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU8(dst, mixerConfig()->platformType); sbufWriteU8(dst, mixerConfig()->platformType);
sbufWriteU8(dst, mixerConfig()->hasFlaps); sbufWriteU8(dst, mixerConfig()->hasFlaps);
@ -2377,11 +2377,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
const uint8_t newChannel = (newFrequency % 8) + 1; const uint8_t newChannel = (newFrequency % 8) + 1;
vtxSettingsConfigMutable()->band = newBand; vtxSettingsConfigMutable()->band = newBand;
vtxSettingsConfigMutable()->channel = newChannel; vtxSettingsConfigMutable()->channel = newChannel;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
} else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { //value is frequency in MHz. Ignore it if it's invalid
vtxSettingsConfigMutable()->band = 0;
vtxSettingsConfigMutable()->channel = 0;
vtxSettingsConfigMutable()->freq = newFrequency;
} }
if (sbufBytesRemaining(src) > 1) { if (sbufBytesRemaining(src) > 1) {
@ -2749,7 +2744,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break; break;
case MSP2_INAV_SET_MIXER: case MSP2_INAV_SET_MIXER:
mixerConfigMutable()->yaw_motor_direction = sbufReadU8(src); mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src);
sbufReadU16(src); // Was yaw_jump_prevention_limit sbufReadU16(src); // Was yaw_jump_prevention_limit
mixerConfigMutable()->platformType = sbufReadU8(src); mixerConfigMutable()->platformType = sbufReadU8(src);
mixerConfigMutable()->hasFlaps = sbufReadU8(src); mixerConfigMutable()->hasFlaps = sbufReadU8(src);

1
src/main/fc/runtime_config.h
View file

@ -130,6 +130,7 @@ typedef enum {
ALTITUDE_CONTROL = (1 << 21), //It means it can fly ALTITUDE_CONTROL = (1 << 21), //It means it can fly
MOVE_FORWARD_ONLY = (1 << 22), MOVE_FORWARD_ONLY = (1 << 22),
SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23), SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23),
FW_HEADING_USE_YAW = (1 << 24),
} stateFlags_t; } stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask)) #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

73
src/main/fc/settings.yaml
View file

@ -22,7 +22,7 @@ tables:
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"] values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
enum: pitotSensor_e enum: pitotSensor_e
- name: receiver_type - name: receiver_type
values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"] values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e enum: rxReceiverType_e
- name: serial_rx - name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"] values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"]
@ -81,7 +81,7 @@ tables:
values: ["NONE", "GYRO", "AGL", "FLOW_RAW", values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM", "RPM_FILTER", "RPM_FREQ"] "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator
@ -186,22 +186,21 @@ groups:
- name: gyro_stage2_lowpass_type - name: gyro_stage2_lowpass_type
field: gyro_stage2_lowpass_type field: gyro_stage2_lowpass_type
table: filter_type table: filter_type
- name: dyn_notch_width_percent - name: dynamic_gyro_notch_enabled
field: dyn_notch_width_percent field: dynamicGyroNotchEnabled
condition: USE_DYNAMIC_FILTERS condition: USE_DYNAMIC_FILTERS
min: 0 type: bool
max: 20 - name: dynamic_gyro_notch_range
- name: dyn_notch_range field: dynamicGyroNotchRange
field: dyn_notch_range
condition: USE_DYNAMIC_FILTERS condition: USE_DYNAMIC_FILTERS
table: dynamicFilterRangeTable table: dynamicFilterRangeTable
- name: dyn_notch_q - name: dynamic_gyro_notch_q
field: dyn_notch_q field: dynamicGyroNotchQ
condition: USE_DYNAMIC_FILTERS condition: USE_DYNAMIC_FILTERS
min: 1 min: 1
max: 1000 max: 1000
- name: dyn_notch_min_hz - name: dynamic_gyro_notch_min_hz
field: dyn_notch_min_hz field: dynamicGyroNotchMinHz
condition: USE_DYNAMIC_FILTERS condition: USE_DYNAMIC_FILTERS
min: 60 min: 60
max: 1000 max: 1000
@ -685,9 +684,9 @@ groups:
- name: PG_MIXER_CONFIG - name: PG_MIXER_CONFIG
type: mixerConfig_t type: mixerConfig_t
members: members:
- name: yaw_motor_direction - name: motor_direction_inverted
min: -1 field: motorDirectionInverted
max: 1 type: bool
- name: platform_type - name: platform_type
field: platformType field: platformType
type: uint8_t type: uint8_t
@ -1220,6 +1219,25 @@ groups:
condition: USE_NAV condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_hdg_p
field: bank_fw.pid[PID_POS_HEADING].P
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_i
field: bank_fw.pid[PID_POS_HEADING].I
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_d
field: bank_fw.pid[PID_POS_HEADING].D
condition: USE_NAV
min: 0
max: 255
- name: nav_fw_pos_hdg_pidsum_limit
field: pidSumLimitYaw
min: PID_SUM_LIMIT_MIN
max: PID_SUM_LIMIT_MAX
- name: mc_iterm_relax_type - name: mc_iterm_relax_type
field: iterm_relax_type field: iterm_relax_type
table: iterm_relax_type table: iterm_relax_type
@ -1636,6 +1654,13 @@ groups:
- name: nav_fw_allow_manual_thr_increase - name: nav_fw_allow_manual_thr_increase
field: fw.allow_manual_thr_increase field: fw.allow_manual_thr_increase
type: bool type: bool
- name: nav_use_fw_yaw_control
field: fw.useFwNavYawControl
type: bool
- name: nav_fw_yaw_deadband
field: fw.yawControlDeadband
min: 0
max: 90
- name: PG_TELEMETRY_CONFIG - name: PG_TELEMETRY_CONFIG
type: telemetryConfig_t type: telemetryConfig_t
@ -1673,8 +1698,8 @@ groups:
field: hottAlarmSoundInterval field: hottAlarmSoundInterval
min: 0 min: 0
max: 120 max: 120
- name: telemetry_uart_unidir - name: telemetry_halfduplex
field: uartUnidirectional field: halfDuplex
type: bool type: bool
- name: smartport_fuel_unit - name: smartport_fuel_unit
field: smartportFuelUnit field: smartportFuelUnit
@ -2062,16 +2087,10 @@ groups:
field: lowPowerDisarm field: lowPowerDisarm
table: vtx_low_power_disarm table: vtx_low_power_disarm
type: uint8_t type: uint8_t
- name: vtx_freq - name: vtx_pit_mode_chan
field: freq field: pitModeChan
min: VTX_SETTINGS_MIN_FREQUENCY_MHZ min: VTX_SETTINGS_MIN_CHANNEL
max: VTX_SETTINGS_MAX_FREQUENCY_MHZ max: VTX_SETTINGS_MAX_CHANNEL
condition: VTX_SETTINGS_FREQCMD
- name: vtx_pit_mode_freq
field: pitModeFreq
min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
condition: VTX_SETTINGS_FREQCMD
- name: PG_PINIOBOX_CONFIG - name: PG_PINIOBOX_CONFIG
type: pinioBoxConfig_t type: pinioBoxConfig_t

86
src/main/flight/dynamic_gyro_notch.c Normal file
View file

@ -0,0 +1,86 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
#ifdef USE_DYNAMIC_FILTERS
#include <stdint.h>
#include "dynamic_gyro_notch.h"
#include "fc/config.h"
#include "build/debug.h"
#include "sensors/gyro.h"
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
state->filtersApplyFn = nullFilterApply;
state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f;
state->enabled = gyroConfig()->dynamicGyroNotchEnabled;
state->looptime = getLooptime();
if (state->enabled) {
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
/*
* Step 1 - init all filters even if they will not be used further down the road
*/
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
}
state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) {
state->frequency[axis] = frequency;
DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
if (state->enabled) {
biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
}
}
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) {
float output = input;
/*
* We always apply all filters. If a filter dimension is disabled, one of
* the function pointers will be a null apply function
*/
output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output);
return output;
}
#endif

50
src/main/flight/dynamic_gyro_notch.h Normal file
View file

@ -0,0 +1,50 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
#include "common/axis.h"
#include "common/filter.h"
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
typedef struct dynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT];
float dynNotchQ;
float dynNotch1Ctr;
float dynNotch2Ctr;
uint32_t looptime;
uint8_t enabled;
/*
* Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type
*/
biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
filterApplyFnPtr filtersApplyFn;
} dynamicGyroNotchState_t;
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state);
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency);
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input);

119
src/main/flight/gyroanalyse.c
View file

@ -35,6 +35,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/time.h" #include "common/time.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/feature.h"
#include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro.h"
#include "drivers/time.h" #include "drivers/time.h"
@ -55,68 +56,40 @@
// we need 4 steps for each axis // we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
#define DYN_NOTCH_OSD_MIN_THROTTLE 20 void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
uint8_t range,
uint32_t targetLooptimeUs
) {
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
state->minFrequency = minFrequency;
static uint16_t EXTENDED_FASTRAM fftSamplingRateHz; if (range == DYN_NOTCH_RANGE_HIGH) {
static float EXTENDED_FASTRAM fftResolution; state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
static uint8_t EXTENDED_FASTRAM fftStartBin;
static uint16_t EXTENDED_FASTRAM dynNotchMaxCtrHz;
static uint8_t dynamicFilterRange;
static float EXTENDED_FASTRAM dynNotchQ;
static float EXTENDED_FASTRAM dynNotch1Ctr;
static float EXTENDED_FASTRAM dynNotch2Ctr;
static uint16_t EXTENDED_FASTRAM dynNotchMinHz;
static bool EXTENDED_FASTRAM dualNotch = true;
static uint16_t EXTENDED_FASTRAM dynNotchMaxFFT;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static EXTENDED_FASTRAM float hanningWindow[FFT_WINDOW_SIZE];
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
if (gyroConfig()->dyn_notch_width_percent == 0) {
dualNotch = false;
} }
else if (range == DYN_NOTCH_RANGE_MEDIUM) {
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) { state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
}
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
} }
// If we get at least 3 samples then use the default FFT sample frequency // If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz); state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz);
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE;
fftStartBin = dynNotchMinHz / lrintf(fftResolution); state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist state->maxFrequency = state->fftSamplingRateHz / 2; //Nyquist
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); state->hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
} }
}
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
// *** can this next line be removed ??? ***
gyroDataAnalyseInit(targetLooptimeUs);
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
state->maxSampleCount = samplingFrequency / fftSamplingRateHz; state->maxSampleCount = samplingFrequency / state->fftSamplingRateHz;
state->maxSampleCountRcp = 1.f / state->maxSampleCount; state->maxSampleCountRcp = 1.f / state->maxSampleCount;
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
@ -124,11 +97,11 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls // recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms // for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); const float looptime = MAX(1000000u / state->fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value // any init value
state->centerFreq[axis] = dynNotchMaxCtrHz; state->centerFreq[axis] = state->maxFrequency;
state->prevCenterFreq[axis] = dynNotchMaxCtrHz; state->prevCenterFreq[axis] = state->maxFrequency;
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
} }
} }
@ -138,13 +111,15 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
state->oversampledGyroAccumulator[axis] += sample; state->oversampledGyroAccumulator[axis] += sample;
} }
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2); static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
/* /*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/ */
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) void gyroDataAnalyse(gyroAnalyseState_t *state)
{ {
state->filterUpdateExecute = false; //This will be changed to true only if new data is present
// samples should have been pushed by `gyroDataAnalysePush` // samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples // if gyro sampling is > 1kHz, accumulate multiple samples
state->sampleCount++; state->sampleCount++;
@ -169,7 +144,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
// calculate FFT and update filters // calculate FFT and update filters
if (state->updateTicks > 0) { if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2); gyroDataAnalyseUpdate(state);
--state->updateTicks; --state->updateTicks;
} }
} }
@ -183,7 +158,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
/* /*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/ */
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
{ {
enum { enum {
STEP_ARM_CFFT_F32, STEP_ARM_CFFT_F32,
@ -245,7 +220,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
uint8_t binStart = 0; uint8_t binStart = 0;
uint8_t binMax = 0; uint8_t binMax = 0;
//for bins after initial decline, identify start bin and max bin //for bins after initial decline, identify start bin and max bin
for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) { for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
if (!fftIncreased) { if (!fftIncreased) {
binStart = i; // first up-step bin binStart = i; // first up-step bin
@ -282,24 +257,20 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
} }
} }
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
float centerFreq = dynNotchMaxCtrHz; float centerFreq = state->maxFrequency;
float fftMeanIndex = 0; float fftMeanIndex = 0;
// idx was shifted by 1 to start at 1, not 0 // idx was shifted by 1 to start at 1, not 0
if (fftSum > 0) { if (fftSum > 0) {
fftMeanIndex = (fftWeightedSum / fftSum) - 1; fftMeanIndex = (fftWeightedSum / fftSum) - 1;
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
centerFreq = fftMeanIndex * fftResolution; centerFreq = fftMeanIndex * state->fftResolution;
} else { } else {
centerFreq = state->prevCenterFreq[state->updateAxis]; centerFreq = state->prevCenterFreq[state->updateAxis];
} }
centerFreq = fmax(centerFreq, dynNotchMinHz); centerFreq = fmax(centerFreq, state->minFrequency);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = centerFreq; state->centerFreq[state->updateAxis] = centerFreq;
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
break; break;
} }
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
@ -307,13 +278,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// 7us // 7us
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
/*
if (dualNotch) { * Filters will be updated inside dynamicGyroNotchFiltersUpdate()
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); */
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH); state->filterUpdateExecute = true;
} else { state->filterUpdateAxis = state->updateAxis;
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH); state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
}
} }
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
@ -326,9 +296,9 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// apply hanning window to gyro samples and store result in fftData // apply hanning window to gyro samples and store result in fftData
// hanning starts and ends with 0, could be skipped for minor speed improvement // hanning starts and ends with 0, could be skipped for minor speed improvement
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &state->hanningWindow[0], &state->fftData[0], ringBufIdx);
if (state->circularBufferIdx > 0) { if (state->circularBufferIdx > 0) {
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &state->hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
} }
} }
} }
@ -336,13 +306,4 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
state->updateStep = (state->updateStep + 1) % STEP_COUNT; state->updateStep = (state->updateStep + 1) % STEP_COUNT;
} }
uint16_t getMaxFFT(void) {
return dynNotchMaxFFT;
}
void resetMaxFFT(void) {
dynNotchMaxFFT = 0;
}
#endif // USE_DYNAMIC_FILTERS #endif // USE_DYNAMIC_FILTERS

39
src/main/flight/gyroanalyse.h
View file

@ -51,13 +51,44 @@ typedef struct gyroAnalyseState_s {
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT]; uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
/*
* Extended Dynamic Filters are 3x3 filter matrix
* In this approach, we assume that vibration peak on one axis
* can be also detected on other axises, but with lower amplitude
* that causes this freqency not to be attenuated.
*
* This approach is similiar to the approach on RPM filter when motor base
* frequency is attenuated on every axis even tho it might not be appearing
* in gyro traces
*
* extendedDynamicFilter[GYRO_AXIS][ANALYZED_AXIS]
*
*/
biquadFilter_t extendedDynamicFilter[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
filterApplyFnPtr extendedDynamicFilterApplyFn;
bool filterUpdateExecute;
uint8_t filterUpdateAxis;
uint16_t filterUpdateFrequency;
uint16_t fftSamplingRateHz;
uint8_t fftStartBin;
float fftResolution;
uint16_t minFrequency;
uint16_t maxFrequency;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
float hanningWindow[FFT_WINDOW_SIZE];
} gyroAnalyseState_t; } gyroAnalyseState_t;
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
uint8_t range,
uint32_t targetLooptimeUs
);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2); void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
#endif #endif

13
src/main/flight/mixer.c
View file

@ -68,6 +68,7 @@ static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
static EXTENDED_FASTRAM int throttleDeadbandHigh = 0; static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMin = 0;
static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0;
static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
@ -77,10 +78,10 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.neutral = 1460 .neutral = 1460
); );
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.yaw_motor_direction = 1, .motorDirectionInverted = 0,
.platformType = PLATFORM_MULTIROTOR, .platformType = PLATFORM_MULTIROTOR,
.hasFlaps = false, .hasFlaps = false,
.appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
@ -253,6 +254,12 @@ void mixerInit(void)
} else { } else {
motorRateLimitingApplyFn = nullMotorRateLimiting; motorRateLimitingApplyFn = nullMotorRateLimiting;
} }
if (mixerConfig()->motorDirectionInverted) {
motorYawMultiplier = -1;
} else {
motorYawMultiplier = 1;
}
} }
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
@ -457,7 +464,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
rpyMix[i] = rpyMix[i] =
(input[PITCH] * currentMixer[i].pitch + (input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll + input[ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw) * mixerScale; -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i]; if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];

2
src/main/flight/mixer.h
View file

@ -63,7 +63,7 @@ typedef struct motorMixer_s {
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
typedef struct mixerConfig_s { typedef struct mixerConfig_s {
int8_t yaw_motor_direction; int8_t motorDirectionInverted;
uint8_t platformType; uint8_t platformType;
bool hasFlaps; bool hasFlaps;
int16_t appliedMixerPreset; int16_t appliedMixerPreset;

17
src/main/flight/pid.c
View file

@ -118,7 +118,6 @@ STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM uint8_t itermRelax; static EXTENDED_FASTRAM uint8_t itermRelax;
static EXTENDED_FASTRAM uint8_t itermRelaxType; static EXTENDED_FASTRAM uint8_t itermRelaxType;
static EXTENDED_FASTRAM float itermRelaxSetpointThreshold;
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf; static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
@ -186,6 +185,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.I = 50, // NAV_VEL_Z_I * 20 .I = 50, // NAV_VEL_Z_I * 20
.D = 10, // NAV_VEL_Z_D * 100 .D = 10, // NAV_VEL_Z_D * 100
.FF = 0, .FF = 0,
},
[PID_POS_HEADING] = {
.P = 0,
.I = 0,
.D = 0,
.FF = 0
} }
} }
}, },
@ -213,6 +218,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.I = 5, // FW_POS_XY_I * 100 .I = 5, // FW_POS_XY_I * 100
.D = 8, // FW_POS_XY_D * 100 .D = 8, // FW_POS_XY_D * 100
.FF = 0, .FF = 0,
},
[PID_POS_HEADING] = {
.P = 30,
.I = 2,
.D = 0,
.FF = 0
} }
} }
}, },
@ -256,6 +267,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.antigravityAccelerator = 1.0f, .antigravityAccelerator = 1.0f,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO, .pidControllerType = PID_TYPE_AUTO,
.navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
); );
bool pidInitFilters(void) bool pidInitFilters(void)
@ -643,7 +655,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
if (itermRelax) { if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) { if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold); const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
if (itermRelaxType == ITERM_RELAX_SETPOINT) { if (itermRelaxType == ITERM_RELAX_SETPOINT) {
*itermErrorRate *= itermRelaxFactor; *itermErrorRate *= itermRelaxFactor;
@ -1038,7 +1050,6 @@ void pidInit(void)
itermRelax = pidProfile()->iterm_relax; itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type; itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
yawLpfHz = pidProfile()->yaw_lpf_hz; yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);

3
src/main/flight/pid.h
View file

@ -65,6 +65,7 @@ typedef enum {
PID_LEVEL, // + + PID_LEVEL, // + +
PID_HEADING, // + + PID_HEADING, // + +
PID_VEL_Z, // + n/a PID_VEL_Z, // + n/a
PID_POS_HEADING,// n/a +
PID_ITEM_COUNT PID_ITEM_COUNT
} pidIndex_e; } pidIndex_e;
@ -148,6 +149,8 @@ typedef struct pidProfile_s {
float antigravityGain; float antigravityGain;
float antigravityAccelerator; float antigravityAccelerator;
uint8_t antigravityCutoff; uint8_t antigravityCutoff;
int navFwPosHdgPidsumLimit;
} pidProfile_t; } pidProfile_t;
typedef struct pidAutotuneConfig_s { typedef struct pidAutotuneConfig_s {

36
src/main/io/osd.c
View file

@ -530,14 +530,6 @@ static uint16_t osdConvertRSSI(void)
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
} }
static void osdGetVTXPowerChar(char *buff)
{
buff[0] = '-';
buff[1] = '\0';
uint8_t powerIndex = 0;
if (vtxCommonGetPowerIndex(vtxCommonDevice(), &powerIndex)) buff[0] = '0' + powerIndex;
}
/** /**
* Displays a temperature postfixed with a symbol depending on the current unit system * Displays a temperature postfixed with a symbol depending on the current unit system
* @param label to display * @param label to display
@ -659,6 +651,8 @@ static const char * osdArmingDisabledReasonMessage(void)
return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST"); return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR"); return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR");
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR("JUMP WAYPOINT MISCONFIGURED");
} }
#endif #endif
break; break;
@ -1681,34 +1675,26 @@ static bool osdDrawSingleElement(uint8_t item)
} }
case OSD_VTX_CHANNEL: case OSD_VTX_CHANNEL:
#if defined(VTX)
// FIXME: This doesn't actually work. It's for boards with
// builtin VTX.
tfp_sprintf(buff, "CH:%2d", current_vtx_channel % CHANNELS_PER_BAND + 1);
#else
{ {
uint8_t band = 0; vtxDeviceOsdInfo_t osdInfo;
uint8_t channel = 0; vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
char bandChr = '-';
const char *channelStr = "-"; tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName);
if (vtxCommonGetBandAndChannel(vtxCommonDevice(), &band, &channel)) {
bandChr = vtx58BandLetter[band];
channelStr = vtx58ChannelNames[channel];
}
tfp_sprintf(buff, "CH:%c%s:", bandChr, channelStr);
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
osdGetVTXPowerChar(buff); tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr);
return true; return true;
} }
#endif
break; break;
case OSD_VTX_POWER: case OSD_VTX_POWER:
{ {
osdGetVTXPowerChar(buff); vtxDeviceOsdInfo_t osdInfo;
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
return true; return true;

166
src/main/io/vtx.c
View file

@ -42,14 +42,13 @@
#include "io/vtx_string.h" #include "io/vtx_string.h"
#include "io/vtx_control.h" #include "io/vtx_control.h"
PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 1);
PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
.band = VTX_SETTINGS_DEFAULT_BAND, .band = VTX_SETTINGS_DEFAULT_BAND,
.channel = VTX_SETTINGS_DEFAULT_CHANNEL, .channel = VTX_SETTINGS_DEFAULT_CHANNEL,
.power = VTX_SETTINGS_DEFAULT_POWER, .power = VTX_SETTINGS_DEFAULT_POWER,
.freq = VTX_SETTINGS_DEFAULT_FREQ, .pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL,
.pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ,
.lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF,
); );
@ -63,51 +62,17 @@ typedef enum {
void vtxInit(void) void vtxInit(void)
{ {
bool settingsUpdated = false;
// sync frequency in parameter group when band/channel are specified
const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
vtxSettingsConfigMutable()->freq = freq;
settingsUpdated = true;
} }
#if defined(VTX_SETTINGS_FREQCMD) static vtxSettingsConfig_t * vtxGetRuntimeSettings(void)
// constrain pit mode frequency
if (vtxSettingsConfig()->pitModeFreq) {
const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ);
if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) {
vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq;
settingsUpdated = true;
}
}
#endif
if (settingsUpdated) {
saveConfigAndNotify();
}
}
static vtxSettingsConfig_t vtxGetSettings(void)
{ {
vtxSettingsConfig_t settings = { static vtxSettingsConfig_t settings;
.band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel,
.power = vtxSettingsConfig()->power,
.freq = vtxSettingsConfig()->freq,
.pitModeFreq = vtxSettingsConfig()->pitModeFreq,
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
};
#if 0 settings.band = vtxSettingsConfig()->band;
#if defined(VTX_SETTINGS_FREQCMD) settings.channel = vtxSettingsConfig()->channel;
if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) { settings.power = vtxSettingsConfig()->power;
settings.band = 0; settings.pitModeChan = vtxSettingsConfig()->pitModeChan;
settings.freq = settings.pitModeFreq; settings.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm;
settings.power = VTX_SETTINGS_DEFAULT_POWER;
}
#endif
#endif
if (!ARMING_FLAG(ARMED) && !failsafeIsActive() && if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) || ((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) ||
@ -116,56 +81,46 @@ static vtxSettingsConfig_t vtxGetSettings(void)
settings.power = VTX_SETTINGS_DEFAULT_POWER; settings.power = VTX_SETTINGS_DEFAULT_POWER;
} }
return settings; return &settings;
} }
static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice) static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{ {
// Shortcut for undefined band
if (!runtimeSettings->band) {
return false;
}
if(!ARMING_FLAG(ARMED)) { if(!ARMING_FLAG(ARMED)) {
uint8_t vtxBand; uint8_t vtxBand;
uint8_t vtxChan; uint8_t vtxChan;
if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) { if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
const vtxSettingsConfig_t settings = vtxGetSettings(); return false;
if (vtxBand != settings.band || vtxChan != settings.channel) {
vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
return true;
} }
if (vtxBand != runtimeSettings->band || vtxChan != runtimeSettings->channel) {
vtxCommonSetBandAndChannel(vtxDevice, runtimeSettings->band, runtimeSettings->channel);
return true;
} }
} }
return false; return false;
} }
#if defined(VTX_SETTINGS_FREQCMD) static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
static bool vtxProcessFrequency(vtxDevice_t *vtxDevice)
{
if(!ARMING_FLAG(ARMED)) {
uint16_t vtxFreq;
if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) {
const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxFreq != settings.freq) {
vtxCommonSetFrequency(vtxDevice, settings.freq);
return true;
}
}
}
return false;
}
#endif
static bool vtxProcessPower(vtxDevice_t *vtxDevice)
{ {
uint8_t vtxPower; uint8_t vtxPower;
if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { if (!vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
const vtxSettingsConfig_t settings = vtxGetSettings(); return false;
if (vtxPower != settings.power) {
vtxCommonSetPowerByIndex(vtxDevice, settings.power);
return true;
} }
if (vtxPower != runtimeSettings->power) {
vtxCommonSetPowerByIndex(vtxDevice, runtimeSettings->power);
return true;
} }
return false; return false;
} }
static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{ {
uint8_t pitOnOff; uint8_t pitOnOff;
@ -173,25 +128,10 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
static bool prevPmSwitchState = false; static bool prevPmSwitchState = false;
if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) { if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
// Not supported on INAV yet. It might not be that useful.
#if 0
currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
#endif
if (currPmSwitchState != prevPmSwitchState) { if (currPmSwitchState != prevPmSwitchState) {
prevPmSwitchState = currPmSwitchState; prevPmSwitchState = currPmSwitchState;
if (currPmSwitchState) { if (currPmSwitchState) {
#if defined(VTX_SETTINGS_FREQCMD)
if (vtxSettingsConfig()->pitModeFreq) {
return false;
}
#endif
#if 0
if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
#endif
if (0) { if (0) {
if (!pitOnOff) { if (!pitOnOff) {
vtxCommonSetPitMode(vtxDevice, true); vtxCommonSetPitMode(vtxDevice, true);
@ -209,22 +149,18 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
return false; return false;
} }
static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice) static bool vtxProcessCheckParameters(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
{ {
const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings(); uint8_t vtxBand;
vtxSettingsConfig_t vtxState = vtxSettingsState; uint8_t vtxChan;
uint8_t vtxPower;
if (vtxSettingsState.band) { vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel); vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan);
#if defined(VTX_SETTINGS_FREQCMD)
} else {
vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
#endif
}
vtxCommonGetPowerIndex(vtxDevice, &vtxState.power); return (runtimeSettings->band && runtimeSettings->band != vtxBand) ||
(runtimeSettings->channel != vtxChan) ||
return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); (runtimeSettings->power != vtxPower);
} }
void vtxUpdate(timeUs_t currentTimeUs) void vtxUpdate(timeUs_t currentTimeUs)
@ -240,36 +176,32 @@ void vtxUpdate(timeUs_t currentTimeUs)
// Check input sources for config updates // Check input sources for config updates
vtxControlInputPoll(); vtxControlInputPoll();
const uint8_t startingSchedule = currentSchedule; // Build runtime settings
const vtxSettingsConfig_t * runtimeSettings = vtxGetRuntimeSettings();
bool vtxUpdatePending = false; bool vtxUpdatePending = false;
do {
switch (currentSchedule) { switch (currentSchedule) {
case VTX_PARAM_POWER: case VTX_PARAM_POWER:
vtxUpdatePending = vtxProcessPower(vtxDevice); vtxUpdatePending = vtxProcessPower(vtxDevice, runtimeSettings);
break; break;
case VTX_PARAM_BANDCHAN: case VTX_PARAM_BANDCHAN:
if (vtxGetSettings().band) { vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice, runtimeSettings);
vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
#if defined(VTX_SETTINGS_FREQCMD)
} else {
vtxUpdatePending = vtxProcessFrequency(vtxDevice);
#endif
}
break; break;
case VTX_PARAM_PITMODE: case VTX_PARAM_PITMODE:
vtxUpdatePending = vtxProcessPitMode(vtxDevice); vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings);
break; break;
case VTX_PARAM_CONFIRM: case VTX_PARAM_CONFIRM:
vtxUpdatePending = vtxProcessStateUpdate(vtxDevice); vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings);
break; break;
default: default:
break; break;
} }
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
} while (!vtxUpdatePending && currentSchedule != startingSchedule);
if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
vtxCommonProcess(vtxDevice, currentTimeUs); vtxCommonProcess(vtxDevice, currentTimeUs);
} }
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
} }
} }

3
src/main/io/vtx.h
View file

@ -38,8 +38,7 @@ typedef struct vtxSettingsConfig_s {
uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande
uint8_t channel; // 1-8 uint8_t channel; // 1-8
uint8_t power; // 0 = lowest uint8_t power; // 0 = lowest
uint16_t freq; // sets freq in MHz if band=0 uint16_t pitModeChan; // sets out-of-range pitmode frequency
uint16_t pitModeFreq; // sets out-of-range pitmode frequency
uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e
} vtxSettingsConfig_t; } vtxSettingsConfig_t;

1
src/main/io/vtx_control.c
View file

@ -42,7 +42,6 @@
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
// .vtxChannelActivationConditions = { 0 },
.halfDuplex = true, .halfDuplex = true,
); );

9
src/main/io/vtx_control.h
View file

@ -33,15 +33,6 @@ typedef struct vtxConfig_s {
uint8_t halfDuplex; uint8_t halfDuplex;
} vtxConfig_t; } vtxConfig_t;
typedef struct vtxRunState_s {
int pitMode;
int band;
int channel;
int frequency;
int powerIndex;
int powerMilliwatt;
} vtxRunState_t;
PG_DECLARE(vtxConfig_t, vtxConfig); PG_DECLARE(vtxConfig_t, vtxConfig);
void vtxControlInit(void); void vtxControlInit(void);

80
src/main/io/vtx_ffpv24g.c
View file

@ -41,8 +41,6 @@
#include "scheduler/protothreads.h" #include "scheduler/protothreads.h"
//#include "cms/cms_menu_vtx_ffpv24g.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_ffpv24g.h" #include "io/vtx_ffpv24g.h"
#include "io/vtx_control.h" #include "io/vtx_control.h"
@ -85,7 +83,6 @@ typedef struct {
// Requested VTX state // Requested VTX state
struct { struct {
bool setByFrequency;
int band; int band;
int channel; int channel;
unsigned freq; unsigned freq;
@ -108,9 +105,9 @@ typedef struct {
/*****************************************************************************/ /*****************************************************************************/
const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = { const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
"--------", "-----",
"FFPV 2.4 A", "A 2.4",
"FFPV 2.4 B", "B 2.4",
}; };
const char * ffpvBandLetters = "-AB"; const char * ffpvBandLetters = "-AB";
@ -353,18 +350,10 @@ static bool impl_DevSetFreq(uint16_t freq)
return true; return true;
} }
static void impl_SetFreq(vtxDevice_t * vtxDevice, uint16_t freq) static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
{ {
UNUSED(vtxDevice); UNUSED(vtxDevice);
if (impl_DevSetFreq(freq)) {
// Keep track that we set frequency directly
vtxState.request.setByFrequency = true;
}
}
void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
{
// Validate band and channel // Validate band and channel
if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) { if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) {
return; return;
@ -372,20 +361,12 @@ void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) { if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) {
// Keep track of band/channel data // Keep track of band/channel data
vtxState.request.setByFrequency = false;
vtxState.request.band = band; vtxState.request.band = band;
vtxState.request.channel = channel; vtxState.request.channel = channel;
} }
} }
static void ffpvSetRFPowerByIndex(uint16_t index)
static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
{
UNUSED(vtxDevice);
ffpvSetBandAndChannel(band, channel);
}
void ffpvSetRFPowerByIndex(uint16_t index)
{ {
// Validate index // Validate index
if (index < 1 || index > VTX_FFPV_POWER_COUNT) { if (index < 1 || index > VTX_FFPV_POWER_COUNT) {
@ -422,7 +403,7 @@ static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
} }
// if in user-freq mode then report band as zero // if in user-freq mode then report band as zero
*pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band; *pBand = vtxState.request.band;
*pChannel = vtxState.request.channel; *pChannel = vtxState.request.channel;
return true; return true;
} }
@ -459,27 +440,33 @@ static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true; return true;
} }
vtxRunState_t * ffpvGetRuntimeState(void) static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{ {
static vtxRunState_t state; if (!impl_IsReady(vtxDevice)) {
return false;
}
if (vtxState.ready) { *pIndex = vtxState.request.powerIndex;
state.pitMode = 0; *pPowerMw = vtxState.request.power;
state.band = vtxState.request.band; return true;
state.channel = vtxState.request.channel;
state.frequency = vtxState.request.freq;
state.powerIndex = vtxState.request.powerIndex;
state.powerMilliwatt = vtxState.request.power;
} }
else {
state.pitMode = 0; static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
state.band = 1; {
state.channel = 1; if (!impl_IsReady(vtxDevice)) {
state.frequency = ffpvFrequencyTable[0][0]; return false;
state.powerIndex = 1;
state.powerMilliwatt = 25;
} }
return &state;
pOsdInfo->band = vtxState.request.band;
pOsdInfo->channel = vtxState.request.channel;
pOsdInfo->frequency = vtxState.request.freq;
pOsdInfo->powerIndex = vtxState.request.powerIndex;
pOsdInfo->powerMilliwatt = vtxState.request.power;
pOsdInfo->bandName = ffpvBandNames[vtxState.request.band];
pOsdInfo->bandLetter = ffpvBandLetters[vtxState.request.band];
pOsdInfo->channelName = ffpvChannelNames[vtxState.request.channel];
pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
return true;
} }
/*****************************************************************************/ /*****************************************************************************/
@ -490,11 +477,12 @@ static const vtxVTable_t impl_vtxVTable = {
.setBandAndChannel = impl_SetBandAndChannel, .setBandAndChannel = impl_SetBandAndChannel,
.setPowerByIndex = impl_SetPowerByIndex, .setPowerByIndex = impl_SetPowerByIndex,
.setPitMode = impl_SetPitMode, .setPitMode = impl_SetPitMode,
.setFrequency = impl_SetFreq,
.getBandAndChannel = impl_GetBandAndChannel, .getBandAndChannel = impl_GetBandAndChannel,
.getPowerIndex = impl_GetPowerIndex, .getPowerIndex = impl_GetPowerIndex,
.getPitMode = impl_GetPitMode, .getPitMode = impl_GetPitMode,
.getFrequency = impl_GetFreq, .getFrequency = impl_GetFreq,
.getPower = impl_GetPower,
.getOsdInfo = impl_GetOsdInfo,
}; };
static vtxDevice_t impl_vtxDevice = { static vtxDevice_t impl_vtxDevice = {
@ -502,9 +490,9 @@ static vtxDevice_t impl_vtxDevice = {
.capability.bandCount = VTX_FFPV_BAND_COUNT, .capability.bandCount = VTX_FFPV_BAND_COUNT,
.capability.channelCount = VTX_FFPV_CHANNEL_COUNT, .capability.channelCount = VTX_FFPV_CHANNEL_COUNT,
.capability.powerCount = VTX_FFPV_POWER_COUNT, .capability.powerCount = VTX_FFPV_POWER_COUNT,
.bandNames = (char **)ffpvBandNames, .capability.bandNames = (char **)ffpvBandNames,
.channelNames = (char **)ffpvChannelNames, .capability.channelNames = (char **)ffpvChannelNames,
.powerNames = (char **)ffpvPowerNames, .capability.powerNames = (char **)ffpvPowerNames,
}; };
bool vtxFuriousFPVInit(void) bool vtxFuriousFPVInit(void)

10
src/main/io/vtx_ffpv24g.h
View file

@ -38,14 +38,4 @@
#define VTX_FFPV_CHANNEL_COUNT 8 #define VTX_FFPV_CHANNEL_COUNT 8
#define VTX_FFPV_POWER_COUNT 4 #define VTX_FFPV_POWER_COUNT 4
extern const char * ffpvBandLetters;
extern const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1];
extern const char * const ffpvChannelNames[VTX_FFPV_CHANNEL_COUNT + 1];
extern const char * const ffpvPowerNames[VTX_FFPV_POWER_COUNT + 1];
extern const uint16_t ffpvFrequencyTable[VTX_FFPV_BAND_COUNT][VTX_FFPV_CHANNEL_COUNT];
bool vtxFuriousFPVInit(void); bool vtxFuriousFPVInit(void);
void ffpvSetBandAndChannel(uint8_t band, uint8_t channel);
void ffpvSetRFPowerByIndex(uint16_t index);
vtxRunState_t * ffpvGetRuntimeState(void);

72
src/main/io/vtx_smartaudio.c
View file

@ -32,7 +32,6 @@
#include "build/debug.h" #include "build/debug.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_menu_vtx_smartaudio.h"
#include "common/log.h" #include "common/log.h"
#include "common/maths.h" #include "common/maths.h"
@ -67,9 +66,9 @@ static vtxDevice_t vtxSmartAudio = {
.capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT, .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT, .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
.capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT, .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
.bandNames = (char **)vtx58BandNames, .capability.bandNames = (char **)vtx58BandNames,
.channelNames = (char **)vtx58ChannelNames, .capability.channelNames = (char **)vtx58ChannelNames,
.powerNames = (char **)saPowerNames, .capability.powerNames = (char **)saPowerNames,
}; };
// SmartAudio command and response codes // SmartAudio command and response codes
@ -332,7 +331,7 @@ static void saProcessResponse(uint8_t *buf, int len)
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) { if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
#ifdef USE_CMS //if changes then trigger saCms update #ifdef USE_CMS //if changes then trigger saCms update
saCmsResetOpmodel(); //saCmsResetOpmodel();
#endif #endif
// Debug // Debug
saPrintSettings(); saPrintSettings();
@ -341,7 +340,7 @@ static void saProcessResponse(uint8_t *buf, int len)
#ifdef USE_CMS #ifdef USE_CMS
// Export current device status for CMS // Export current device status for CMS
saCmsUpdate(); //saCmsUpdate();
#endif #endif
} }
@ -540,11 +539,6 @@ static void saGetSettings(void)
saQueueCmd(bufGetSettings, 5); saQueueCmd(bufGetSettings, 5);
} }
static bool saValidateFreq(uint16_t freq)
{
return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ);
}
static void saDoDevSetFreq(uint16_t freq) static void saDoDevSetFreq(uint16_t freq)
{ {
static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 }; static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
@ -802,15 +796,6 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
return; return;
} }
static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (saValidateFreq(freq)) {
saSetMode(0); //need to be in FREE mode to set freq
saSetFreq(freq);
}
}
static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxSAIsReady(vtxDevice)) { if (!vtxSAIsReady(vtxDevice)) {
@ -857,6 +842,50 @@ static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true; return true;
} }
static bool vtxSAGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
uint8_t powerIndex;
if (!vtxSAGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = powerIndex;
*pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex-1].rfpower : 0;
return true;
}
static bool vtxSAGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
uint8_t powerIndex;
uint16_t powerMw;
uint16_t freq;
uint8_t band, channel;
if (!vtxSAGetBandAndChannel(vtxDevice, &band, &channel)) {
return false;
}
if (!vtxSAGetFreq(vtxDevice, &freq)) {
return false;
}
if (!vtxSAGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = band;
pOsdInfo->channel = channel;
pOsdInfo->frequency = freq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[band][0];
pOsdInfo->bandName = vtx58BandNames[band];
pOsdInfo->channelName = vtx58ChannelNames[channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
static const vtxVTable_t saVTable = { static const vtxVTable_t saVTable = {
.process = vtxSAProcess, .process = vtxSAProcess,
.getDeviceType = vtxSAGetDeviceType, .getDeviceType = vtxSAGetDeviceType,
@ -864,11 +893,12 @@ static const vtxVTable_t saVTable = {
.setBandAndChannel = vtxSASetBandAndChannel, .setBandAndChannel = vtxSASetBandAndChannel,
.setPowerByIndex = vtxSASetPowerByIndex, .setPowerByIndex = vtxSASetPowerByIndex,
.setPitMode = vtxSASetPitMode, .setPitMode = vtxSASetPitMode,
.setFrequency = vtxSASetFreq,
.getBandAndChannel = vtxSAGetBandAndChannel, .getBandAndChannel = vtxSAGetBandAndChannel,
.getPowerIndex = vtxSAGetPowerIndex, .getPowerIndex = vtxSAGetPowerIndex,
.getPitMode = vtxSAGetPitMode, .getPitMode = vtxSAGetPitMode,
.getFrequency = vtxSAGetFreq, .getFrequency = vtxSAGetFreq,
.getPower = vtxSAGetPower,
.getOsdInfo = vtxSAGetOsdInfo,
}; };

5
src/main/io/vtx_string.c
View file

@ -27,6 +27,7 @@
#define VTX_STRING_BAND_COUNT 5 #define VTX_STRING_BAND_COUNT 5
#define VTX_STRING_CHAN_COUNT 8 #define VTX_STRING_CHAN_COUNT 8
#define VTX_STRING_POWER_COUNT 5
const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] = const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] =
{ {
@ -52,6 +53,10 @@ const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = {
"-", "1", "2", "3", "4", "5", "6", "7", "8", "-", "1", "2", "3", "4", "5", "6", "7", "8",
}; };
const char * const vtx58DefaultPowerNames[VTX_STRING_POWER_COUNT + 1] = {
"---", "PL1", "PL2", "PL3", "PL4", "PL5"
};
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
{ {
int8_t band; int8_t band;

1
src/main/io/vtx_string.h
View file

@ -5,6 +5,7 @@
extern const uint16_t vtx58frequencyTable[5][8]; extern const uint16_t vtx58frequencyTable[5][8];
extern const char * const vtx58BandNames[]; extern const char * const vtx58BandNames[];
extern const char * const vtx58ChannelNames[]; extern const char * const vtx58ChannelNames[];
extern const char * const vtx58DefaultPowerNames[];
extern const char vtx58BandLetter[]; extern const char vtx58BandLetter[];
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel); bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel);

61
src/main/io/vtx_tramp.c
View file

@ -34,8 +34,6 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "cms/cms_menu_vtx_tramp.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#include "io/serial.h" #include "io/serial.h"
@ -44,7 +42,6 @@
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
#if defined(USE_CMS)
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
25, 100, 200, 400, 600 25, 100, 200, 400, 600
}; };
@ -52,7 +49,6 @@ const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = { const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
"---", "25 ", "100", "200", "400", "600" "---", "25 ", "100", "200", "400", "600"
}; };
#endif
static const vtxVTable_t trampVTable; // forward static const vtxVTable_t trampVTable; // forward
static vtxDevice_t vtxTramp = { static vtxDevice_t vtxTramp = {
@ -60,9 +56,9 @@ static vtxDevice_t vtxTramp = {
.capability.bandCount = VTX_TRAMP_BAND_COUNT, .capability.bandCount = VTX_TRAMP_BAND_COUNT,
.capability.channelCount = VTX_TRAMP_CHANNEL_COUNT, .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
.capability.powerCount = VTX_TRAMP_POWER_COUNT, .capability.powerCount = VTX_TRAMP_POWER_COUNT,
.bandNames = (char **)vtx58BandNames, .capability.bandNames = (char **)vtx58BandNames,
.channelNames = (char **)vtx58ChannelNames, .capability.channelNames = (char **)vtx58ChannelNames,
.powerNames = (char **)trampPowerNames, .capability.powerNames = (char **)trampPowerNames,
}; };
static serialPort_t *trampSerialPort = NULL; static serialPort_t *trampSerialPort = NULL;
@ -126,11 +122,6 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
trampWriteBuf(trampReqBuffer); trampWriteBuf(trampReqBuffer);
} }
static bool trampValidateFreq(uint16_t freq)
{
return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
}
static void trampDevSetFreq(uint16_t freq) static void trampDevSetFreq(uint16_t freq)
{ {
trampConfFreq = freq; trampConfFreq = freq;
@ -511,15 +502,6 @@ static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
trampSetPitMode(onoff); trampSetPitMode(onoff);
} }
static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{
UNUSED(vtxDevice);
if (trampValidateFreq(freq)) {
trampSetFreq(freq);
trampCommitChanges();
}
}
static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxTrampIsReady(vtxDevice)) { if (!vtxTrampIsReady(vtxDevice)) {
@ -570,6 +552,40 @@ static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
return true; return true;
} }
static bool vtxTrampGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
uint8_t powerIndex;
if (!vtxTrampGetPowerIndex(vtxDevice, &powerIndex)) {
return false;
}
*pIndex = trampData.configuredPower ? powerIndex : 0;
*pPowerMw = trampData.configuredPower;
return true;
}
static bool vtxTrampGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
{
uint8_t powerIndex;
uint16_t powerMw;
if (!vtxTrampGetPower(vtxDevice, &powerIndex, &powerMw)) {
return false;
}
pOsdInfo->band = trampData.setByFreqFlag ? 0 : trampData.band;
pOsdInfo->channel = trampData.channel;
pOsdInfo->frequency = trampData.curFreq;
pOsdInfo->powerIndex = powerIndex;
pOsdInfo->powerMilliwatt = powerMw;
pOsdInfo->bandLetter = vtx58BandNames[pOsdInfo->band][0];
pOsdInfo->bandName = vtx58BandNames[pOsdInfo->band];
pOsdInfo->channelName = vtx58ChannelNames[pOsdInfo->channel];
pOsdInfo->powerIndexLetter = '0' + powerIndex;
return true;
}
static const vtxVTable_t trampVTable = { static const vtxVTable_t trampVTable = {
.process = vtxTrampProcess, .process = vtxTrampProcess,
.getDeviceType = vtxTrampGetDeviceType, .getDeviceType = vtxTrampGetDeviceType,
@ -577,11 +593,12 @@ static const vtxVTable_t trampVTable = {
.setBandAndChannel = vtxTrampSetBandAndChannel, .setBandAndChannel = vtxTrampSetBandAndChannel,
.setPowerByIndex = vtxTrampSetPowerByIndex, .setPowerByIndex = vtxTrampSetPowerByIndex,
.setPitMode = vtxTrampSetPitMode, .setPitMode = vtxTrampSetPitMode,
.setFrequency = vtxTrampSetFreq,
.getBandAndChannel = vtxTrampGetBandAndChannel, .getBandAndChannel = vtxTrampGetBandAndChannel,
.getPowerIndex = vtxTrampGetPowerIndex, .getPowerIndex = vtxTrampGetPowerIndex,
.getPitMode = vtxTrampGetPitMode, .getPitMode = vtxTrampGetPitMode,
.getFrequency = vtxTrampGetFreq, .getFrequency = vtxTrampGetFreq,
.getPower = vtxTrampGetPower,
.getOsdInfo = vtxTrampGetOsdInfo,
}; };

77
src/main/navigation/navigation.c
View file

@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig, PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = { .general = {
@ -165,7 +165,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_climb_angle = 18, // 18 degrees .launch_climb_angle = 18, // 18 degrees
.launch_max_angle = 45, // 45 deg .launch_max_angle = 45, // 45 deg
.cruise_yaw_rate = 20, // 20dps .cruise_yaw_rate = 20, // 20dps
.allow_manual_thr_increase = false .allow_manual_thr_increase = false,
.useFwNavYawControl = 0,
.yawControlDeadband = 0,
} }
); );
@ -617,12 +619,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_PRE_ACTION] = { [NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, .persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0, .timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT, .mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE, .mwError = MW_NAV_ERROR_NONE,
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -1374,6 +1377,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
posControl.rthState.rthInitialDistance = posControl.homeDistance; posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
@ -1407,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
} }
break; break;
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1438,6 +1469,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1779,6 +1813,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
} }
} }
/*
* Limit both output and Iterm to limit windup
*/
pid->integrator = constrain(pid->integrator, outMin, outMax);
return outValConstrained; return outValConstrained;
} }
@ -2600,9 +2639,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags); setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
} }
// WP #1 - #15 - common waypoints - pre-programmed mission // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_RTH) { if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission) // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) { if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData; posControl.waypointList[wpNumber - 1] = *wpData;
@ -3076,6 +3115,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
} }
} }
// Don't allow arming if any of JUMP waypoint has invalid settings
if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
}
}
}
}
return NAV_ARMING_BLOCKER_NONE; return NAV_ARMING_BLOCKER_NONE;
} }
@ -3210,6 +3260,13 @@ void navigationUsePIDs(void)
0.0f, 0.0f,
NAV_DTERM_CUT_HZ NAV_DTERM_CUT_HZ
); );
navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
0.0f,
2.0f
);
} }
void navigationInit(void) void navigationInit(void)
@ -3240,6 +3297,16 @@ void navigationInit(void)
/* Use system config */ /* Use system config */
navigationUsePIDs(); navigationUsePIDs();
if (
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER ||
navConfig()->fw.useFwNavYawControl
) {
ENABLE_STATE(FW_HEADING_USE_YAW);
} else {
DISABLE_STATE(FW_HEADING_USE_YAW);
}
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

5
src/main/navigation/navigation.h
View file

@ -101,6 +101,7 @@ typedef enum {
NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1, NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1,
NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2, NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2,
NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3, NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3,
NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
} navArmingBlocker_e; } navArmingBlocker_e;
typedef struct positionEstimationConfig_s { typedef struct positionEstimationConfig_s {
@ -216,6 +217,8 @@ typedef struct navConfig_s {
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase; bool allow_manual_thr_increase;
bool useFwNavYawControl;
uint8_t yawControlDeadband;
} fw; } fw;
} navConfig_t; } navConfig_t;
@ -231,6 +234,7 @@ typedef struct gpsOrigin_s {
typedef enum { typedef enum {
NAV_WP_ACTION_WAYPOINT = 0x01, NAV_WP_ACTION_WAYPOINT = 0x01,
NAV_WP_ACTION_JUMP = 0x06,
NAV_WP_ACTION_RTH = 0x04 NAV_WP_ACTION_RTH = 0x04
} navWaypointActions_e; } navWaypointActions_e;
@ -308,6 +312,7 @@ typedef struct navigationPIDControllers_s {
/* Fixed-wing PIDs */ /* Fixed-wing PIDs */
pidController_t fw_alt; pidController_t fw_alt;
pidController_t fw_nav; pidController_t fw_nav;
pidController_t fw_heading;
} navigationPIDControllers_t; } navigationPIDControllers_t;
/* MultiWii-compatible params for telemetry */ /* MultiWii-compatible params for telemetry */

50
src/main/navigation/navigation_fixedwing.c
View file

@ -61,6 +61,7 @@
static bool isPitchAdjustmentValid = false; static bool isPitchAdjustmentValid = false;
static bool isRollAdjustmentValid = false; static bool isRollAdjustmentValid = false;
static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0; static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false; static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError; static int32_t navHeadingError;
@ -209,8 +210,11 @@ void resetFixedWingPositionController(void)
virtualDesiredPosition.z = 0; virtualDesiredPosition.z = 0;
navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_nav);
navPidReset(&posControl.pids.fw_heading);
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
posControl.rcAdjustment[YAW] = 0;
isRollAdjustmentValid = false; isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f); pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
} }
@ -292,7 +296,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// We have virtual position target, calculate heading error // We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
// Calculate NAV heading error /*
* Calculate NAV heading error
* Units are centidegrees
*/
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw); navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
// Forced turn direction // Forced turn direction
@ -333,6 +340,41 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment); posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
/*
* Yaw adjustment
* It is working in relative mode and we aim to keep error at zero
*/
if (STATE(FW_HEADING_USE_YAW)) {
static float limit = 0.0f;
if (limit == 0.0f) {
limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
}
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
float yawAdjustment = navPidApply2(
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
US2S(deltaMicros),
-limit,
limit,
yawPidFlags
) / 100.0f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
posControl.rcAdjustment[YAW] = yawAdjustment;
} else {
posControl.rcAdjustment[YAW] = 0;
}
} }
void applyFixedWingPositionController(timeUs_t currentTimeUs) void applyFixedWingPositionController(timeUs_t currentTimeUs)
@ -376,10 +418,12 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
} }
isRollAdjustmentValid = true; isRollAdjustmentValid = true;
isYawAdjustmentValid = true;
} }
else { else {
// No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller // No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
isRollAdjustmentValid = false; isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
} }
} }
@ -448,6 +492,10 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
} }
if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rcCommand[YAW] = posControl.rcAdjustment[YAW];
}
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) { if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH >0 dive, <0 climb // PITCH >0 dive, <0 climb
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

3
src/main/rx/rx.c
View file

@ -104,7 +104,7 @@ static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
rxRuntimeConfig_t rxRuntimeConfig; rxRuntimeConfig_t rxRuntimeConfig;
static uint8_t rcSampleIndex = 0; static uint8_t rcSampleIndex = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 8); PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 9);
#ifndef RX_SPI_DEFAULT_PROTOCOL #ifndef RX_SPI_DEFAULT_PROTOCOL
#define RX_SPI_DEFAULT_PROTOCOL 0 #define RX_SPI_DEFAULT_PROTOCOL 0
@ -327,7 +327,6 @@ void rxInit(void)
default: default:
case RX_TYPE_NONE: case RX_TYPE_NONE:
case RX_TYPE_PWM:
rxConfigMutable()->receiverType = RX_TYPE_NONE; rxConfigMutable()->receiverType = RX_TYPE_NONE;
rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;

11
src/main/rx/rx.h
View file

@ -59,12 +59,11 @@ typedef enum {
typedef enum { typedef enum {
RX_TYPE_NONE = 0, RX_TYPE_NONE = 0,
RX_TYPE_PWM = 1, RX_TYPE_PPM = 1,
RX_TYPE_PPM = 2, RX_TYPE_SERIAL = 2,
RX_TYPE_SERIAL = 3, RX_TYPE_MSP = 3,
RX_TYPE_MSP = 4, RX_TYPE_SPI = 4,
RX_TYPE_SPI = 5, RX_TYPE_UIB = 5
RX_TYPE_UIB = 6
} rxReceiverType_e; } rxReceiverType_e;
typedef enum { typedef enum {

75
src/main/sensors/gyro.c
View file

@ -69,6 +69,7 @@
#include "flight/gyroanalyse.h" #include "flight/gyroanalyse.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/dynamic_gyro_notch.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h" #include "hardware_revision.h"
@ -95,17 +96,12 @@ STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn2;
static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros
@ -122,10 +118,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_2 = 1, .gyro_soft_notch_cutoff_2 = 1,
.gyro_stage2_lowpass_hz = 0, .gyro_stage2_lowpass_hz = 0,
.gyro_stage2_lowpass_type = FILTER_BIQUAD, .gyro_stage2_lowpass_type = FILTER_BIQUAD,
.dyn_notch_width_percent = 8, .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
.dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM, .dynamicGyroNotchQ = 120,
.dyn_notch_q = 120, .dynamicGyroNotchMinHz = 150,
.dyn_notch_min_hz = 150, .dynamicGyroNotchEnabled = 0
); );
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@ -265,33 +261,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
return gyroHardware; return gyroHardware;
} }
#ifdef USE_DYNAMIC_FILTERS
bool isDynamicFilterActive(void)
{
return feature(FEATURE_DYNAMIC_FILTERS);
}
static void gyroInitFilterDynamicNotch(void)
{
notchFilterDynApplyFn = nullFilterApply;
notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) {
notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
}
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
biquadFilterInit(&notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
}
}
}
#endif
bool gyroInit(void) bool gyroInit(void)
{ {
memset(&gyro, 0, sizeof(gyro)); memset(&gyro, 0, sizeof(gyro));
@ -322,8 +291,13 @@ bool gyroInit(void)
gyroInitFilters(); gyroInitFilters();
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
gyroInitFilterDynamicNotch(); dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
gyroDataAnalyseStateInit(&gyroAnalyseState, getLooptime()); gyroDataAnalyseStateInit(
&gyroAnalyseState,
gyroConfig()->dynamicGyroNotchMinHz,
gyroConfig()->dynamicGyroNotchRange,
getLooptime()
);
#endif #endif
return true; return true;
} }
@ -468,18 +442,27 @@ void FAST_CODE NOINLINE gyroUpdate()
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf); gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) { if (dynamicGyroNotchState.enabled) {
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf); DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis, gyroADCf);
gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf); gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf);
} }
#endif #endif
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
} }
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) { if (dynamicGyroNotchState.enabled) {
gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2); gyroDataAnalyse(&gyroAnalyseState);
if (gyroAnalyseState.filterUpdateExecute) {
dynamicGyroNotchFiltersUpdate(
&dynamicGyroNotchState,
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.filterUpdateFrequency
);
}
} }
#endif #endif

8
src/main/sensors/gyro.h
View file

@ -71,10 +71,10 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_cutoff_2; uint16_t gyro_soft_notch_cutoff_2;
uint16_t gyro_stage2_lowpass_hz; uint16_t gyro_stage2_lowpass_hz;
uint8_t gyro_stage2_lowpass_type; uint8_t gyro_stage2_lowpass_type;
uint8_t dyn_notch_width_percent; uint8_t dynamicGyroNotchRange;
uint8_t dyn_notch_range; uint16_t dynamicGyroNotchQ;
uint16_t dyn_notch_q; uint16_t dynamicGyroNotchMinHz;
uint16_t dyn_notch_min_hz; uint8_t dynamicGyroNotchEnabled;
} gyroConfig_t; } gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig); PG_DECLARE(gyroConfig_t, gyroConfig);

4
src/main/target/BETAFLIGHTF4/target.c
View file

@ -27,8 +27,8 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
// Motors // Motors
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1

3
src/main/target/BETAFLIGHTF4/target.h
View file

@ -180,3 +180,6 @@
#define MAX_PWM_OUTPUT_PORTS 4 #define MAX_PWM_OUTPUT_PORTS 4
#define PCA9685_I2C_BUS BUS_I2C2 #define PCA9685_I2C_BUS BUS_I2C2
#define USE_DSHOT
#define USE_ESC_SENSOR

1
src/main/target/FF_F35_LIGHTNING/config.c
View file

@ -48,7 +48,6 @@ void targetConfiguration(void)
rxConfigMutable()->serialrx_provider = SERIALRX_CRSF; rxConfigMutable()->serialrx_provider = SERIALRX_CRSF;
serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_TELEMETRY_SMARTPORT; serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
telemetryConfigMutable()->uartUnidirectional = 1;
mixerConfigMutable()->platformType = PLATFORM_AIRPLANE; mixerConfigMutable()->platformType = PLATFORM_AIRPLANE;
} }

1
src/main/target/FF_PIKOF4/config.c
View file

@ -28,6 +28,5 @@
void targetConfiguration(void) void targetConfiguration(void)
{ {
rxConfigMutable()->halfDuplex = false; rxConfigMutable()->halfDuplex = false;
telemetryConfigMutable()->uartUnidirectional = true;
batteryMetersConfigMutable()->current.scale = CURRENTSCALE; batteryMetersConfigMutable()->current.scale = CURRENTSCALE;
} }

12
src/main/target/SPRACINGF3NEO/target.h
View file

@ -107,9 +107,6 @@
#define SPI3_MISO_PIN PB4 #define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5 #define SPI3_MOSI_PIN PB5
#define USE_VTX_RTC6705
#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
#undef USE_VTX_FFPV #undef USE_VTX_FFPV
#undef USE_VTX_SMARTAUDIO // Disabled due to flash size #undef USE_VTX_SMARTAUDIO // Disabled due to flash size
#undef USE_VTX_TRAMP // Disabled due to flash size #undef USE_VTX_TRAMP // Disabled due to flash size
@ -117,19 +114,10 @@
#undef USE_PITOT // Disabled due to RAM size #undef USE_PITOT // Disabled due to RAM size
#undef USE_PITOT_ADC // Disabled due to RAM size #undef USE_PITOT_ADC // Disabled due to RAM size
#define RTC6705_CS_PIN PF4
#define RTC6705_SPI_INSTANCE SPI3
#define RTC6705_POWER_PIN PC3
#define USE_RTC6705_CLK_HACK
#define RTC6705_CLK_PIN SPI3_SCK_PIN
#define USE_MAX7456 #define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PA15 #define MAX7456_CS_PIN PA15
#define SPI_SHARED_MAX7456_AND_RTC6705
#define USE_SDCARD #define USE_SDCARD
#define USE_SDCARD_SPI #define USE_SDCARD_SPI
#define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_INVERTED

3
src/main/target/SPRACINGF3NEO/target.mk_
View file

@ -15,5 +15,4 @@ TARGET_SRC = \
drivers/compass/compass_mag3110.c \ drivers/compass/compass_mag3110.c \
drivers/compass/compass_lis3mdl.c \ drivers/compass/compass_lis3mdl.c \
drivers/light_ws2811strip.c \ drivers/light_ws2811strip.c \
drivers/max7456.c \ drivers/max7456.c
drivers/vtx_rtc6705.c

6
src/main/target/SPRACINGF4EVO/target.h
View file

@ -126,12 +126,6 @@
#define SPI3_MISO_PIN PB4 // NC #define SPI3_MISO_PIN PB4 // NC
#define SPI3_MOSI_PIN PB5 // NC #define SPI3_MOSI_PIN PB5 // NC
#define USE_VTX_RTC6705
#define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control.
#define RTC6705_CS_PIN SPI3_NSS_PIN
#define RTC6705_SPI_INSTANCE SPI3
#define USE_SDCARD #define USE_SDCARD
#define USE_SDCARD_SPI #define USE_SDCARD_SPI
#define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_INVERTED

1
src/main/target/SPRACINGF4EVO/target.mk
View file

@ -15,4 +15,3 @@ TARGET_SRC = \
drivers/barometer/barometer_ms56xx.c \ drivers/barometer/barometer_ms56xx.c \
drivers/light_ws2811strip.c \ drivers/light_ws2811strip.c \
drivers/max7456.c drivers/max7456.c
# drivers/vtx_rtc6705.c

10
src/main/target/SPRACINGF7DUAL/target.h
View file

@ -155,16 +155,6 @@
#define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_SPI_BUS BUS_SPI3
#define SDCARD_CS_PIN PC3 #define SDCARD_CS_PIN PC3
// disabled for motor outputs 5-8:
//#define USE_VTX_RTC6705
//#define USE_VTX_RTC6705_SOFTSPI
//#define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL
//#define RTC6705_SPI_MOSI_PIN PB0 // Shared with PWM8
//#define RTC6705_CS_PIN PB6 // Shared with PWM5
//#define RTC6705_SPICLK_PIN PB1 // Shared with PWM7
//#define RTC6705_POWER_PIN PB7 // Shared with PWM6
#define GYRO_1_CS_PIN SPI1_NSS_PIN #define GYRO_1_CS_PIN SPI1_NSS_PIN
#define GYRO_1_SPI_INSTANCE BUS_SPI1 #define GYRO_1_SPI_INSTANCE BUS_SPI1
#define GYRO_2_CS_PIN PB2 #define GYRO_2_CS_PIN PB2

3
src/main/target/SPRACINGF7DUAL/target.mk
View file

@ -15,5 +15,4 @@ TARGET_SRC = \
drivers/compass/compass_mag3110.c \ drivers/compass/compass_mag3110.c \
drivers/compass/compass_lis3mdl.c \ drivers/compass/compass_lis3mdl.c \
drivers/light_ws2811strip.c \ drivers/light_ws2811strip.c \
drivers/max7456.c \ drivers/max7456.c
drivers/vtx_rtc6705_soft_spi.c

5
src/main/target/common_post.h
View file

@ -19,11 +19,6 @@
#pragma once #pragma once
// Targets with built-in vtx do not need external vtx
#if defined(VTX) || defined(USE_RTC6705)
# undef USE_VTX_CONTROL
#endif
// Backward compatibility for I2C OLED display // Backward compatibility for I2C OLED display
#if !defined(USE_I2C) #if !defined(USE_I2C)
# undef USE_DASHBOARD # undef USE_DASHBOARD

2
src/main/telemetry/hott.c
View file

@ -368,7 +368,7 @@ void configureHoTTTelemetryPort(void)
return; return;
} }
portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (SERIAL_NOT_INVERTED); portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (SERIAL_NOT_INVERTED);
hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, portOptions); hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, portOptions);

2
src/main/telemetry/smartport.c
View file

@ -306,7 +306,7 @@ static void freeSmartPortTelemetryPort(void)
static void configureSmartPortTelemetryPort(void) static void configureSmartPortTelemetryPort(void)
{ {
if (portConfig) { if (portConfig) {
portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
} }

2
src/main/telemetry/telemetry.c
View file

@ -64,7 +64,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.frsky_pitch_roll = 0, .frsky_pitch_roll = 0,
.report_cell_voltage = 0, .report_cell_voltage = 0,
.hottAlarmSoundInterval = 5, .hottAlarmSoundInterval = 5,
.uartUnidirectional = 0, .halfDuplex = 1,
.smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH, .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH,
.ibusTelemetryType = 0, .ibusTelemetryType = 0,
.ltmUpdateRate = LTM_RATE_NORMAL, .ltmUpdateRate = LTM_RATE_NORMAL,

2
src/main/telemetry/telemetry.h
View file

@ -64,7 +64,7 @@ typedef struct telemetryConfig_s {
uint8_t frsky_pitch_roll; uint8_t frsky_pitch_roll;
uint8_t report_cell_voltage; uint8_t report_cell_voltage;
uint8_t hottAlarmSoundInterval; uint8_t hottAlarmSoundInterval;
uint8_t uartUnidirectional; uint8_t halfDuplex;
smartportFuelUnit_e smartportFuelUnit; smartportFuelUnit_e smartportFuelUnit;
uint8_t ibusTelemetryType; uint8_t ibusTelemetryType;
uint8_t ltmUpdateRate; uint8_t ltmUpdateRate;