mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Merge branch 'development' into dzikuvx-correct-3d-throttle-handling
This commit is contained in:
Pawel Spychalski (DzikuVx)
commit
fab7de6a48
71 changed files with 1065 additions and 2468 deletions
|
|
@ -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_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) |
|
||||
| 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 |
|
||||
| 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" |
|
||||
| dyn_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_enabled | `OFF` | Enable/disable dynamic gyro notch also known as Matrix Filter |
|
||||
| dynamic_gyro_notch_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers |
|
||||
| dynamic_gyro_notch_q | 120 | Q factor for dynamic notches |
|
||||
| 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_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 |
|
||||
|
|
|
|||
|
|
@ -70,6 +70,15 @@ Logic conditions can be edited using INAV Configurator user interface, of via CL
|
|||
| 14 | TROTTLE_POS | in `%` |
|
||||
| 15 | ATTITUDE_ROLL | 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
|
||||
|
||||
|
|
|
|||
|
|
@ -154,11 +154,9 @@ The receiver type can be set from the configurator or CLI.
|
|||
```
|
||||
# get receiver_type
|
||||
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
|
||||
|
||||
The software has signal loss detection which is always enabled. Signal loss detection is used for safety and failsafe reasons.
|
||||
|
|
|
|||
|
|
@ -98,6 +98,7 @@ COMMON_SRC = \
|
|||
flight/wind_estimator.c \
|
||||
flight/gyroanalyse.c \
|
||||
flight/rpm_filter.c \
|
||||
flight/dynamic_gyro_notch.c \
|
||||
io/beeper.c \
|
||||
io/esc_serialshot.c \
|
||||
io/frsky_osd.c \
|
||||
|
|
@ -161,9 +162,7 @@ COMMON_SRC = \
|
|||
cms/cms_menu_navigation.c \
|
||||
cms/cms_menu_osd.c \
|
||||
cms/cms_menu_saveexit.c \
|
||||
cms/cms_menu_vtx_smartaudio.c \
|
||||
cms/cms_menu_vtx_tramp.c \
|
||||
cms/cms_menu_vtx_ffpv.c \
|
||||
cms/cms_menu_vtx.c \
|
||||
drivers/display_ug2864hsweg01.c \
|
||||
drivers/rangefinder/rangefinder_hcsr04.c \
|
||||
drivers/rangefinder/rangefinder_hcsr04_i2c.c \
|
||||
|
|
|
|||
|
|
@ -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_type", "%d", gyroConfig()->gyro_soft_lpf_type);
|
||||
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("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
|
||||
gyroConfig()->gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
|
|
|
|||
|
|
@ -67,5 +67,8 @@ typedef enum {
|
|||
DEBUG_ERPM,
|
||||
DEBUG_RPM_FILTER,
|
||||
DEBUG_RPM_FREQ,
|
||||
DEBUG_NAV_YAW,
|
||||
DEBUG_DYNAMIC_FILTER,
|
||||
DEBUG_DYNAMIC_FILTER_FREQUENCY,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
|||
|
|
@ -50,13 +50,6 @@
|
|||
#include "cms/cms_menu_battery.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
|
||||
|
||||
static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
|
||||
|
|
@ -111,19 +104,8 @@ static const OSD_Entry menuFeaturesEntries[] =
|
|||
#if defined(USE_NAV)
|
||||
OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation),
|
||||
#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_SMARTAUDIO)
|
||||
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
|
||||
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl),
|
||||
#endif // VTX_CONTROL
|
||||
#ifdef USE_LED_STRIP
|
||||
OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip),
|
||||
|
|
|
|||
|
|
@ -15,132 +15,247 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <ctype.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.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_types.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)
|
||||
{
|
||||
if (!featureRead) {
|
||||
cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0;
|
||||
featureRead = true;
|
||||
}
|
||||
// Config-time settings
|
||||
static uint8_t vtxBand = 0;
|
||||
static uint8_t vtxChan = 0;
|
||||
static uint8_t vtxPower = 0;
|
||||
static uint8_t vtxPitMode = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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 char * const vtxCmsPitModeNames[] = {
|
||||
"---", "OFF", "ON "
|
||||
};
|
||||
|
||||
static const OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]};
|
||||
static const OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1};
|
||||
// Menus (these are not const because we update them at run-time )
|
||||
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)
|
||||
{
|
||||
#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)
|
||||
static long cms_Vtx_configPitMode(displayPort_t *pDisp, const void *self)
|
||||
{
|
||||
UNUSED(pDisp);
|
||||
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;
|
||||
}
|
||||
|
||||
#ifdef VTX
|
||||
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[] =
|
||||
static long cms_Vtx_configBand(displayPort_t *pDisp, const void *self)
|
||||
{
|
||||
OSD_LABEL_ENTRY("--- VTX ---"),
|
||||
OSD_BOOL_ENTRY("ENABLED", &cmsx_featureVtx),
|
||||
#ifdef VTX
|
||||
OSD_UINT8_ENTRY("VTX MODE", &entryVtxMode),
|
||||
OSD_UINT16_ENTRY("VTX MHZ", &entryVtxMhz),
|
||||
#endif // VTX
|
||||
OSD_TAB_ENTRY("BAND", &entryVtxBand),
|
||||
OSD_UINT8_ENTRY("CHANNEL", &entryVtxChannel),
|
||||
#ifdef USE_RTC6705
|
||||
OSD_BOOL_ENTRY("LOW POWER", &masterConfig.vtx_power),
|
||||
#endif // USE_RTC6705
|
||||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
if (vtxBand == 0) {
|
||||
vtxBand = 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static long cms_Vtx_configChan(displayPort_t *pDisp, const void *self)
|
||||
{
|
||||
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,
|
||||
};
|
||||
|
||||
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
|
||||
.GUARD_text = "MENUVTX",
|
||||
.GUARD_type = OME_MENU,
|
||||
#endif
|
||||
.onEnter = cmsx_Vtx_onEnter,
|
||||
.onExit= cmsx_Vtx_onExit,
|
||||
.onGlobalExit = cmsx_Vtx_FeatureWriteback,
|
||||
.entries = cmsx_menuVtxEntries
|
||||
.onEnter = cms_Vtx_onEnter,
|
||||
.onExit = NULL,
|
||||
.onGlobalExit = NULL,
|
||||
.entries = cms_menuVtxEntries
|
||||
};
|
||||
|
||||
#endif // VTX || USE_RTC6705
|
||||
#endif // CMS
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
extern const CMS_Menu cmsx_menuVtx;
|
||||
extern const CMS_Menu cmsx_menuVtxControl;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -40,6 +40,9 @@
|
|||
#include "sensors/pitotmeter.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);
|
||||
|
||||
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
|
||||
|
|
@ -232,6 +235,42 @@ static int logicConditionGetFlightOperandValue(int operand) {
|
|||
return constrain(attitude.values.pitch / 10, -180, 180);
|
||||
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:
|
||||
return 0;
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -56,23 +56,32 @@ typedef enum logicOperandType_s {
|
|||
} logicOperandType_e;
|
||||
|
||||
typedef enum {
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI,
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS,
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // %
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s // 0
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m // 1
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m // 2
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_RSSI, // 3
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10 // 4
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10 // 5
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100 // 6
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh // 7
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS, // 8
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s // 9
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s // 10
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s // 11
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm // 12
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s // 13
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // % // 14
|
||||
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg // 15
|
||||
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;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -49,9 +49,9 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
|
|||
}
|
||||
|
||||
// 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.
|
||||
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;
|
||||
// cached value of RCC->CSR
|
||||
uint32_t cachedRccCsrValue;
|
||||
|
|
@ -76,7 +76,7 @@ void cycleCounterInit(void)
|
|||
|
||||
// SysTick
|
||||
|
||||
static volatile int sysTickPending = 0;
|
||||
static EXTENDED_FASTRAM volatile int sysTickPending = 0;
|
||||
|
||||
void SysTick_Handler(void)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
if (vtxDevice && vtxDevice->vTable->getBandAndChannel) {
|
||||
|
|
@ -150,3 +143,35 @@ bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t
|
|||
}
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,19 +30,12 @@
|
|||
|
||||
#define VTX_SETTINGS_DEFAULT_BAND 4
|
||||
#define VTX_SETTINGS_DEFAULT_CHANNEL 1
|
||||
#define VTX_SETTINGS_DEFAULT_FREQ 5740
|
||||
#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0
|
||||
#define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1
|
||||
#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_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)
|
||||
|
||||
#define VTX_SETTINGS_POWER_COUNT 5
|
||||
|
|
@ -51,14 +44,7 @@
|
|||
#define VTX_SETTINGS_MIN_USER_FREQ 5000
|
||||
#define VTX_SETTINGS_MAX_USER_FREQ 5999
|
||||
#define VTX_SETTINGS_FREQCMD
|
||||
#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)
|
||||
#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -68,7 +54,7 @@
|
|||
|
||||
typedef enum {
|
||||
VTXDEV_UNSUPPORTED = 0, // reserved for MSP
|
||||
VTXDEV_RTC6705 = 1,
|
||||
VTXDEV_RTC6705 = 1, // deprecated
|
||||
// 2 reserved
|
||||
VTXDEV_SMARTAUDIO = 3,
|
||||
VTXDEV_TRAMP = 4,
|
||||
|
|
@ -82,23 +68,32 @@ typedef struct vtxDeviceCapability_s {
|
|||
uint8_t bandCount;
|
||||
uint8_t channelCount;
|
||||
uint8_t powerCount;
|
||||
char **bandNames;
|
||||
char **channelNames;
|
||||
char **powerNames;
|
||||
} 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 {
|
||||
const struct vtxVTable_s * const vTable;
|
||||
|
||||
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 channel; // CH1 = 1, 1-based
|
||||
uint8_t powerIndex; // Lowest/Off = 0
|
||||
uint8_t pitMode; // 0 = non-PIT, 1 = PIT
|
||||
|
||||
} vtxDevice_t;
|
||||
|
||||
// {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 (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level);
|
||||
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 (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
|
||||
bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
|
||||
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;
|
||||
|
||||
// 3.1.0
|
||||
|
|
@ -137,9 +134,10 @@ bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice);
|
|||
void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
|
||||
void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index);
|
||||
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 vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex);
|
||||
bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff);
|
||||
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
|
|
@ -67,6 +67,7 @@ extern uint8_t __config_end;
|
|||
#include "drivers/time.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/usb_msc.h"
|
||||
#include "drivers/vtx_common.h"
|
||||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/cli.h"
|
||||
|
|
@ -145,7 +146,7 @@ static bool commandBatchError = false;
|
|||
// sync this with features_e
|
||||
static const char * const featureNames[] = {
|
||||
"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", "",
|
||||
"", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
|
||||
"BLACKBOX", "", "TRANSPONDER", "AIRMODE",
|
||||
|
|
@ -1321,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
|
|||
} else if (sl_strcasecmp(cmdline, "save") == 0) {
|
||||
posControl.waypointListValid = false;
|
||||
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) {
|
||||
posControl.waypointCount = i + 1;
|
||||
posControl.waypointListValid = true;
|
||||
|
|
@ -1907,7 +1908,7 @@ static void cliGlobalFunctions(char *cmdline) {
|
|||
if (
|
||||
i >= 0 && i < MAX_GLOBAL_FUNCTIONS &&
|
||||
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[VALUE_TYPE] >= 0 && args[VALUE_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
|
||||
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);
|
||||
#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 (getPwmInitError() != PWM_INIT_ERROR_NONE) {
|
||||
cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());
|
||||
|
|
@ -3146,13 +3170,13 @@ static void printConfig(const char *cmdline, bool doDiff)
|
|||
//printResource(dumpMask, &defaultConfig);
|
||||
|
||||
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));
|
||||
|
||||
// print custom servo mixer if exists
|
||||
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));
|
||||
|
||||
// print servo parameters
|
||||
|
|
|
|||
|
|
@ -181,13 +181,7 @@ void validateAndFixConfig(void)
|
|||
}
|
||||
|
||||
// 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);
|
||||
|
||||
#if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM)
|
||||
if (rxConfig()->receiverType == RX_TYPE_PWM) {
|
||||
rxConfigMutable()->receiverType = RX_TYPE_NONE;
|
||||
}
|
||||
#endif
|
||||
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(USE_RX_PPM)
|
||||
if (rxConfig()->receiverType == RX_TYPE_PPM) {
|
||||
|
|
@ -196,16 +190,6 @@ void validateAndFixConfig(void)
|
|||
#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 (featureConfigured(FEATURE_SOFTSERIAL) && featureConfigured(FEATURE_LED_STRIP)) {
|
||||
const timerHardware_t *ledTimerHardware = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ typedef enum {
|
|||
FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command
|
||||
FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
|
||||
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_GPS = 1 << 7,
|
||||
FEATURE_UNUSED_3 = 1 << 8, // was FEATURE_FAILSAFE
|
||||
|
|
|
|||
|
|
@ -74,7 +74,6 @@
|
|||
#include "drivers/io.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io_pca9685.h"
|
||||
#include "drivers/vtx_rtc6705.h"
|
||||
#include "drivers/vtx_common.h"
|
||||
#ifdef USE_USB_MSC
|
||||
#include "drivers/usb_msc.h"
|
||||
|
|
@ -253,10 +252,10 @@ void init(void)
|
|||
|
||||
#if defined(AVOID_UART2_FOR_PWM_PPM)
|
||||
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)
|
||||
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
|
||||
serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
|
||||
#endif
|
||||
|
|
@ -299,37 +298,6 @@ void init(void)
|
|||
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;
|
||||
|
||||
#ifdef BEEPER
|
||||
|
|
|
|||
|
|
@ -1419,7 +1419,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
break;
|
||||
|
||||
case MSP2_INAV_MIXER:
|
||||
sbufWriteU8(dst, mixerConfig()->yaw_motor_direction);
|
||||
sbufWriteU8(dst, mixerConfig()->motorDirectionInverted);
|
||||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU8(dst, mixerConfig()->platformType);
|
||||
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;
|
||||
vtxSettingsConfigMutable()->band = newBand;
|
||||
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) {
|
||||
|
|
@ -2749,7 +2744,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP2_INAV_SET_MIXER:
|
||||
mixerConfigMutable()->yaw_motor_direction = sbufReadU8(src);
|
||||
mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src);
|
||||
sbufReadU16(src); // Was yaw_jump_prevention_limit
|
||||
mixerConfigMutable()->platformType = sbufReadU8(src);
|
||||
mixerConfigMutable()->hasFlaps = sbufReadU8(src);
|
||||
|
|
|
|||
|
|
@ -130,6 +130,7 @@ typedef enum {
|
|||
ALTITUDE_CONTROL = (1 << 21), //It means it can fly
|
||||
MOVE_FORWARD_ONLY = (1 << 22),
|
||||
SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23),
|
||||
FW_HEADING_USE_YAW = (1 << 24),
|
||||
} stateFlags_t;
|
||||
|
||||
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ tables:
|
|||
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"]
|
||||
enum: pitotSensor_e
|
||||
- name: receiver_type
|
||||
values: ["NONE", "PWM", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
|
||||
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
|
||||
enum: rxReceiverType_e
|
||||
- name: serial_rx
|
||||
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",
|
||||
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
|
||||
"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
|
||||
values: ["NONE", "GYRO", "ALL"]
|
||||
- name: aux_operator
|
||||
|
|
@ -186,22 +186,21 @@ groups:
|
|||
- name: gyro_stage2_lowpass_type
|
||||
field: gyro_stage2_lowpass_type
|
||||
table: filter_type
|
||||
- name: dyn_notch_width_percent
|
||||
field: dyn_notch_width_percent
|
||||
- name: dynamic_gyro_notch_enabled
|
||||
field: dynamicGyroNotchEnabled
|
||||
condition: USE_DYNAMIC_FILTERS
|
||||
min: 0
|
||||
max: 20
|
||||
- name: dyn_notch_range
|
||||
field: dyn_notch_range
|
||||
type: bool
|
||||
- name: dynamic_gyro_notch_range
|
||||
field: dynamicGyroNotchRange
|
||||
condition: USE_DYNAMIC_FILTERS
|
||||
table: dynamicFilterRangeTable
|
||||
- name: dyn_notch_q
|
||||
field: dyn_notch_q
|
||||
- name: dynamic_gyro_notch_q
|
||||
field: dynamicGyroNotchQ
|
||||
condition: USE_DYNAMIC_FILTERS
|
||||
min: 1
|
||||
max: 1000
|
||||
- name: dyn_notch_min_hz
|
||||
field: dyn_notch_min_hz
|
||||
- name: dynamic_gyro_notch_min_hz
|
||||
field: dynamicGyroNotchMinHz
|
||||
condition: USE_DYNAMIC_FILTERS
|
||||
min: 60
|
||||
max: 1000
|
||||
|
|
@ -685,9 +684,9 @@ groups:
|
|||
- name: PG_MIXER_CONFIG
|
||||
type: mixerConfig_t
|
||||
members:
|
||||
- name: yaw_motor_direction
|
||||
min: -1
|
||||
max: 1
|
||||
- name: motor_direction_inverted
|
||||
field: motorDirectionInverted
|
||||
type: bool
|
||||
- name: platform_type
|
||||
field: platformType
|
||||
type: uint8_t
|
||||
|
|
@ -1220,6 +1219,25 @@ groups:
|
|||
condition: USE_NAV
|
||||
min: 0
|
||||
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
|
||||
field: iterm_relax_type
|
||||
table: iterm_relax_type
|
||||
|
|
@ -1636,6 +1654,13 @@ groups:
|
|||
- name: nav_fw_allow_manual_thr_increase
|
||||
field: fw.allow_manual_thr_increase
|
||||
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
|
||||
type: telemetryConfig_t
|
||||
|
|
@ -1673,8 +1698,8 @@ groups:
|
|||
field: hottAlarmSoundInterval
|
||||
min: 0
|
||||
max: 120
|
||||
- name: telemetry_uart_unidir
|
||||
field: uartUnidirectional
|
||||
- name: telemetry_halfduplex
|
||||
field: halfDuplex
|
||||
type: bool
|
||||
- name: smartport_fuel_unit
|
||||
field: smartportFuelUnit
|
||||
|
|
@ -2062,16 +2087,10 @@ groups:
|
|||
field: lowPowerDisarm
|
||||
table: vtx_low_power_disarm
|
||||
type: uint8_t
|
||||
- name: vtx_freq
|
||||
field: freq
|
||||
min: VTX_SETTINGS_MIN_FREQUENCY_MHZ
|
||||
max: VTX_SETTINGS_MAX_FREQUENCY_MHZ
|
||||
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: vtx_pit_mode_chan
|
||||
field: pitModeChan
|
||||
min: VTX_SETTINGS_MIN_CHANNEL
|
||||
max: VTX_SETTINGS_MAX_CHANNEL
|
||||
|
||||
- name: PG_PINIOBOX_CONFIG
|
||||
type: pinioBoxConfig_t
|
||||
|
|
|
|||
86
src/main/flight/dynamic_gyro_notch.c
Normal file
86
src/main/flight/dynamic_gyro_notch.c
Normal 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
50
src/main/flight/dynamic_gyro_notch.h
Normal 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);
|
||||
|
|
@ -35,6 +35,7 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/time.h"
|
||||
|
|
@ -55,68 +56,40 @@
|
|||
// we need 4 steps for each axis
|
||||
#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;
|
||||
static float EXTENDED_FASTRAM fftResolution;
|
||||
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;
|
||||
if (range == DYN_NOTCH_RANGE_HIGH) {
|
||||
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
|
||||
}
|
||||
|
||||
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
|
||||
}
|
||||
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
|
||||
else if (range == DYN_NOTCH_RANGE_MEDIUM) {
|
||||
state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
|
||||
}
|
||||
|
||||
// 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)
|
||||
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++) {
|
||||
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;
|
||||
state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
|
||||
state->maxSampleCount = samplingFrequency / state->fftSamplingRateHz;
|
||||
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
|
||||
|
||||
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
|
||||
// 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
|
||||
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++) {
|
||||
// any init value
|
||||
state->centerFreq[axis] = dynNotchMaxCtrHz;
|
||||
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
|
||||
state->centerFreq[axis] = state->maxFrequency;
|
||||
state->prevCenterFreq[axis] = state->maxFrequency;
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
*/
|
||||
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`
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
state->sampleCount++;
|
||||
|
|
@ -169,7 +144,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
|
|||
|
||||
// calculate FFT and update filters
|
||||
if (state->updateTicks > 0) {
|
||||
gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
|
||||
gyroDataAnalyseUpdate(state);
|
||||
--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
|
||||
*/
|
||||
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
|
||||
static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
||||
{
|
||||
enum {
|
||||
STEP_ARM_CFFT_F32,
|
||||
|
|
@ -245,7 +220,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
|
|||
uint8_t binStart = 0;
|
||||
uint8_t binMax = 0;
|
||||
//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) {
|
||||
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)
|
||||
float centerFreq = dynNotchMaxCtrHz;
|
||||
float centerFreq = state->maxFrequency;
|
||||
float fftMeanIndex = 0;
|
||||
// idx was shifted by 1 to start at 1, not 0
|
||||
if (fftSum > 0) {
|
||||
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
||||
// 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 {
|
||||
centerFreq = state->prevCenterFreq[state->updateAxis];
|
||||
}
|
||||
centerFreq = fmax(centerFreq, dynNotchMinHz);
|
||||
centerFreq = fmax(centerFreq, state->minFrequency);
|
||||
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
|
||||
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
||||
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;
|
||||
}
|
||||
case STEP_UPDATE_FILTERS:
|
||||
|
|
@ -307,13 +278,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
|
|||
// 7us
|
||||
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
||||
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
|
||||
|
||||
if (dualNotch) {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
|
||||
} else {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
|
||||
}
|
||||
/*
|
||||
* Filters will be updated inside dynamicGyroNotchFiltersUpdate()
|
||||
*/
|
||||
state->filterUpdateExecute = true;
|
||||
state->filterUpdateAxis = state->updateAxis;
|
||||
state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
|
||||
}
|
||||
|
||||
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
|
||||
// hanning starts and ends with 0, could be skipped for minor speed improvement
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
uint16_t getMaxFFT(void) {
|
||||
return dynNotchMaxFFT;
|
||||
}
|
||||
|
||||
void resetMaxFFT(void) {
|
||||
dynNotchMaxFFT = 0;
|
||||
}
|
||||
|
||||
#endif // USE_DYNAMIC_FILTERS
|
||||
|
|
|
|||
|
|
@ -51,13 +51,44 @@ typedef struct gyroAnalyseState_s {
|
|||
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
|
||||
uint16_t centerFreq[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;
|
||||
|
||||
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 gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
|
||||
uint16_t getMaxFFT(void);
|
||||
void resetMaxFFT(void);
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse);
|
||||
#endif
|
||||
|
|
@ -68,6 +68,7 @@ static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
|
|||
static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
|
||||
static EXTENDED_FASTRAM int throttleRangeMin = 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);
|
||||
|
||||
|
|
@ -77,10 +78,10 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
|
|||
.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,
|
||||
.yaw_motor_direction = 1,
|
||||
.motorDirectionInverted = 0,
|
||||
.platformType = PLATFORM_MULTIROTOR,
|
||||
.hasFlaps = false,
|
||||
.appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
|
||||
|
|
@ -253,6 +254,12 @@ void mixerInit(void)
|
|||
} else {
|
||||
motorRateLimitingApplyFn = nullMotorRateLimiting;
|
||||
}
|
||||
|
||||
if (mixerConfig()->motorDirectionInverted) {
|
||||
motorYawMultiplier = -1;
|
||||
} else {
|
||||
motorYawMultiplier = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void mixerResetDisarmedMotors(void)
|
||||
|
|
@ -457,7 +464,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
|||
rpyMix[i] =
|
||||
(input[PITCH] * currentMixer[i].pitch +
|
||||
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] < rpyMixMin) rpyMixMin = rpyMix[i];
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ typedef struct motorMixer_s {
|
|||
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
|
||||
|
||||
typedef struct mixerConfig_s {
|
||||
int8_t yaw_motor_direction;
|
||||
int8_t motorDirectionInverted;
|
||||
uint8_t platformType;
|
||||
bool hasFlaps;
|
||||
int16_t appliedMixerPreset;
|
||||
|
|
|
|||
|
|
@ -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 uint8_t itermRelax;
|
||||
static EXTENDED_FASTRAM uint8_t itermRelaxType;
|
||||
static EXTENDED_FASTRAM float itermRelaxSetpointThreshold;
|
||||
|
||||
#ifdef USE_ANTIGRAVITY
|
||||
static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
|
||||
|
|
@ -186,6 +185,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
.I = 50, // NAV_VEL_Z_I * 20
|
||||
.D = 10, // NAV_VEL_Z_D * 100
|
||||
.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
|
||||
.D = 8, // FW_POS_XY_D * 100
|
||||
.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,
|
||||
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
|
||||
.pidControllerType = PID_TYPE_AUTO,
|
||||
.navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
|
||||
);
|
||||
|
||||
bool pidInitFilters(void)
|
||||
|
|
@ -643,7 +655,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
|
|||
if (itermRelax) {
|
||||
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) {
|
||||
*itermErrorRate *= itermRelaxFactor;
|
||||
|
|
@ -1038,7 +1050,6 @@ void pidInit(void)
|
|||
|
||||
itermRelax = pidProfile()->iterm_relax;
|
||||
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;
|
||||
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@ typedef enum {
|
|||
PID_LEVEL, // + +
|
||||
PID_HEADING, // + +
|
||||
PID_VEL_Z, // + n/a
|
||||
PID_POS_HEADING,// n/a +
|
||||
PID_ITEM_COUNT
|
||||
} pidIndex_e;
|
||||
|
||||
|
|
@ -148,6 +149,8 @@ typedef struct pidProfile_s {
|
|||
float antigravityGain;
|
||||
float antigravityAccelerator;
|
||||
uint8_t antigravityCutoff;
|
||||
|
||||
int navFwPosHdgPidsumLimit;
|
||||
} pidProfile_t;
|
||||
|
||||
typedef struct pidAutotuneConfig_s {
|
||||
|
|
|
|||
|
|
@ -530,14 +530,6 @@ static uint16_t osdConvertRSSI(void)
|
|||
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
|
||||
* @param label to display
|
||||
|
|
@ -659,6 +651,8 @@ static const char * osdArmingDisabledReasonMessage(void)
|
|||
return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
|
||||
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_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
|
||||
break;
|
||||
|
|
@ -1681,34 +1675,26 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
}
|
||||
|
||||
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;
|
||||
uint8_t channel = 0;
|
||||
char bandChr = '-';
|
||||
const char *channelStr = "-";
|
||||
if (vtxCommonGetBandAndChannel(vtxCommonDevice(), &band, &channel)) {
|
||||
bandChr = vtx58BandLetter[band];
|
||||
channelStr = vtx58ChannelNames[channel];
|
||||
}
|
||||
tfp_sprintf(buff, "CH:%c%s:", bandChr, channelStr);
|
||||
vtxDeviceOsdInfo_t osdInfo;
|
||||
vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
|
||||
|
||||
tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName);
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
|
||||
|
||||
osdGetVTXPowerChar(buff);
|
||||
tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
|
||||
if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
||||
displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr);
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
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);
|
||||
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -42,14 +42,13 @@
|
|||
#include "io/vtx_string.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,
|
||||
.band = VTX_SETTINGS_DEFAULT_BAND,
|
||||
.channel = VTX_SETTINGS_DEFAULT_CHANNEL,
|
||||
.power = VTX_SETTINGS_DEFAULT_POWER,
|
||||
.freq = VTX_SETTINGS_DEFAULT_FREQ,
|
||||
.pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ,
|
||||
.pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL,
|
||||
.lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF,
|
||||
);
|
||||
|
||||
|
|
@ -63,51 +62,17 @@ typedef enum {
|
|||
|
||||
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)
|
||||
// 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)
|
||||
static vtxSettingsConfig_t * vtxGetRuntimeSettings(void)
|
||||
{
|
||||
vtxSettingsConfig_t settings = {
|
||||
.band = vtxSettingsConfig()->band,
|
||||
.channel = vtxSettingsConfig()->channel,
|
||||
.power = vtxSettingsConfig()->power,
|
||||
.freq = vtxSettingsConfig()->freq,
|
||||
.pitModeFreq = vtxSettingsConfig()->pitModeFreq,
|
||||
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
|
||||
};
|
||||
static vtxSettingsConfig_t settings;
|
||||
|
||||
#if 0
|
||||
#if defined(VTX_SETTINGS_FREQCMD)
|
||||
if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) {
|
||||
settings.band = 0;
|
||||
settings.freq = settings.pitModeFreq;
|
||||
settings.power = VTX_SETTINGS_DEFAULT_POWER;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
settings.band = vtxSettingsConfig()->band;
|
||||
settings.channel = vtxSettingsConfig()->channel;
|
||||
settings.power = vtxSettingsConfig()->power;
|
||||
settings.pitModeChan = vtxSettingsConfig()->pitModeChan;
|
||||
settings.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm;
|
||||
|
||||
if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
|
||||
((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) ||
|
||||
|
|
@ -116,56 +81,46 @@ static vtxSettingsConfig_t vtxGetSettings(void)
|
|||
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)) {
|
||||
uint8_t vtxBand;
|
||||
uint8_t vtxChan;
|
||||
if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
|
||||
const vtxSettingsConfig_t settings = vtxGetSettings();
|
||||
if (vtxBand != settings.band || vtxChan != settings.channel) {
|
||||
vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
|
||||
return true;
|
||||
}
|
||||
if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
#if defined(VTX_SETTINGS_FREQCMD)
|
||||
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;
|
||||
if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
|
||||
const vtxSettingsConfig_t settings = vtxGetSettings();
|
||||
if (vtxPower != settings.power) {
|
||||
vtxCommonSetPowerByIndex(vtxDevice, settings.power);
|
||||
if (vtxBand != runtimeSettings->band || vtxChan != runtimeSettings->channel) {
|
||||
vtxCommonSetBandAndChannel(vtxDevice, runtimeSettings->band, runtimeSettings->channel);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
|
||||
static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
|
||||
{
|
||||
uint8_t vtxPower;
|
||||
if (!vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (vtxPower != runtimeSettings->power) {
|
||||
vtxCommonSetPowerByIndex(vtxDevice, runtimeSettings->power);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
|
||||
{
|
||||
uint8_t pitOnOff;
|
||||
|
||||
|
|
@ -173,25 +128,10 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
|
|||
static bool prevPmSwitchState = false;
|
||||
|
||||
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) {
|
||||
prevPmSwitchState = currPmSwitchState;
|
||||
|
||||
if (currPmSwitchState) {
|
||||
#if defined(VTX_SETTINGS_FREQCMD)
|
||||
if (vtxSettingsConfig()->pitModeFreq) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
|
||||
#endif
|
||||
if (0) {
|
||||
if (!pitOnOff) {
|
||||
vtxCommonSetPitMode(vtxDevice, true);
|
||||
|
|
@ -209,22 +149,18 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice)
|
||||
static bool vtxProcessCheckParameters(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
|
||||
{
|
||||
const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings();
|
||||
vtxSettingsConfig_t vtxState = vtxSettingsState;
|
||||
uint8_t vtxBand;
|
||||
uint8_t vtxChan;
|
||||
uint8_t vtxPower;
|
||||
|
||||
if (vtxSettingsState.band) {
|
||||
vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel);
|
||||
#if defined(VTX_SETTINGS_FREQCMD)
|
||||
} else {
|
||||
vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
|
||||
#endif
|
||||
}
|
||||
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
|
||||
vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan);
|
||||
|
||||
vtxCommonGetPowerIndex(vtxDevice, &vtxState.power);
|
||||
|
||||
return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t));
|
||||
return (runtimeSettings->band && runtimeSettings->band != vtxBand) ||
|
||||
(runtimeSettings->channel != vtxChan) ||
|
||||
(runtimeSettings->power != vtxPower);
|
||||
}
|
||||
|
||||
void vtxUpdate(timeUs_t currentTimeUs)
|
||||
|
|
@ -240,36 +176,32 @@ void vtxUpdate(timeUs_t currentTimeUs)
|
|||
// Check input sources for config updates
|
||||
vtxControlInputPoll();
|
||||
|
||||
const uint8_t startingSchedule = currentSchedule;
|
||||
// Build runtime settings
|
||||
const vtxSettingsConfig_t * runtimeSettings = vtxGetRuntimeSettings();
|
||||
|
||||
bool vtxUpdatePending = false;
|
||||
do {
|
||||
switch (currentSchedule) {
|
||||
case VTX_PARAM_POWER:
|
||||
vtxUpdatePending = vtxProcessPower(vtxDevice);
|
||||
break;
|
||||
case VTX_PARAM_BANDCHAN:
|
||||
if (vtxGetSettings().band) {
|
||||
vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
|
||||
#if defined(VTX_SETTINGS_FREQCMD)
|
||||
} else {
|
||||
vtxUpdatePending = vtxProcessFrequency(vtxDevice);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
case VTX_PARAM_PITMODE:
|
||||
vtxUpdatePending = vtxProcessPitMode(vtxDevice);
|
||||
break;
|
||||
case VTX_PARAM_CONFIRM:
|
||||
vtxUpdatePending = vtxProcessStateUpdate(vtxDevice);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
|
||||
} while (!vtxUpdatePending && currentSchedule != startingSchedule);
|
||||
|
||||
switch (currentSchedule) {
|
||||
case VTX_PARAM_POWER:
|
||||
vtxUpdatePending = vtxProcessPower(vtxDevice, runtimeSettings);
|
||||
break;
|
||||
case VTX_PARAM_BANDCHAN:
|
||||
vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice, runtimeSettings);
|
||||
break;
|
||||
case VTX_PARAM_PITMODE:
|
||||
vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings);
|
||||
break;
|
||||
case VTX_PARAM_CONFIRM:
|
||||
vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
|
||||
vtxCommonProcess(vtxDevice, currentTimeUs);
|
||||
}
|
||||
|
||||
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 channel; // 1-8
|
||||
uint8_t power; // 0 = lowest
|
||||
uint16_t freq; // sets freq in MHz if band=0
|
||||
uint16_t pitModeFreq; // sets out-of-range pitmode frequency
|
||||
uint16_t pitModeChan; // sets out-of-range pitmode frequency
|
||||
uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e
|
||||
} vtxSettingsConfig_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@
|
|||
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
|
||||
// .vtxChannelActivationConditions = { 0 },
|
||||
.halfDuplex = true,
|
||||
);
|
||||
|
||||
|
|
|
|||
|
|
@ -33,15 +33,6 @@ typedef struct vtxConfig_s {
|
|||
uint8_t halfDuplex;
|
||||
} 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);
|
||||
|
||||
void vtxControlInit(void);
|
||||
|
|
|
|||
|
|
@ -41,8 +41,6 @@
|
|||
|
||||
#include "scheduler/protothreads.h"
|
||||
|
||||
//#include "cms/cms_menu_vtx_ffpv24g.h"
|
||||
|
||||
#include "io/vtx.h"
|
||||
#include "io/vtx_ffpv24g.h"
|
||||
#include "io/vtx_control.h"
|
||||
|
|
@ -85,7 +83,6 @@ typedef struct {
|
|||
|
||||
// Requested VTX state
|
||||
struct {
|
||||
bool setByFrequency;
|
||||
int band;
|
||||
int channel;
|
||||
unsigned freq;
|
||||
|
|
@ -108,9 +105,9 @@ typedef struct {
|
|||
|
||||
/*****************************************************************************/
|
||||
const char * const ffpvBandNames[VTX_FFPV_BAND_COUNT + 1] = {
|
||||
"--------",
|
||||
"FFPV 2.4 A",
|
||||
"FFPV 2.4 B",
|
||||
"-----",
|
||||
"A 2.4",
|
||||
"B 2.4",
|
||||
};
|
||||
|
||||
const char * ffpvBandLetters = "-AB";
|
||||
|
|
@ -353,18 +350,10 @@ static bool impl_DevSetFreq(uint16_t freq)
|
|||
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);
|
||||
|
||||
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
|
||||
if (band < VTX_FFPV_MIN_BAND || band > VTX_FFPV_MAX_BAND || channel < VTX_FFPV_MIN_CHANNEL || channel > VTX_FFPV_MAX_CHANNEL) {
|
||||
return;
|
||||
|
|
@ -372,20 +361,12 @@ void ffpvSetBandAndChannel(uint8_t band, uint8_t channel)
|
|||
|
||||
if (impl_DevSetFreq(ffpvFrequencyTable[band - 1][channel - 1])) {
|
||||
// Keep track of band/channel data
|
||||
vtxState.request.setByFrequency = false;
|
||||
vtxState.request.band = band;
|
||||
vtxState.request.channel = channel;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
|
||||
{
|
||||
UNUSED(vtxDevice);
|
||||
ffpvSetBandAndChannel(band, channel);
|
||||
}
|
||||
|
||||
void ffpvSetRFPowerByIndex(uint16_t index)
|
||||
static void ffpvSetRFPowerByIndex(uint16_t index)
|
||||
{
|
||||
// Validate index
|
||||
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
|
||||
*pBand = vtxState.request.setByFrequency ? 0 : vtxState.request.band;
|
||||
*pBand = vtxState.request.band;
|
||||
*pChannel = vtxState.request.channel;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -459,27 +440,33 @@ static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
|
|||
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) {
|
||||
state.pitMode = 0;
|
||||
state.band = vtxState.request.band;
|
||||
state.channel = vtxState.request.channel;
|
||||
state.frequency = vtxState.request.freq;
|
||||
state.powerIndex = vtxState.request.powerIndex;
|
||||
state.powerMilliwatt = vtxState.request.power;
|
||||
*pIndex = vtxState.request.powerIndex;
|
||||
*pPowerMw = vtxState.request.power;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
|
||||
{
|
||||
if (!impl_IsReady(vtxDevice)) {
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
state.pitMode = 0;
|
||||
state.band = 1;
|
||||
state.channel = 1;
|
||||
state.frequency = ffpvFrequencyTable[0][0];
|
||||
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,
|
||||
.setPowerByIndex = impl_SetPowerByIndex,
|
||||
.setPitMode = impl_SetPitMode,
|
||||
.setFrequency = impl_SetFreq,
|
||||
.getBandAndChannel = impl_GetBandAndChannel,
|
||||
.getPowerIndex = impl_GetPowerIndex,
|
||||
.getPitMode = impl_GetPitMode,
|
||||
.getFrequency = impl_GetFreq,
|
||||
.getPower = impl_GetPower,
|
||||
.getOsdInfo = impl_GetOsdInfo,
|
||||
};
|
||||
|
||||
static vtxDevice_t impl_vtxDevice = {
|
||||
|
|
@ -502,9 +490,9 @@ static vtxDevice_t impl_vtxDevice = {
|
|||
.capability.bandCount = VTX_FFPV_BAND_COUNT,
|
||||
.capability.channelCount = VTX_FFPV_CHANNEL_COUNT,
|
||||
.capability.powerCount = VTX_FFPV_POWER_COUNT,
|
||||
.bandNames = (char **)ffpvBandNames,
|
||||
.channelNames = (char **)ffpvChannelNames,
|
||||
.powerNames = (char **)ffpvPowerNames,
|
||||
.capability.bandNames = (char **)ffpvBandNames,
|
||||
.capability.channelNames = (char **)ffpvChannelNames,
|
||||
.capability.powerNames = (char **)ffpvPowerNames,
|
||||
};
|
||||
|
||||
bool vtxFuriousFPVInit(void)
|
||||
|
|
|
|||
|
|
@ -38,14 +38,4 @@
|
|||
#define VTX_FFPV_CHANNEL_COUNT 8
|
||||
#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);
|
||||
void ffpvSetBandAndChannel(uint8_t band, uint8_t channel);
|
||||
void ffpvSetRFPowerByIndex(uint16_t index);
|
||||
|
||||
vtxRunState_t * ffpvGetRuntimeState(void);
|
||||
|
|
@ -32,7 +32,6 @@
|
|||
#include "build/debug.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
#include "cms/cms_menu_vtx_smartaudio.h"
|
||||
|
||||
#include "common/log.h"
|
||||
#include "common/maths.h"
|
||||
|
|
@ -67,9 +66,9 @@ static vtxDevice_t vtxSmartAudio = {
|
|||
.capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
|
||||
.capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
|
||||
.capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
|
||||
.bandNames = (char **)vtx58BandNames,
|
||||
.channelNames = (char **)vtx58ChannelNames,
|
||||
.powerNames = (char **)saPowerNames,
|
||||
.capability.bandNames = (char **)vtx58BandNames,
|
||||
.capability.channelNames = (char **)vtx58ChannelNames,
|
||||
.capability.powerNames = (char **)saPowerNames,
|
||||
};
|
||||
|
||||
// SmartAudio command and response codes
|
||||
|
|
@ -332,7 +331,7 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
|
||||
if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
|
||||
#ifdef USE_CMS //if changes then trigger saCms update
|
||||
saCmsResetOpmodel();
|
||||
//saCmsResetOpmodel();
|
||||
#endif
|
||||
// Debug
|
||||
saPrintSettings();
|
||||
|
|
@ -341,7 +340,7 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
|
||||
#ifdef USE_CMS
|
||||
// Export current device status for CMS
|
||||
saCmsUpdate();
|
||||
//saCmsUpdate();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -540,11 +539,6 @@ static void saGetSettings(void)
|
|||
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 uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
|
||||
|
|
@ -620,7 +614,7 @@ void saSetMode(int mode)
|
|||
{
|
||||
static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
|
||||
|
||||
buf[4] = (mode & 0x3f)|saLockMode;
|
||||
buf[4] = (mode & 0x3f) | saLockMode;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
|
||||
saQueueCmd(buf, 6);
|
||||
|
|
@ -802,15 +796,6 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
|
|||
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)
|
||||
{
|
||||
if (!vtxSAIsReady(vtxDevice)) {
|
||||
|
|
@ -857,6 +842,50 @@ static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
|
|||
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 = {
|
||||
.process = vtxSAProcess,
|
||||
.getDeviceType = vtxSAGetDeviceType,
|
||||
|
|
@ -864,11 +893,12 @@ static const vtxVTable_t saVTable = {
|
|||
.setBandAndChannel = vtxSASetBandAndChannel,
|
||||
.setPowerByIndex = vtxSASetPowerByIndex,
|
||||
.setPitMode = vtxSASetPitMode,
|
||||
.setFrequency = vtxSASetFreq,
|
||||
.getBandAndChannel = vtxSAGetBandAndChannel,
|
||||
.getPowerIndex = vtxSAGetPowerIndex,
|
||||
.getPitMode = vtxSAGetPitMode,
|
||||
.getFrequency = vtxSAGetFreq,
|
||||
.getPower = vtxSAGetPower,
|
||||
.getOsdInfo = vtxSAGetOsdInfo,
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -25,8 +25,9 @@
|
|||
#include "platform.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#define VTX_STRING_BAND_COUNT 5
|
||||
#define VTX_STRING_CHAN_COUNT 8
|
||||
#define VTX_STRING_BAND_COUNT 5
|
||||
#define VTX_STRING_CHAN_COUNT 8
|
||||
#define VTX_STRING_POWER_COUNT 5
|
||||
|
||||
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",
|
||||
};
|
||||
|
||||
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)
|
||||
{
|
||||
int8_t band;
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
extern const uint16_t vtx58frequencyTable[5][8];
|
||||
extern const char * const vtx58BandNames[];
|
||||
extern const char * const vtx58ChannelNames[];
|
||||
extern const char * const vtx58DefaultPowerNames[];
|
||||
extern const char vtx58BandLetter[];
|
||||
|
||||
bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel);
|
||||
|
|
|
|||
|
|
@ -34,8 +34,6 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "cms/cms_menu_vtx_tramp.h"
|
||||
|
||||
#include "drivers/vtx_common.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
|
@ -44,7 +42,6 @@
|
|||
#include "io/vtx.h"
|
||||
#include "io/vtx_string.h"
|
||||
|
||||
#if defined(USE_CMS)
|
||||
const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
|
||||
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] = {
|
||||
"---", "25 ", "100", "200", "400", "600"
|
||||
};
|
||||
#endif
|
||||
|
||||
static const vtxVTable_t trampVTable; // forward
|
||||
static vtxDevice_t vtxTramp = {
|
||||
|
|
@ -60,9 +56,9 @@ static vtxDevice_t vtxTramp = {
|
|||
.capability.bandCount = VTX_TRAMP_BAND_COUNT,
|
||||
.capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
|
||||
.capability.powerCount = VTX_TRAMP_POWER_COUNT,
|
||||
.bandNames = (char **)vtx58BandNames,
|
||||
.channelNames = (char **)vtx58ChannelNames,
|
||||
.powerNames = (char **)trampPowerNames,
|
||||
.capability.bandNames = (char **)vtx58BandNames,
|
||||
.capability.channelNames = (char **)vtx58ChannelNames,
|
||||
.capability.powerNames = (char **)trampPowerNames,
|
||||
};
|
||||
|
||||
static serialPort_t *trampSerialPort = NULL;
|
||||
|
|
@ -126,11 +122,6 @@ void trampCmdU16(uint8_t cmd, uint16_t param)
|
|||
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)
|
||||
{
|
||||
trampConfFreq = freq;
|
||||
|
|
@ -511,15 +502,6 @@ static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t 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)
|
||||
{
|
||||
if (!vtxTrampIsReady(vtxDevice)) {
|
||||
|
|
@ -570,6 +552,40 @@ static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
|
|||
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 = {
|
||||
.process = vtxTrampProcess,
|
||||
.getDeviceType = vtxTrampGetDeviceType,
|
||||
|
|
@ -577,11 +593,12 @@ static const vtxVTable_t trampVTable = {
|
|||
.setBandAndChannel = vtxTrampSetBandAndChannel,
|
||||
.setPowerByIndex = vtxTrampSetPowerByIndex,
|
||||
.setPitMode = vtxTrampSetPitMode,
|
||||
.setFrequency = vtxTrampSetFreq,
|
||||
.getBandAndChannel = vtxTrampGetBandAndChannel,
|
||||
.getPowerIndex = vtxTrampGetPowerIndex,
|
||||
.getPitMode = vtxTrampGetPitMode,
|
||||
.getFrequency = vtxTrampGetFreq,
|
||||
.getPower = vtxTrampGetPower,
|
||||
.getOsdInfo = vtxTrampGetOsdInfo,
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
#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,
|
||||
.general = {
|
||||
|
|
@ -165,7 +165,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_climb_angle = 18, // 18 degrees
|
||||
.launch_max_angle = 45, // 45 deg
|
||||
.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] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_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,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_PROCESS_NEXT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.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_ERROR] = 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;
|
||||
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:
|
||||
posControl.rthState.rthInitialDistance = posControl.homeDistance;
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
|
|
@ -1407,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
}
|
||||
break;
|
||||
|
||||
case NAV_WP_ACTION_JUMP:
|
||||
UNREACHABLE();
|
||||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
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:
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
|
||||
|
||||
case NAV_WP_ACTION_JUMP:
|
||||
UNREACHABLE();
|
||||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -2600,9 +2639,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
|
||||
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)) {
|
||||
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)
|
||||
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -3210,6 +3260,13 @@ void navigationUsePIDs(void)
|
|||
0.0f,
|
||||
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)
|
||||
|
|
@ -3240,6 +3297,16 @@ void navigationInit(void)
|
|||
|
||||
/* Use system config */
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -101,6 +101,7 @@ typedef enum {
|
|||
NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1,
|
||||
NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2,
|
||||
NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3,
|
||||
NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4,
|
||||
} navArmingBlocker_e;
|
||||
|
||||
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 cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
bool allow_manual_thr_increase;
|
||||
bool useFwNavYawControl;
|
||||
uint8_t yawControlDeadband;
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
@ -231,6 +234,7 @@ typedef struct gpsOrigin_s {
|
|||
|
||||
typedef enum {
|
||||
NAV_WP_ACTION_WAYPOINT = 0x01,
|
||||
NAV_WP_ACTION_JUMP = 0x06,
|
||||
NAV_WP_ACTION_RTH = 0x04
|
||||
} navWaypointActions_e;
|
||||
|
||||
|
|
@ -308,6 +312,7 @@ typedef struct navigationPIDControllers_s {
|
|||
/* Fixed-wing PIDs */
|
||||
pidController_t fw_alt;
|
||||
pidController_t fw_nav;
|
||||
pidController_t fw_heading;
|
||||
} navigationPIDControllers_t;
|
||||
|
||||
/* MultiWii-compatible params for telemetry */
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@
|
|||
|
||||
static bool isPitchAdjustmentValid = false;
|
||||
static bool isRollAdjustmentValid = false;
|
||||
static bool isYawAdjustmentValid = false;
|
||||
static float throttleSpeedAdjustment = 0;
|
||||
static bool isAutoThrottleManuallyIncreased = false;
|
||||
static int32_t navHeadingError;
|
||||
|
|
@ -209,8 +210,11 @@ void resetFixedWingPositionController(void)
|
|||
virtualDesiredPosition.z = 0;
|
||||
|
||||
navPidReset(&posControl.pids.fw_nav);
|
||||
navPidReset(&posControl.pids.fw_heading);
|
||||
posControl.rcAdjustment[ROLL] = 0;
|
||||
posControl.rcAdjustment[YAW] = 0;
|
||||
isRollAdjustmentValid = false;
|
||||
isYawAdjustmentValid = false;
|
||||
|
||||
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
|
||||
}
|
||||
|
|
@ -292,7 +296,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
// We have virtual position target, calculate heading error
|
||||
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
|
||||
|
||||
// Calculate NAV heading error
|
||||
/*
|
||||
* Calculate NAV heading error
|
||||
* Units are centidegrees
|
||||
*/
|
||||
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
|
||||
|
||||
// Forced turn direction
|
||||
|
|
@ -333,6 +340,41 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
|
||||
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
|
||||
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)
|
||||
|
|
@ -376,10 +418,12 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
isRollAdjustmentValid = true;
|
||||
isYawAdjustmentValid = true;
|
||||
}
|
||||
else {
|
||||
// No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
|
||||
isRollAdjustmentValid = false;
|
||||
isYawAdjustmentValid = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -448,6 +492,10 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
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)) {
|
||||
// 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));
|
||||
|
|
|
|||
|
|
@ -104,7 +104,7 @@ static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
|||
rxRuntimeConfig_t rxRuntimeConfig;
|
||||
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
|
||||
#define RX_SPI_DEFAULT_PROTOCOL 0
|
||||
|
|
@ -327,7 +327,6 @@ void rxInit(void)
|
|||
|
||||
default:
|
||||
case RX_TYPE_NONE:
|
||||
case RX_TYPE_PWM:
|
||||
rxConfigMutable()->receiverType = RX_TYPE_NONE;
|
||||
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
|
||||
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
|
||||
|
|
|
|||
|
|
@ -59,12 +59,11 @@ typedef enum {
|
|||
|
||||
typedef enum {
|
||||
RX_TYPE_NONE = 0,
|
||||
RX_TYPE_PWM = 1,
|
||||
RX_TYPE_PPM = 2,
|
||||
RX_TYPE_SERIAL = 3,
|
||||
RX_TYPE_MSP = 4,
|
||||
RX_TYPE_SPI = 5,
|
||||
RX_TYPE_UIB = 6
|
||||
RX_TYPE_PPM = 1,
|
||||
RX_TYPE_SERIAL = 2,
|
||||
RX_TYPE_MSP = 3,
|
||||
RX_TYPE_SPI = 4,
|
||||
RX_TYPE_UIB = 5
|
||||
} rxReceiverType_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@
|
|||
|
||||
#include "flight/gyroanalyse.h"
|
||||
#include "flight/rpm_filter.h"
|
||||
#include "flight/dynamic_gyro_notch.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
|
|
@ -95,17 +96,12 @@ STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
|
|||
|
||||
#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 dynamicGyroNotchState_t dynamicGyroNotchState;
|
||||
|
||||
#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,
|
||||
.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_stage2_lowpass_hz = 0,
|
||||
.gyro_stage2_lowpass_type = FILTER_BIQUAD,
|
||||
.dyn_notch_width_percent = 8,
|
||||
.dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM,
|
||||
.dyn_notch_q = 120,
|
||||
.dyn_notch_min_hz = 150,
|
||||
.dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
|
||||
.dynamicGyroNotchQ = 120,
|
||||
.dynamicGyroNotchMinHz = 150,
|
||||
.dynamicGyroNotchEnabled = 0
|
||||
);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#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(¬chFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
|
||||
biquadFilterInit(¬chFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
bool gyroInit(void)
|
||||
{
|
||||
memset(&gyro, 0, sizeof(gyro));
|
||||
|
|
@ -322,8 +291,13 @@ bool gyroInit(void)
|
|||
|
||||
gyroInitFilters();
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
gyroInitFilterDynamicNotch();
|
||||
gyroDataAnalyseStateInit(&gyroAnalyseState, getLooptime());
|
||||
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
|
||||
gyroDataAnalyseStateInit(
|
||||
&gyroAnalyseState,
|
||||
gyroConfig()->dynamicGyroNotchMinHz,
|
||||
gyroConfig()->dynamicGyroNotchRange,
|
||||
getLooptime()
|
||||
);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
|
@ -468,18 +442,27 @@ void FAST_CODE NOINLINE gyroUpdate()
|
|||
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
if (isDynamicFilterActive()) {
|
||||
if (dynamicGyroNotchState.enabled) {
|
||||
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
|
||||
gyroADCf = notchFilterDynApplyFn((filter_t *)¬chFilterDyn[axis], gyroADCf);
|
||||
gyroADCf = notchFilterDynApplyFn2((filter_t *)¬chFilterDyn2[axis], gyroADCf);
|
||||
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis, gyroADCf);
|
||||
gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
|
||||
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf);
|
||||
}
|
||||
#endif
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
|
||||
if (dynamicGyroNotchState.enabled) {
|
||||
gyroDataAnalyse(&gyroAnalyseState);
|
||||
|
||||
if (gyroAnalyseState.filterUpdateExecute) {
|
||||
dynamicGyroNotchFiltersUpdate(
|
||||
&dynamicGyroNotchState,
|
||||
gyroAnalyseState.filterUpdateAxis,
|
||||
gyroAnalyseState.filterUpdateFrequency
|
||||
);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -71,10 +71,10 @@ typedef struct gyroConfig_s {
|
|||
uint16_t gyro_soft_notch_cutoff_2;
|
||||
uint16_t gyro_stage2_lowpass_hz;
|
||||
uint8_t gyro_stage2_lowpass_type;
|
||||
uint8_t dyn_notch_width_percent;
|
||||
uint8_t dyn_notch_range;
|
||||
uint16_t dyn_notch_q;
|
||||
uint16_t dyn_notch_min_hz;
|
||||
uint8_t dynamicGyroNotchRange;
|
||||
uint16_t dynamicGyroNotchQ;
|
||||
uint16_t dynamicGyroNotchMinHz;
|
||||
uint8_t dynamicGyroNotchEnabled;
|
||||
} gyroConfig_t;
|
||||
|
||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||
|
|
|
|||
|
|
@ -27,8 +27,8 @@ const timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM
|
||||
|
||||
// Motors
|
||||
DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2
|
||||
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_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, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1
|
||||
|
||||
|
|
|
|||
|
|
@ -180,3 +180,6 @@
|
|||
#define MAX_PWM_OUTPUT_PORTS 4
|
||||
|
||||
#define PCA9685_I2C_BUS BUS_I2C2
|
||||
|
||||
#define USE_DSHOT
|
||||
#define USE_ESC_SENSOR
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ void targetConfiguration(void)
|
|||
rxConfigMutable()->serialrx_provider = SERIALRX_CRSF;
|
||||
|
||||
serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
||||
telemetryConfigMutable()->uartUnidirectional = 1;
|
||||
|
||||
mixerConfigMutable()->platformType = PLATFORM_AIRPLANE;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,6 +28,5 @@
|
|||
void targetConfiguration(void)
|
||||
{
|
||||
rxConfigMutable()->halfDuplex = false;
|
||||
telemetryConfigMutable()->uartUnidirectional = true;
|
||||
batteryMetersConfigMutable()->current.scale = CURRENTSCALE;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -107,9 +107,6 @@
|
|||
#define SPI3_MISO_PIN PB4
|
||||
#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_SMARTAUDIO // 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_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 MAX7456_SPI_BUS BUS_SPI3
|
||||
#define MAX7456_CS_PIN PA15
|
||||
|
||||
#define SPI_SHARED_MAX7456_AND_RTC6705
|
||||
|
||||
#define USE_SDCARD
|
||||
#define USE_SDCARD_SPI
|
||||
#define SDCARD_DETECT_INVERTED
|
||||
|
|
|
|||
|
|
@ -15,5 +15,4 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_mag3110.c \
|
||||
drivers/compass/compass_lis3mdl.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/max7456.c \
|
||||
drivers/vtx_rtc6705.c
|
||||
drivers/max7456.c
|
||||
|
|
|
|||
|
|
@ -126,12 +126,6 @@
|
|||
#define SPI3_MISO_PIN PB4 // 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_SPI
|
||||
#define SDCARD_DETECT_INVERTED
|
||||
|
|
|
|||
|
|
@ -15,4 +15,3 @@ TARGET_SRC = \
|
|||
drivers/barometer/barometer_ms56xx.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/max7456.c
|
||||
# drivers/vtx_rtc6705.c
|
||||
|
|
|
|||
|
|
@ -155,16 +155,6 @@
|
|||
#define SDCARD_SPI_BUS BUS_SPI3
|
||||
#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_SPI_INSTANCE BUS_SPI1
|
||||
#define GYRO_2_CS_PIN PB2
|
||||
|
|
|
|||
|
|
@ -15,5 +15,4 @@ TARGET_SRC = \
|
|||
drivers/compass/compass_mag3110.c \
|
||||
drivers/compass/compass_lis3mdl.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/max7456.c \
|
||||
drivers/vtx_rtc6705_soft_spi.c
|
||||
drivers/max7456.c
|
||||
|
|
@ -19,11 +19,6 @@
|
|||
|
||||
#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
|
||||
#if !defined(USE_I2C)
|
||||
# undef USE_DASHBOARD
|
||||
|
|
|
|||
|
|
@ -368,7 +368,7 @@ void configureHoTTTelemetryPort(void)
|
|||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -306,7 +306,7 @@ static void freeSmartPortTelemetryPort(void)
|
|||
static void configureSmartPortTelemetryPort(void)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
|
|||
.frsky_pitch_roll = 0,
|
||||
.report_cell_voltage = 0,
|
||||
.hottAlarmSoundInterval = 5,
|
||||
.uartUnidirectional = 0,
|
||||
.halfDuplex = 1,
|
||||
.smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH,
|
||||
.ibusTelemetryType = 0,
|
||||
.ltmUpdateRate = LTM_RATE_NORMAL,
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ typedef struct telemetryConfig_s {
|
|||
uint8_t frsky_pitch_roll;
|
||||
uint8_t report_cell_voltage;
|
||||
uint8_t hottAlarmSoundInterval;
|
||||
uint8_t uartUnidirectional;
|
||||
uint8_t halfDuplex;
|
||||
smartportFuelUnit_e smartportFuelUnit;
|
||||
uint8_t ibusTelemetryType;
|
||||
uint8_t ltmUpdateRate;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue