Merge branch 'master_test'

This commit is contained in:
borisbstyle 2017-01-31 00:51:59 +01:00
commit 46e1526103
31 changed files with 810 additions and 530 deletions

View File

@ -591,6 +591,7 @@ COMMON_SRC = \
fc/fc_dispatch.c \ fc/fc_dispatch.c \
fc/fc_hardfaults.c \ fc/fc_hardfaults.c \
fc/fc_core.c \ fc/fc_core.c \
fc/fc_rc.c \
fc/fc_msp.c \ fc/fc_msp.c \
fc/fc_tasks.c \ fc/fc_tasks.c \
fc/rc_controls.c \ fc/rc_controls.c \
@ -712,7 +713,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/timer.c \ drivers/timer.c \
fc/fc_core.c \ fc/fc_core.c \
fc/fc_tasks.c \ fc/fc_tasks.c \
fc/mw.c \ fc/fc_rc.c \
fc/rc_controls.c \ fc/rc_controls.c \
fc/runtime_config.c \ fc/runtime_config.c \
flight/imu.c \ flight/imu.c \

View File

@ -755,7 +755,7 @@ static int gcd(int num, int denom)
return gcd(denom, num % denom); return gcd(denom, num % denom);
} }
static void validateBlackboxConfig() void validateBlackboxConfig()
{ {
int div; int div;
@ -1250,8 +1250,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", currentProfile->pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentProfile->pidProfile.itermWindupPointPercent);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", currentProfile->pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation);

View File

@ -32,6 +32,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void); void initBlackbox(void);
void handleBlackbox(timeUs_t currentTimeUs); void handleBlackbox(timeUs_t currentTimeUs);
void validateBlackboxConfig();
void startBlackbox(void); void startBlackbox(void);
void finishBlackbox(void); void finishBlackbox(void);

View File

@ -21,6 +21,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h>
#include <string.h> #include <string.h>
#include <ctype.h> #include <ctype.h>
@ -42,8 +43,11 @@
#include "config/config_master.h" #include "config/config_master.h"
#include "config/feature.h" #include "config/feature.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h" #include "io/flashfs.h"
#include "blackbox/blackbox_io.h"
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
{ {
@ -65,11 +69,99 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
} }
#endif // USE_FLASHFS #endif // USE_FLASHFS
static const char * const cmsx_BlackboxDeviceNames[] = {
"SERIAL",
"FLASH ",
"SDCARD"
};
static bool featureRead = false; static bool featureRead = false;
static uint8_t cmsx_FeatureBlackbox; static uint8_t cmsx_FeatureBlackbox;
static long cmsx_Blackbox_FeatureRead(void) static uint8_t cmsx_BlackboxDevice;
static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames };
#define CMS_BLACKBOX_STRING_LENGTH 8
static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH];
static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH];
static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH];
static void cmsx_Blackbox_GetDeviceStatus()
{ {
char * unit = "B";
bool storageDeviceIsWorking = false;
uint32_t storageUsed = 0;
uint32_t storageFree = 0;
switch (blackboxConfig()->device)
{
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
unit = "MB";
if (!sdcard_isInserted()) {
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "NO CARD");
} else if (!sdcard_isFunctional()) {
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT");
} else {
switch (afatfs_getFilesystemState()) {
case AFATFS_FILESYSTEM_STATE_READY:
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "READY");
storageDeviceIsWorking = true;
break;
case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "INIT");
break;
case AFATFS_FILESYSTEM_STATE_FATAL:
case AFATFS_FILESYSTEM_STATE_UNKNOWN:
default:
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT");
break;
}
}
if (storageDeviceIsWorking) {
storageFree = afatfs_getContiguousFreeSpace() / 1024000;
storageUsed = (sdcard_getMetadata()->numBlocks / 2000) - storageFree;
}
break;
#endif
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
unit = "KB";
storageDeviceIsWorking = flashfsIsReady();
if (storageDeviceIsWorking) {
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "READY");
const flashGeometry_t *geometry = flashfsGetGeometry();
storageUsed = flashfsGetOffset() / 1024;
storageFree = (geometry->totalSize / 1024) - storageUsed;
} else {
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT");
}
break;
#endif
default:
storageDeviceIsWorking = true;
snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "---");
}
/* Storage counters */
snprintf(cmsx_BlackboxDeviceStorageUsed, CMS_BLACKBOX_STRING_LENGTH, "%ld%s", storageUsed, unit);
snprintf(cmsx_BlackboxDeviceStorageFree, CMS_BLACKBOX_STRING_LENGTH, "%ld%s", storageFree, unit);
}
static long cmsx_Blackbox_onEnter(void)
{
cmsx_Blackbox_GetDeviceStatus();
cmsx_BlackboxDevice = blackboxConfig()->device;
if (!featureRead) { if (!featureRead) {
cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
featureRead = true; featureRead = true;
@ -78,6 +170,18 @@ static long cmsx_Blackbox_FeatureRead(void)
return 0; return 0;
} }
static long cmsx_Blackbox_onExit(const OSD_Entry *self)
{
UNUSED(self);
if (blackboxMayEditConfig()) {
blackboxConfig()->device = cmsx_BlackboxDevice;
validateBlackboxConfig();
}
return 0;
}
static long cmsx_Blackbox_FeatureWriteback(void) static long cmsx_Blackbox_FeatureWriteback(void)
{ {
if (featureRead) { if (featureRead) {
@ -94,7 +198,11 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
{ {
{ "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, { "-- BLACKBOX --", OME_Label, NULL, NULL, 0},
{ "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom,1,32,1 }, 0 }, { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, 0 },
{ "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 },
{ "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 },
{ "(FREE)", OME_String, NULL, &cmsx_BlackboxDeviceStorageFree, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 },
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
{ "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 },
@ -107,9 +215,10 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
CMS_Menu cmsx_menuBlackbox = { CMS_Menu cmsx_menuBlackbox = {
.GUARD_text = "MENUBB", .GUARD_text = "MENUBB",
.GUARD_type = OME_MENU, .GUARD_type = OME_MENU,
.onEnter = cmsx_Blackbox_FeatureRead, .onEnter = cmsx_Blackbox_onEnter,
.onExit = NULL, .onExit = cmsx_Blackbox_onExit,
.onGlobalExit = cmsx_Blackbox_FeatureWriteback, .onGlobalExit = cmsx_Blackbox_FeatureWriteback,
.entries = cmsx_menuBlackboxEntries .entries = cmsx_menuBlackboxEntries
}; };
#endif #endif

View File

@ -17,6 +17,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <string.h>
#include <math.h> #include <math.h>
#include "platform.h" #include "platform.h"
@ -108,6 +109,12 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
*port->ccr = 0; *port->ccr = 0;
} }
static void pwmWriteUnused(uint8_t index, uint16_t value)
{
UNUSED(index);
UNUSED(value);
}
static void pwmWriteBrushed(uint8_t index, uint16_t value) static void pwmWriteBrushed(uint8_t index, uint16_t value)
{ {
*motors[index].ccr = (value - 1000) * motors[index].period / 1000; *motors[index].ccr = (value - 1000) * motors[index].period / 1000;
@ -156,7 +163,8 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void) void pwmEnableMotors(void)
{ {
pwmMotorsEnabled = true; /* check motors can be enabled */
pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused);
} }
bool pwmAreMotorsEnabled(void) bool pwmAreMotorsEnabled(void)
@ -164,18 +172,15 @@ bool pwmAreMotorsEnabled(void)
return pwmMotorsEnabled; return pwmMotorsEnabled;
} }
static void pwmCompleteWriteUnused(uint8_t motorCount)
{
UNUSED(motorCount);
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{ {
for (int index = 0; index < motorCount; index++) { for (int index = 0; index < motorCount; index++) {
bool overflowed = false; if (motors[index].forceOverflow) {
// If we have not already overflowed this timer
for (int j = 0; j < index; j++) {
if (motors[j].tim == motors[index].tim) {
overflowed = true;
break;
}
}
if (!overflowed) {
timerForceOverflow(motors[index].tim); timerForceOverflow(motors[index].tim);
} }
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
@ -186,13 +191,13 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmCompleteMotorUpdate(uint8_t motorCount) void pwmCompleteMotorUpdate(uint8_t motorCount)
{ {
if (pwmCompleteWritePtr) {
pwmCompleteWritePtr(motorCount); pwmCompleteWritePtr(motorCount);
}
} }
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{ {
memset(motors, 0, sizeof(motors));
uint32_t timerMhzCounter = 0; uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false; bool isDigital = false;
@ -235,22 +240,20 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#endif #endif
} }
if (!useUnsyncedPwm && !isDigital) { if (!isDigital) {
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate; pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate;
} }
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex]; const ioTag_t tag = motorConfig->ioTags[motorIndex];
if (!tag) {
break;
}
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY); const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
if (timerHardware == NULL) { if (timerHardware == NULL) {
/* flag failure and disable ability to arm */ /* not enough motors initialised for the mixer or a break in the motors */
break; pwmWritePtr = pwmWriteUnused;
pwmCompleteWritePtr = pwmCompleteWriteUnused;
/* TODO: block arming and add reason system cannot arm */
return;
} }
motors[motorIndex].io = IOGetByTag(tag); motors[motorIndex].io = IOGetByTag(tag);
@ -277,8 +280,18 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
} else { } else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
} }
bool timerAlreadyUsed = false;
for (int i = 0; i < motorIndex; i++) {
if (motors[i].tim == motors[motorIndex].tim) {
timerAlreadyUsed = true;
break;
}
}
motors[motorIndex].forceOverflow = !timerAlreadyUsed;
motors[motorIndex].enabled = true; motors[motorIndex].enabled = true;
} }
pwmMotorsEnabled = true; pwmMotorsEnabled = true;
} }

View File

@ -105,6 +105,7 @@ typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointe
typedef struct { typedef struct {
volatile timCCR_t *ccr; volatile timCCR_t *ccr;
TIM_TypeDef *tim; TIM_TypeDef *tim;
bool forceOverflow;
uint16_t period; uint16_t period;
bool enabled; bool enabled;
IO_t io; IO_t io;

View File

@ -61,7 +61,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware->dmaChannel) { if (!motor->timerHardware || !motor->timerHardware->dmaChannel) {
return; return;
} }

View File

@ -59,7 +59,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware->dmaStream) { if (!motor->timerHardware || !motor->timerHardware->dmaStream) {
return; return;
} }

View File

@ -58,7 +58,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index]; motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware->dmaStream) { if (!motor->timerHardware || !motor->timerHardware->dmaStream) {
return; return;
} }

View File

@ -769,6 +769,10 @@ void timerForceOverflow(TIM_TypeDef *tim)
const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag)
{ {
if (!tag) {
return NULL;
}
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].tag == tag) { if (timerHardware[i].tag == tag) {
if (timerHardware[i].usageFlags & flag || flag == 0) { if (timerHardware[i].usageFlags & flag || flag == 0) {

View File

@ -509,14 +509,14 @@ static const clivalue_t valueTable[] = {
{ "max_check", VAR_UINT16 | MASTER_VALUE, &rxConfig()->maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &rxConfig()->maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &rxConfig()->rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &rxConfig()->rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
{ "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } }, { "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } },
{ "rc_interpolation_channels", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolationChannels, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS } }, { "rc_interp_ch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolationChannels, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS } },
{ "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rcInterpolationInterval, .config.minmax = { 1, 50 } }, { "rc_interp_int", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rcInterpolationInterval, .config.minmax = { 1, 50 } },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
#if defined(USE_PWM) #if defined(USE_PWM)
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, 13 } }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, 13 } },
{ "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
@ -581,41 +581,41 @@ static const clivalue_t valueTable[] = {
#ifdef SPEKTRUM_BIND #ifdef SPEKTRUM_BIND
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} },
{ "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} }, { "spektrum_sat_bind_autorst", VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} },
#endif #endif
#ifdef TELEMETRY #ifdef TELEMETRY
{ "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_switch, .config.lookup = { TABLE_OFF_ON } }, { "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_switch, .config.lookup = { TABLE_OFF_ON } },
{ "telemetry_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_inversion, .config.lookup = { TABLE_OFF_ON } }, { "tlm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_inversion, .config.lookup = { TABLE_OFF_ON } },
{ "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->sportHalfDuplex, .config.lookup = { TABLE_OFF_ON } }, { "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->sportHalfDuplex, .config.lookup = { TABLE_OFF_ON } },
{ "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } }, { "frsky_default_lat", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } },
{ "frsky_default_longitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLongitude, .config.minmax = { -180.0, 180.0 } }, { "frsky_default_long", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLongitude, .config.minmax = { -180.0, 180.0 } },
{ "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_coordinate_format, .config.minmax = { 0, FRSKY_FORMAT_NMEA } }, { "frsky_gps_format", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_coordinate_format, .config.minmax = { 0, FRSKY_FORMAT_NMEA } },
{ "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_unit, .config.lookup = { TABLE_UNIT } }, { "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_unit, .config.lookup = { TABLE_UNIT } },
{ "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } }, { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } },
{ "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } },
{ "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, { "hott_alarm_sound_int", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } },
{ "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, { "pid_in_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } },
#if defined(TELEMETRY_IBUS) #if defined(TELEMETRY_IBUS)
{ "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
#endif #endif
{ "battery_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, { "vbat_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } },
{ "vbat_scale", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } },
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmaxcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbathysteresis, .config.minmax = { 0, 250 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbathysteresis, .config.minmax = { 0, 250 } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, { "current_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } },
{ "current_meter_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, { "current_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } },
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "mwii_current_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
{ "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } }, { "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } },
{ "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, { "no_vbat_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } },
{ "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
{ "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, { "vbat_usage_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },
{ "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "vbat_usage_percent", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } },
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } },
@ -633,10 +633,10 @@ static const clivalue_t valueTable[] = {
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 16000 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 16000 } },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 16000 } }, { "gyro_notch1_cut", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 16000 } },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_2, .config.minmax = { 0, 16000 } }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_2, .config.minmax = { 0, 16000 } },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 16000 } }, { "gyro_notch2_cut", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 16000 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 200 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp, .config.minmax = { 0, 32000 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp, .config.minmax = { 0, 32000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki, .config.minmax = { 0, 32000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki, .config.minmax = { 0, 32000 } },
@ -645,8 +645,8 @@ static const clivalue_t valueTable[] = {
{ "deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->deadband, .config.minmax = { 0, 32 } }, { "deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->deadband, .config.minmax = { 0, 32 } },
{ "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->yaw_deadband, .config.minmax = { 0, 100 } }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->yaw_deadband, .config.minmax = { 0, 100 } },
{ "throttle_correction_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, { "throttle_correct_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } },
{ "throttle_correction_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, { "throttle_correct_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } },
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } },
@ -656,8 +656,8 @@ static const clivalue_t valueTable[] = {
#ifdef USE_SERVOS #ifdef USE_SERVOS
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} }, { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} },
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, { "servo_lowpass", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } },
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } },
#endif #endif
@ -673,7 +673,7 @@ static const clivalue_t valueTable[] = {
{ "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &rxConfig()->airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, &rxConfig()->airModeActivateThreshold, .config.minmax = {1000, 2000 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_off_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -705,21 +705,21 @@ static const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } },
#endif #endif
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } }, { "d_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } },
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } }, { "d_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } },
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, { "d_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } },
{ "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } },
{ "anti_gravity_rate_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
{ "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },
{ "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } },
{ "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } },
{ "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } },
@ -745,8 +745,8 @@ static const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "level_stick_sensitivity", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10, 200 } }, { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10, 200 } },
{ "level_angle_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10, 120 } }, { "level_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10, 120 } },
#ifdef BLACKBOX #ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } }, { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } },
@ -785,25 +785,27 @@ static const clivalue_t valueTable[] = {
{ "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } }, { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } },
{ "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } },
{ "osd_main_voltage_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_ontime_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_artificial_horizon", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_current_draw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POSCFG_MAX } },
{ "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POSCFG_MAX } },
#endif #endif
#ifdef USE_MAX7456 #ifdef USE_MAX7456
{ "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } }, { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } },
@ -829,7 +831,7 @@ static void cliPrint(const char *str)
bufWriterFlush(cliWriter); bufWriterFlush(cliWriter);
} }
#ifdef CLI_MINIMAL_VERBOSITY #ifdef MINIMAL_CLI
#define cliPrintHashLine(str) #define cliPrintHashLine(str)
#else #else
static void cliPrintHashLine(const char *str) static void cliPrintHashLine(const char *str)
@ -1211,7 +1213,7 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
} }
} }
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
static void cliRepeat(char ch, uint8_t len) static void cliRepeat(char ch, uint8_t len)
{ {
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
@ -1233,7 +1235,7 @@ static void cliShowParseError(void)
static void cliShowArgumentRangeError(char *name, int min, int max) static void cliShowArgumentRangeError(char *name, int min, int max)
{ {
cliPrintf("%s must be between %d and %d\r\n", name, min, max); cliPrintf("%s not between %d and %d\r\n", name, min, max);
} }
static const char *nextArg(const char *currentArg) static const char *nextArg(const char *currentArg)
@ -1279,7 +1281,7 @@ static void printRxFailsafe(uint8_t dumpMask, const rxConfig_t *rxConfig, const
for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) {
const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel];
const rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; const rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault;
bool equalsDefault = true; bool equalsDefault = false;
if (defaultRxConfig) { if (defaultRxConfig) {
channelFailsafeConfigurationDefault = &defaultRxConfig->failsafe_channel_configurations[channel]; channelFailsafeConfigurationDefault = &defaultRxConfig->failsafe_channel_configurations[channel];
equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode
@ -1402,7 +1404,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva
// print out aux channel settings // print out aux channel settings
for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = &modeActivationProfile->modeActivationConditions[i]; const modeActivationCondition_t *mac = &modeActivationProfile->modeActivationConditions[i];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultModeActivationProfile) { if (defaultModeActivationProfile) {
const modeActivationCondition_t *macDefault = &defaultModeActivationProfile->modeActivationConditions[i]; const modeActivationCondition_t *macDefault = &defaultModeActivationProfile->modeActivationConditions[i];
equalsDefault = mac->modeId == macDefault->modeId equalsDefault = mac->modeId == macDefault->modeId
@ -1474,7 +1476,7 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co
if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) {
continue; continue;
}; };
bool equalsDefault = true; bool equalsDefault = false;
if (serialConfigDefault) { if (serialConfigDefault) {
equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier
&& serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask
@ -1619,7 +1621,7 @@ static void cliSerialPassthrough(char *cmdline)
serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id); serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) { if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
if (!baud) { if (!baud) {
printf("Port %d is not open, you must specify baud\r\n", id); printf("Port %d is closed, must specify baud.\r\n", id);
return; return;
} }
if (!mode) if (!mode)
@ -1629,30 +1631,29 @@ static void cliSerialPassthrough(char *cmdline)
baud, mode, baud, mode,
SERIAL_NOT_INVERTED); SERIAL_NOT_INVERTED);
if (!passThroughPort) { if (!passThroughPort) {
printf("Port %d could not be opened\r\n", id); printf("Port %d could not be opened.\r\n", id);
return; return;
} }
printf("Port %d opened, baud=%d\r\n", id, baud); printf("Port %d opened, baud = %d.\r\n", id, baud);
} else { } else {
passThroughPort = passThroughPortUsage->serialPort; passThroughPort = passThroughPortUsage->serialPort;
// If the user supplied a mode, override the port's mode, otherwise // If the user supplied a mode, override the port's mode, otherwise
// leave the mode unchanged. serialPassthrough() handles one-way ports. // leave the mode unchanged. serialPassthrough() handles one-way ports.
printf("Port %d already open\r\n", id); printf("Port %d already open.\r\n", id);
if (mode && passThroughPort->mode != mode) { if (mode && passThroughPort->mode != mode) {
printf("Adjusting mode from configured value %d to %d\r\n", printf("Adjusting mode from %d to %d.\r\n",
passThroughPort->mode, mode); passThroughPort->mode, mode);
serialSetMode(passThroughPort, mode); serialSetMode(passThroughPort, mode);
} }
// If this port has a rx callback associated we need to remove it now. // If this port has a rx callback associated we need to remove it now.
// Otherwise no data will be pushed in the serial port buffer! // Otherwise no data will be pushed in the serial port buffer!
if (passThroughPort->rxCallback) { if (passThroughPort->rxCallback) {
printf("Removing rxCallback from port\r\n"); printf("Removing rxCallback\r\n");
passThroughPort->rxCallback = 0; passThroughPort->rxCallback = 0;
} }
} }
printf("Relaying data to device on port %d, Reset your board to exit " printf("Forwarding data to %d, power cycle to exit.\r\n", id);
"serial passthrough mode.\r\n", id);
serialPassthrough(cliPort, passThroughPort, NULL, NULL); serialPassthrough(cliPort, passThroughPort, NULL, NULL);
} }
@ -1664,7 +1665,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad
// print out adjustment ranges channel settings // print out adjustment ranges channel settings
for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
const adjustmentRange_t *ar = &adjustmentProfile->adjustmentRanges[i]; const adjustmentRange_t *ar = &adjustmentProfile->adjustmentRanges[i];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultAdjustmentProfile) { if (defaultAdjustmentProfile) {
const adjustmentRange_t *arDefault = &defaultAdjustmentProfile->adjustmentRanges[i]; const adjustmentRange_t *arDefault = &defaultAdjustmentProfile->adjustmentRanges[i];
equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex
@ -1770,7 +1771,7 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer
const float roll = customMotorMixer[i].roll; const float roll = customMotorMixer[i].roll;
const float pitch = customMotorMixer[i].pitch; const float pitch = customMotorMixer[i].pitch;
const float yaw = customMotorMixer[i].yaw; const float yaw = customMotorMixer[i].yaw;
bool equalsDefault = true; bool equalsDefault = false;
if (defaultCustomMotorMixer) { if (defaultCustomMotorMixer) {
const float thrDefault = defaultCustomMotorMixer[i].throttle; const float thrDefault = defaultCustomMotorMixer[i].throttle;
const float rollDefault = defaultCustomMotorMixer[i].roll; const float rollDefault = defaultCustomMotorMixer[i].roll;
@ -1868,7 +1869,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC
const char *format = "rxrange %u %u %u\r\n"; const char *format = "rxrange %u %u %u\r\n";
for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
const rxChannelRangeConfiguration_t *channelRangeConfiguration = &rxConfig->channelRanges[i]; const rxChannelRangeConfiguration_t *channelRangeConfiguration = &rxConfig->channelRanges[i];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultRxConfig) { if (defaultRxConfig) {
const rxChannelRangeConfiguration_t *channelRangeConfigurationDefault = &defaultRxConfig->channelRanges[i]; const rxChannelRangeConfiguration_t *channelRangeConfigurationDefault = &defaultRxConfig->channelRanges[i];
equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min
@ -1938,7 +1939,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC
for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t ledConfig = ledConfigs[i]; ledConfig_t ledConfig = ledConfigs[i];
generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer));
bool equalsDefault = true; bool equalsDefault = false;
if (defaultLedConfigs) { if (defaultLedConfigs) {
ledConfig_t ledConfigDefault = defaultLedConfigs[i]; ledConfig_t ledConfigDefault = defaultLedConfigs[i];
equalsDefault = ledConfig == ledConfigDefault; equalsDefault = ledConfig == ledConfigDefault;
@ -1975,7 +1976,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo
const char *format = "color %u %d,%u,%u\r\n"; const char *format = "color %u %d,%u,%u\r\n";
for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
const hsvColor_t *color = &colors[i]; const hsvColor_t *color = &colors[i];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultColors) { if (defaultColors) {
const hsvColor_t *colorDefault = &defaultColors[i]; const hsvColor_t *colorDefault = &defaultColors[i];
equalsDefault = color->h == colorDefault->h equalsDefault = color->h == colorDefault->h
@ -2014,7 +2015,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon
for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { for (uint32_t i = 0; i < LED_MODE_COUNT; i++) {
for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) {
int colorIndex = ledStripConfig->modeColors[i].color[j]; int colorIndex = ledStripConfig->modeColors[i].color[j];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultLedStripConfig) { if (defaultLedStripConfig) {
int colorIndexDefault = defaultLedStripConfig->modeColors[i].color[j]; int colorIndexDefault = defaultLedStripConfig->modeColors[i].color[j];
equalsDefault = colorIndex == colorIndexDefault; equalsDefault = colorIndex == colorIndexDefault;
@ -2026,7 +2027,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon
for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
const int colorIndex = ledStripConfig->specialColors.color[j]; const int colorIndex = ledStripConfig->specialColors.color[j];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultLedStripConfig) { if (defaultLedStripConfig) {
const int colorIndexDefault = defaultLedStripConfig->specialColors.color[j]; const int colorIndexDefault = defaultLedStripConfig->specialColors.color[j];
equalsDefault = colorIndex == colorIndexDefault; equalsDefault = colorIndex == colorIndexDefault;
@ -2036,7 +2037,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon
} }
const int ledStripAuxChannel = ledStripConfig->ledstrip_aux_channel; const int ledStripAuxChannel = ledStripConfig->ledstrip_aux_channel;
bool equalsDefault = true; bool equalsDefault = false;
if (defaultLedStripConfig) { if (defaultLedStripConfig) {
const int ledStripAuxChannelDefault = defaultLedStripConfig->ledstrip_aux_channel; const int ledStripAuxChannelDefault = defaultLedStripConfig->ledstrip_aux_channel;
equalsDefault = ledStripAuxChannel == ledStripAuxChannelDefault; equalsDefault = ledStripAuxChannel == ledStripAuxChannelDefault;
@ -2084,7 +2085,7 @@ static void printServo(uint8_t dumpMask, servoProfile_t *defaultServoProfile)
const char *format = "servo %u %d %d %d %d %d %d %d\r\n"; const char *format = "servo %u %d %d %d %d %d %d %d\r\n";
for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
const servoParam_t *servoConf = &servoProfile()->servoConf[i]; const servoParam_t *servoConf = &servoProfile()->servoConf[i];
bool equalsDefault = true; bool equalsDefault = false;
if (defaultServoProfile) { if (defaultServoProfile) {
const servoParam_t *servoConfDefault = &defaultServoProfile->servoConf[i]; const servoParam_t *servoConfDefault = &defaultServoProfile->servoConf[i];
equalsDefault = servoConf->min == servoConfDefault->min equalsDefault = servoConf->min == servoConfDefault->min
@ -2204,7 +2205,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig)
break; break;
} }
bool equalsDefault = true; bool equalsDefault = false;
if (defaultConfig) { if (defaultConfig) {
servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i];
equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel
@ -2463,7 +2464,7 @@ static void cliFlashErase(char *cmdline)
{ {
UNUSED(cmdline); UNUSED(cmdline);
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
uint32_t i; uint32_t i;
cliPrintf("Erasing, please wait ... \r\n"); cliPrintf("Erasing, please wait ... \r\n");
#else #else
@ -2474,7 +2475,7 @@ static void cliFlashErase(char *cmdline)
flashfsEraseCompletely(); flashfsEraseCompletely();
while (!flashfsIsReady()) { while (!flashfsIsReady()) {
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
cliPrintf("."); cliPrintf(".");
if (i++ > 120) { if (i++ > 120) {
i=0; i=0;
@ -2550,7 +2551,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig)
{ {
// print out vtx channel settings // print out vtx channel settings
const char *format = "vtx %u %u %u %u %u %u\r\n"; const char *format = "vtx %u %u %u %u %u %u\r\n";
bool equalsDefault = true; bool equalsDefault = false;
for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) {
const vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; const vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i];
if (defaultConfig) { if (defaultConfig) {
@ -2975,14 +2976,14 @@ static void cliEscPassthrough(char *cmdline)
index = atoi(pch); index = atoi(pch);
if(mode == 2 && index == 255) if(mode == 2 && index == 255)
{ {
printf("passthru on all pwm outputs enabled\r\n"); printf("passthrough on all outputs enabled\r\n");
} }
else{ else{
if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) {
printf("passthru at pwm output %d enabled\r\n", index); printf("passthrough on output %d enabled\r\n", index);
} }
else { else {
printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); printf("invalid output, range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT);
return; return;
} }
} }
@ -3075,7 +3076,7 @@ static void cliMotor(char *cmdline)
cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
} }
#if (FLASH_SIZE > 128) #ifndef MINIMAL_CLI
static void cliPlaySound(char *cmdline) static void cliPlaySound(char *cmdline)
{ {
int i; int i;
@ -3373,11 +3374,11 @@ static void cliTasks(char *cmdline)
int maxLoadSum = 0; int maxLoadSum = 0;
int averageLoadSum = 0; int averageLoadSum = 0;
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
if (masterConfig.task_statistics) { if (masterConfig.task_statistics) {
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
} else { } else {
cliPrintf("Task list rate/hz\r\n"); cliPrintf("Task list\r\n");
} }
#endif #endif
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) {
@ -3442,6 +3443,8 @@ static void cliVersion(char *cmdline)
#if defined(USE_RESOURCE_MGMT) #if defined(USE_RESOURCE_MGMT)
#define MAX_RESOURCE_INDEX(x) (x == 0 ? 1 : x)
typedef struct { typedef struct {
const uint8_t owner; const uint8_t owner;
ioTag_t *ptr; ioTag_t *ptr;
@ -3475,8 +3478,7 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig)
const char* owner; const char* owner;
owner = ownerNames[resourceTable[i].owner]; owner = ownerNames[resourceTable[i].owner];
if (resourceTable[i].maxIndex > 0) { for (int index = 0; index < MAX_RESOURCE_INDEX(resourceTable[i].maxIndex); index++) {
for (int index = 0; index < resourceTable[i].maxIndex; index++) {
ioTag_t ioTag = *(resourceTable[i].ptr + index); ioTag_t ioTag = *(resourceTable[i].ptr + index);
ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
@ -3496,58 +3498,36 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig)
cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
} }
} }
} else {
ioTag_t ioTag = *resourceTable[i].ptr;
ioTag_t ioTagDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
bool equalsDefault = ioTag == ioTagDefault;
const char *format = "resource %s %c%02d\r\n";
const char *formatUnassigned = "resource %s NONE\r\n";
if (!ioTagDefault) {
cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
} else {
cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault));
}
if (!ioTag) {
if (!(dumpMask & HIDE_UNUSED)) {
cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
}
} else {
cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
}
}
} }
} }
#ifndef CLI_MINIMAL_VERBOSITY static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag)
static bool resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag)
{ {
const char * format = "\r\n* %s * %c%d also used by %s"; const char * format = "\r\n* NOTE * %c%d moved from %s";
for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) {
for (int i = 0; i < (resourceTable[r].maxIndex == 0 ? 1 : resourceTable[r].maxIndex); i++) { for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) {
if (*(resourceTable[r].ptr + i) == tag) { if (*(resourceTable[r].ptr + i) == tag) {
bool error = false; bool cleared = false;
if (r == resourceIndex) { if (r == resourceIndex) {
if (i == index) { if (i == index) {
continue; continue;
} }
error = true; *(resourceTable[r].ptr + i) = IO_TAG_NONE;
cleared = true;
} }
cliPrintf(format, error ? "ERROR" : "WARNING", DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag), ownerNames[resourceTable[r].owner]); cliPrintf(format, DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag), ownerNames[resourceTable[r].owner]);
if (resourceTable[r].maxIndex > 0) { if (resourceTable[r].maxIndex > 0) {
cliPrintf(" %d", RESOURCE_INDEX(i)); cliPrintf(" %d", RESOURCE_INDEX(i));
} }
if (cleared) {
cliPrintf(". Cleared.");
}
cliPrint("\r\n"); cliPrint("\r\n");
if (error) {
return false;
} }
} }
} }
}
return true;
} }
#endif
static void cliResource(char *cmdline) static void cliResource(char *cmdline)
{ {
@ -3558,7 +3538,9 @@ static void cliResource(char *cmdline)
return; return;
} else if (strncasecmp(cmdline, "list", len) == 0) { } else if (strncasecmp(cmdline, "list", len) == 0) {
#ifndef CLI_MINIMAL_VERBOSITY #ifdef MINIMAL_CLI
cliPrintf("IO\r\n");
#else
cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n"); cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n");
cliRepeat('-', 20); cliRepeat('-', 20);
#endif #endif
@ -3566,15 +3548,18 @@ static void cliResource(char *cmdline)
const char* owner; const char* owner;
owner = ownerNames[ioRecs[i].owner]; owner = ownerNames[ioRecs[i].owner];
cliPrintf("%c%02d: %s ", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner);
if (ioRecs[i].index > 0) { if (ioRecs[i].index > 0) {
cliPrintf("%c%02d: %s %d\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, ioRecs[i].index); cliPrintf("%d", ioRecs[i].index);
} else {
cliPrintf("%c%02d: %s \r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner);
} }
cliPrintf("\r\n");
} }
cliPrintf("\r\n\r\nCurrently active DMA:\r\n"); cliPrintf("\r\n\r\n");
#ifndef CLI_MINIMAL_VERBOSITY #ifdef MINIMAL_CLI
cliPrintf("DMA:\r\n");
#else
cliPrintf("Currently active DMA:\r\n");
cliRepeat('-', 20); cliRepeat('-', 20);
#endif #endif
for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) {
@ -3590,7 +3575,7 @@ static void cliResource(char *cmdline)
} }
} }
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n");
#endif #endif
@ -3605,7 +3590,7 @@ static void cliResource(char *cmdline)
pch = strtok_r(cmdline, " ", &saveptr); pch = strtok_r(cmdline, " ", &saveptr);
for (resourceIndex = 0; ; resourceIndex++) { for (resourceIndex = 0; ; resourceIndex++) {
if (resourceIndex >= ARRAYLEN(resourceTable)) { if (resourceIndex >= ARRAYLEN(resourceTable)) {
cliPrint("Invalid resource\r\n"); cliPrint("Invalid\r\n");
return; return;
} }
@ -3614,25 +3599,30 @@ static void cliResource(char *cmdline)
} }
} }
if (resourceTable[resourceIndex].maxIndex > 0) {
pch = strtok_r(NULL, " ", &saveptr); pch = strtok_r(NULL, " ", &saveptr);
index = atoi(pch); index = atoi(pch);
if (index <= 0 || index > resourceTable[resourceIndex].maxIndex) { if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) {
cliShowArgumentRangeError("index", 1, resourceTable[resourceIndex].maxIndex); if (index <= 0 || index > MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)) {
cliShowArgumentRangeError("index", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex));
return; return;
} }
index -= 1; index -= 1;
}
pch = strtok_r(NULL, " ", &saveptr); pch = strtok_r(NULL, " ", &saveptr);
}
ioTag_t *tag = (ioTag_t*)(resourceTable[resourceIndex].ptr + index); ioTag_t *tag = (ioTag_t*)(resourceTable[resourceIndex].ptr + index);
uint8_t pin = 0; uint8_t pin = 0;
if (strlen(pch) > 0) { if (strlen(pch) > 0) {
if (strcasecmp(pch, "NONE") == 0) { if (strcasecmp(pch, "NONE") == 0) {
*tag = IO_TAG_NONE; *tag = IO_TAG_NONE;
cliPrintf("Resource is freed!\r\n"); #ifdef MINIMAL_CLI
cliPrintf("Freed\r\n");
#else
cliPrintf("Resource is freed\r\n");
#endif
return; return;
} else { } else {
uint8_t port = (*pch) - 'A'; uint8_t port = (*pch) - 'A';
@ -3646,17 +3636,15 @@ static void cliResource(char *cmdline)
if (pin < 16) { if (pin < 16) {
ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin)));
if (rec) { if (rec) {
#ifndef CLI_MINIMAL_VERBOSITY resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin));
if (!resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin))) { #ifdef MINIMAL_CLI
return; cliPrintf(" %c%02d set\r\n", port + 'A', pin);
}
cliPrintf("\r\nResource is set to %c%02d!\r\n", port + 'A', pin);
#else #else
cliPrintf("Set to %c%02d!", port + 'A', pin); cliPrintf("\r\nResource is set to %c%02d!\r\n", port + 'A', pin);
#endif #endif
*tag = DEFIO_TAG_MAKE(port, pin); *tag = DEFIO_TAG_MAKE(port, pin);
} else { } else {
cliPrintf("Resource is invalid!\r\n"); cliShowParseError();
} }
return; return;
} }
@ -3851,14 +3839,14 @@ static void cliDiff(char *cmdline)
typedef struct { typedef struct {
const char *name; const char *name;
#ifndef SKIP_CLI_COMMAND_HELP #ifndef MINIMAL_CLI
const char *description; const char *description;
const char *args; const char *args;
#endif #endif
void (*func)(char *cmdline); void (*func)(char *cmdline);
} clicmd_t; } clicmd_t;
#ifndef SKIP_CLI_COMMAND_HELP #ifndef MINIMAL_CLI
#define CLI_COMMAND_DEF(name, description, args, method) \ #define CLI_COMMAND_DEF(name, description, args, method) \
{ \ { \
name , \ name , \
@ -3926,7 +3914,7 @@ const clicmd_t cmdTable[] = {
#endif #endif
CLI_COMMAND_DEF("motor", "get/set motor", "<index> [<value>]", cliMotor), CLI_COMMAND_DEF("motor", "get/set motor", "<index> [<value>]", cliMotor),
CLI_COMMAND_DEF("name", "name of craft", NULL, cliName), CLI_COMMAND_DEF("name", "name of craft", NULL, cliName),
#if (FLASH_SIZE > 128) #ifndef MINIMAL_CLI
CLI_COMMAND_DEF("play_sound", NULL, "[<index>]", cliPlaySound), CLI_COMMAND_DEF("play_sound", NULL, "[<index>]", cliPlaySound),
#endif #endif
CLI_COMMAND_DEF("profile", "change profile", "[<index>]", cliProfile), CLI_COMMAND_DEF("profile", "change profile", "[<index>]", cliProfile),
@ -3969,7 +3957,7 @@ static void cliHelp(char *cmdline)
for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) {
cliPrint(cmdTable[i].name); cliPrint(cmdTable[i].name);
#ifndef SKIP_CLI_COMMAND_HELP #ifndef MINIMAL_CLI
if (cmdTable[i].description) { if (cmdTable[i].description) {
cliPrintf(" - %s", cmdTable[i].description); cliPrintf(" - %s", cmdTable[i].description);
} }
@ -4100,7 +4088,7 @@ void cliEnter(serialPort_t *serialPort)
schedulerSetCalulateTaskStatistics(masterConfig.task_statistics); schedulerSetCalulateTaskStatistics(masterConfig.task_statistics);
#ifndef CLI_MINIMAL_VERBOSITY #ifndef MINIMAL_CLI
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n"); cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
#else #else
cliPrint("\r\nCLI\r\n"); cliPrint("\r\nCLI\r\n");

View File

@ -171,8 +171,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->yaw_lpf_hz = 0; pidProfile->yaw_lpf_hz = 0;
pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->itermWindupPointPercent = 50;
pidProfile->yawItermIgnoreRate = 55;
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->dterm_notch_hz = 260; pidProfile->dterm_notch_hz = 260;
@ -186,7 +185,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yawRateAccelLimit = 10.0f; pidProfile->yawRateAccelLimit = 10.0f;
pidProfile->rateAccelLimit = 0.0f; pidProfile->rateAccelLimit = 0.0f;
pidProfile->itermThrottleThreshold = 300; pidProfile->itermThrottleThreshold = 300;
pidProfile->itermAcceleratorGain = 4.0f; pidProfile->itermAcceleratorGain = 3.0f;
pidProfile->itermAcceleratorRateLimit = 80;
} }
void resetProfile(profile_t *profile) void resetProfile(profile_t *profile)
@ -283,7 +283,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
#endif #endif
motorConfig->maxthrottle = 2000; motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000; motorConfig->mincommand = 1000;
motorConfig->digitalIdleOffsetPercent = 3.0f; motorConfig->digitalIdleOffsetPercent = 4.5f;
int motorIndex = 0; int motorIndex = 0;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
@ -1083,9 +1083,13 @@ void validateAndFixGyroConfig(void)
samplingTime = 0.00003125; samplingTime = 0.00003125;
// F1 and F3 can't handle high sample speed. // F1 and F3 can't handle high sample speed.
#if defined(STM32F1) #if defined(STM32F1)
gyroConfig()->gyro_sync_denom = constrain(gyroConfig()->gyro_sync_denom, 16, 16); gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3) #elif defined(STM32F3)
gyroConfig()->gyro_sync_denom = constrain(gyroConfig()->gyro_sync_denom, 4, 16); gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif #endif
} }

View File

@ -44,6 +44,7 @@
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/cli.h" #include "fc/cli.h"
#include "fc/fc_rc.h"
#include "msp/msp_serial.h" #include "msp/msp_serial.h"
@ -90,23 +91,8 @@ uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
static float throttlePIDAttenuation;
bool isRXDataNew; bool isRXDataNew;
static bool armingCalibrationWasInitialised; static bool armingCalibrationWasInitialised;
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
float getSetpointRate(int axis) {
return setpointRate[axis];
}
float getRcDeflection(int axis) {
return rcDeflection[axis];
}
float getRcDeflectionAbs(int axis) {
return rcDeflectionAbs[axis];
}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{ {
@ -129,226 +115,6 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
} }
#define SETPOINT_RATE_LIMIT 1998.0f
#define RC_RATE_INCREMENTAL 14.54f
void calculateSetpointRate(int axis, int16_t rc) {
float angleRate, rcRate, rcSuperfactor, rcCommandf;
uint8_t rcExpo;
if (axis != YAW) {
rcExpo = currentControlRateProfile->rcExpo8;
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
} else {
rcExpo = currentControlRateProfile->rcYawExpo8;
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
}
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcDeflection[axis] = rcCommandf;
rcDeflectionAbs[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof);
}
angleRate = 200.0f * rcRate * rcCommandf;
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate *= rcSuperfactor;
}
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
}
float roll = setpointRate[ROLL];
float yaw = setpointRate[YAW];
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
}
#define THROTTLE_BUFFER_MAX 20
#define THROTTLE_DELTA_MS 100
void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
static int index;
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
const int rxRefreshRateMs = rxRefreshRate / 1000;
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
if (index >= indexMax)
index = 0;
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain);
else
pidSetItermAccelerator(1.0f);
}
void processRcCommand(void)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
if (isRXDataNew) {
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
checkForThrottleErrorResetState(currentRxRefreshRate);
}
if (rxConfig()->rcInterpolation || flightModeFlags) {
// Set RC refresh rate for sampling and channels to filter
switch(rxConfig()->rcInterpolation) {
case(RC_SMOOTHING_AUTO):
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
break;
case(RC_SMOOTHING_MANUAL):
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
break;
case(RC_SMOOTHING_OFF):
case(RC_SMOOTHING_DEFAULT):
default:
rxRefreshRate = rxGetRefreshRate();
}
if (isRXDataNew) {
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (debugMode == DEBUG_RC_INTERPOLATION) {
for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
debug[3] = rxRefreshRate;
}
for (int channel=ROLL; channel < interpolationChannels; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=ROLL; channel < interpolationChannels; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation
readyToCalculateRate = true;
}
} else {
factor = 0;
}
} else {
factor = 0; // reset factor in case of level modes flip flopping
}
if (readyToCalculateRate || isRXDataNew) {
if (isRXDataNew)
readyToCalculateRateAxisCnt = FD_YAW;
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
calculateSetpointRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle();
isRXDataNew = false;
}
}
void updateRcCommands(void)
{
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
throttlePIDAttenuation = 1.0f;
} else {
if (rcData[THROTTLE] < 2000) {
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop = 100 - currentControlRateProfile->dynThrPID;
}
throttlePIDAttenuation = prop / 100.0f;
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp;
} else {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
}
if (rcData[axis] < rxConfig()->midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
int32_t tmp;
if (feature(FEATURE_3D)) {
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
} else {
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
}
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
void updateLEDs(void) void updateLEDs(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
@ -698,7 +464,7 @@ static void subTaskPidController(void)
uint32_t startTime; uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController() // PID - note this is function pointer set by setPIDController()
pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation); pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
} }
@ -741,8 +507,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
&& mixerConfig()->mixerMode != MIXER_FLYING_WING && mixerConfig()->mixerMode != MIXER_FLYING_WING
#endif #endif
) { ) {
rcCommand[YAW] = 0; resetYawAxis();
setpointRate[YAW] = 0;
} }
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {

View File

@ -21,6 +21,7 @@
extern int16_t magHold; extern int16_t magHold;
extern bool isRXDataNew; extern bool isRXDataNew;
extern int16_t headFreeModeHold;
union rollAndPitchTrims_u; union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
@ -34,6 +35,3 @@ void updateLEDs(void);
void updateRcCommands(void); void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs); void taskMainPidLoop(timeUs_t currentTimeUs);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);

View File

@ -1140,7 +1140,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, motorConfig()->useUnsyncedPwm); sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, motorConfig()->motorPwmRate);
sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100)); sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100));
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
//!!TODO gyro_isr_update to be added pending decision //!!TODO gyro_isr_update to be added pending decision
//sbufWriteU8(dst, gyroConfig()->gyro_isr_update); //sbufWriteU8(dst, gyroConfig()->gyro_isr_update);
@ -1160,8 +1160,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_PID_ADVANCED: case MSP_PID_ADVANCED:
sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate); sbufWriteU16(dst, 0);
sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate); sbufWriteU16(dst, 0);
sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit); sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit);
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation); sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation);
@ -1170,8 +1170,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved
sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit * 10); sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.rateAccelLimit * 10));
sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit * 10); sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.yawRateAccelLimit * 10));
sbufWriteU8(dst, currentProfile->pidProfile.levelAngleLimit); sbufWriteU8(dst, currentProfile->pidProfile.levelAngleLimit);
sbufWriteU8(dst, currentProfile->pidProfile.levelSensitivity); sbufWriteU8(dst, currentProfile->pidProfile.levelSensitivity);
break; break;
@ -1549,8 +1549,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_PID_ADVANCED: case MSP_SET_PID_ADVANCED:
currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src); sbufReadU16(src);
currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src); sbufReadU16(src);
currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src);
sbufReadU8(src); // reserved sbufReadU8(src); // reserved
currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src); currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src);

289
src/main/fc/fc_rc.c Executable file
View File

@ -0,0 +1,289 @@
/*
* 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"
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/filter.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_curves.h"
#include "fc/runtime_config.h"
#include "fc/fc_rc.h"
#include "fc/fc_core.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "flight/pid.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
float getSetpointRate(int axis) {
return setpointRate[axis];
}
float getRcDeflection(int axis) {
return rcDeflection[axis];
}
float getRcDeflectionAbs(int axis) {
return rcDeflectionAbs[axis];
}
float getThrottlePIDAttenuation(void) {
return throttlePIDAttenuation;
}
#define SETPOINT_RATE_LIMIT 1998.0f
#define RC_RATE_INCREMENTAL 14.54f
void calculateSetpointRate(int axis, int16_t rc) {
float angleRate, rcRate, rcSuperfactor, rcCommandf;
uint8_t rcExpo;
if (axis != YAW) {
rcExpo = currentControlRateProfile->rcExpo8;
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
} else {
rcExpo = currentControlRateProfile->rcYawExpo8;
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
}
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcDeflection[axis] = rcCommandf;
rcDeflectionAbs[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof);
}
angleRate = 200.0f * rcRate * rcCommandf;
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate *= rcSuperfactor;
}
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
}
float roll = setpointRate[ROLL];
float yaw = setpointRate[YAW];
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
}
#define THROTTLE_BUFFER_MAX 20
#define THROTTLE_DELTA_MS 100
void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
static int index;
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
const int rxRefreshRateMs = rxRefreshRate / 1000;
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
if (index >= indexMax)
index = 0;
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain);
else
pidSetItermAccelerator(1.0f);
}
void processRcCommand(void)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
if (isRXDataNew) {
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
checkForThrottleErrorResetState(currentRxRefreshRate);
}
if (rxConfig()->rcInterpolation || flightModeFlags) {
// Set RC refresh rate for sampling and channels to filter
switch(rxConfig()->rcInterpolation) {
case(RC_SMOOTHING_AUTO):
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
break;
case(RC_SMOOTHING_MANUAL):
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
break;
case(RC_SMOOTHING_OFF):
case(RC_SMOOTHING_DEFAULT):
default:
rxRefreshRate = rxGetRefreshRate();
}
if (isRXDataNew) {
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (debugMode == DEBUG_RC_INTERPOLATION) {
for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
debug[3] = rxRefreshRate;
}
for (int channel=ROLL; channel < interpolationChannels; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=ROLL; channel < interpolationChannels; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation
readyToCalculateRate = true;
}
} else {
factor = 0;
}
} else {
factor = 0; // reset factor in case of level modes flip flopping
}
if (readyToCalculateRate || isRXDataNew) {
if (isRXDataNew)
readyToCalculateRateAxisCnt = FD_YAW;
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
calculateSetpointRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle();
isRXDataNew = false;
}
}
void updateRcCommands(void)
{
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
throttlePIDAttenuation = 1.0f;
} else {
if (rcData[THROTTLE] < 2000) {
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop = 100 - currentControlRateProfile->dynThrPID;
}
throttlePIDAttenuation = prop / 100.0f;
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp;
} else {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
}
if (rcData[axis] < rxConfig()->midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
int32_t tmp;
if (feature(FEATURE_3D)) {
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
} else {
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
}
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
void resetYawAxis(void) {
rcCommand[YAW] = 0;
setpointRate[YAW] = 0;
}

31
src/main/fc/fc_rc.h Executable file
View File

@ -0,0 +1,31 @@
/*
* 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 <stdbool.h>
void calculateSetpointRate(int axis, int16_t rc);
void scaleRcCommandToFpvCamAngle(void);
void checkForThrottleErrorResetState(uint16_t rxRefreshRate);
void checkForThrottleErrorResetState(uint16_t rxRefreshRate);
void processRcCommand(void);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);
float getThrottlePIDAttenuation(void);
void updateRcCommands(void);
void resetYawAxis(void);

View File

@ -56,6 +56,7 @@
#define EXTERNAL_CONVERSION_MAX_VALUE 2000 #define EXTERNAL_CONVERSION_MAX_VALUE 2000
static uint8_t motorCount; static uint8_t motorCount;
static float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -245,6 +246,11 @@ uint8_t getMotorCount()
return motorCount; return motorCount;
} }
float getMotorMixRange()
{
return motorMixRange;
}
bool isMotorProtocolDshot(void) { bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT #ifdef USE_DSHOT
switch(motorConfig->motorPwmProtocol) { switch(motorConfig->motorPwmProtocol) {
@ -500,7 +506,7 @@ void mixTable(pidProfile_t *pidProfile)
} }
} }
const float motorMixRange = motorMixMax - motorMixMin; motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) { if (motorMixRange > 1.0f) {
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {

View File

@ -113,10 +113,12 @@ typedef struct airplaneConfig_s {
extern const mixer_t mixers[]; extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct motorConfig_s; struct motorConfig_s;
struct rxConfig_s; struct rxConfig_s;
uint8_t getMotorCount(); uint8_t getMotorCount();
float getMotorMixRange();
void mixerUseConfigs( void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,

View File

@ -29,11 +29,14 @@
#include "common/filter.h" #include "common/filter.h"
#include "fc/fc_core.h" #include "fc/fc_core.h"
#include "fc/fc_rc.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
@ -146,7 +149,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
} }
static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, maxVelocity[3], relaxFactor[3]; static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
void pidInitConfig(const pidProfile_t *pidProfile) { void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -154,13 +158,16 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
} }
levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL]; horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
} }
static float calcHorizonLevelStrength(void) { static float calcHorizonLevelStrength(void) {
@ -206,10 +213,14 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab) // Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor) void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
{ {
static float previousRateError[2]; static float previousRateError[2];
static float previousSetpoint[3]; const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
// Dynamic ki component to gradually scale back integration when above windup point
float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -238,30 +249,23 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
} }
// -----calculate I component // -----calculate I component
// Reduce strong Iterm accumulation during higher stick inputs
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f);
float ITerm = previousGyroIf[axis]; float ITerm = previousGyroIf[axis];
ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator; if (motorMixRange < 1.0f) {
// limit maximum integrator value to prevent WindUp // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
ITerm = constrainf(ITerm, -250.0f, 250.0f); // Iterm will only be accelerated below steady rate threshold
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
dynKi *= itermAccelerator;
float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
ITerm += ITermDelta;
previousGyroIf[axis] = ITerm; previousGyroIf[axis] = ITerm;
}
// -----calculate D component (Yaw D not yet supported) // -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0; float DTerm = 0.0;
if (axis != FD_YAW) { if (axis != FD_YAW) {
float dynC = c[axis]; float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) { if (pidProfile->setpointRelaxRatio < 100) {
const float rcDeflection = getRcDeflectionAbs(axis); dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f);
dynC = c[axis];
if (currentPidSetpoint > 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis])
dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
} else if (currentPidSetpoint < 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis])
dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
}
} }
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt) // Divide rate change by dT to get differential (ie dr/dt)
@ -276,7 +280,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
} }
previousSetpoint[axis] = currentPidSetpoint;
// -----calculate total PID output // -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm; axisPIDf[axis] = PTerm + ITerm + DTerm;

View File

@ -66,8 +66,7 @@ typedef struct pidProfile_s {
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit; uint16_t yaw_p_limit;
float pidSumLimit; float pidSumLimit;
uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t dterm_average_count; // Configurable delta count for dterm
@ -77,8 +76,9 @@ typedef struct pidProfile_s {
uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick
// Betaflight PID controller parameters // Betaflight PID controller parameters
uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint16_t itermAcceleratorRateLimit; // Setpointrate limit for iterm accelerator to operate within
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
@ -90,7 +90,7 @@ typedef struct pidConfig_s {
} pidConfig_t; } pidConfig_t;
union rollAndPitchTrims_u; union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor); void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
extern float axisPIDf[3]; extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

View File

@ -68,20 +68,28 @@
#define VIDEO_BUFFER_CHARS_PAL 480 #define VIDEO_BUFFER_CHARS_PAL 480
// Character coordinate and attributes // Character coordinate
#define OSD_POS(x,y) (x | (y << 5)) #define OSD_POS(x,y) (x | (y << 5))
#define OSD_X(x) (x & 0x001F) #define OSD_X(x) (x & 0x001F)
#define OSD_Y(x) ((x >> 5) & 0x001F) #define OSD_Y(x) ((x >> 5) & 0x001F)
// Blink control
bool blinkState = true;
static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
#define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
#define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
#define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
#define BLINK(item) (IS_BLINK(item) && blinkState)
// Things in both OSD and CMS // Things in both OSD and CMS
#define IS_HI(X) (rcData[X] > 1750) #define IS_HI(X) (rcData[X] > 1750)
#define IS_LO(X) (rcData[X] < 1250) #define IS_LO(X) (rcData[X] < 1250)
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
bool blinkState = true;
//extern uint8_t RSSI; // TODO: not used? //extern uint8_t RSSI; // TODO: not used?
static uint16_t flyTime = 0; static uint16_t flyTime = 0;
@ -139,7 +147,7 @@ static int32_t osdGetAltitude(int32_t alt)
static void osdDrawSingleElement(uint8_t item) static void osdDrawSingleElement(uint8_t item)
{ {
if (!VISIBLE(osdProfile()->item_pos[item]) || BLINK(osdProfile()->item_pos[item])) if (!VISIBLE(osdProfile()->item_pos[item]) || BLINK(item))
return; return;
uint8_t elemPosX = OSD_X(osdProfile()->item_pos[item]); uint8_t elemPosX = OSD_X(osdProfile()->item_pos[item]);
@ -366,6 +374,23 @@ static void osdDrawSingleElement(uint8_t item)
break; break;
} }
case OSD_PIDRATE_PROFILE:
{
uint8_t profileIndex = masterConfig.current_profile_index;
uint8_t rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile;
sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1);
break;
}
case OSD_MAIN_BATT_WARNING:
{
if (getVbat() > (batteryWarningVoltage - 1))
return;
sprintf(buff, "LOW VOLTAGE");
break;
}
default: default:
return; return;
} }
@ -377,6 +402,10 @@ void osdDrawElements(void)
{ {
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
/* Hide OSD when OSDSW mode is active */
if (IS_RC_MODE_ACTIVE(BOXOSD))
return;
#if 0 #if 0
if (currentElement) if (currentElement)
osdDrawElementPositioningHelp(); osdDrawElementPositioningHelp();
@ -409,6 +438,8 @@ void osdDrawElements(void)
osdDrawSingleElement(OSD_PITCH_PIDS); osdDrawSingleElement(OSD_PITCH_PIDS);
osdDrawSingleElement(OSD_YAW_PIDS); osdDrawSingleElement(OSD_YAW_PIDS);
osdDrawSingleElement(OSD_POWER); osdDrawSingleElement(OSD_POWER);
osdDrawSingleElement(OSD_PIDRATE_PROFILE);
osdDrawSingleElement(OSD_MAIN_BATT_WARNING);
#ifdef GPS #ifdef GPS
#ifdef CMS #ifdef CMS
@ -444,6 +475,8 @@ void osdResetConfig(osd_profile_t *osdProfile)
osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(2, 11); osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(2, 11);
osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(2, 12); osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(2, 12);
osdProfile->item_pos[OSD_POWER] = OSD_POS(15, 1); osdProfile->item_pos[OSD_POWER] = OSD_POS(15, 1);
osdProfile->item_pos[OSD_PIDRATE_PROFILE] = OSD_POS(2, 13);
osdProfile->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(8, 6);
osdProfile->rssi_alarm = 20; osdProfile->rssi_alarm = 20;
osdProfile->cap_alarm = 2200; osdProfile->cap_alarm = 2200;
@ -462,6 +495,8 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
armState = ARMING_FLAG(ARMED); armState = ARMING_FLAG(ARMED);
memset(blinkBits, 0, sizeof(blinkBits));
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
// display logo and help // display logo and help
@ -498,45 +533,48 @@ void osdUpdateAlarms(void)
statRssi = rssi * 100 / 1024; statRssi = rssi * 100 / 1024;
if (statRssi < pOsdProfile->rssi_alarm) if (statRssi < pOsdProfile->rssi_alarm)
pOsdProfile->item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; SET_BLINK(OSD_RSSI_VALUE);
else else
pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; CLR_BLINK(OSD_RSSI_VALUE);
if (getVbat() <= (batteryWarningVoltage - 1)) if (getVbat() <= (batteryWarningVoltage - 1)) {
pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
else SET_BLINK(OSD_MAIN_BATT_WARNING);
pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; } else {
CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
CLR_BLINK(OSD_MAIN_BATT_WARNING);
}
if (STATE(GPS_FIX) == 0) if (STATE(GPS_FIX) == 0)
pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; SET_BLINK(OSD_GPS_SATS);
else else
pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; CLR_BLINK(OSD_GPS_SATS);
if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED))
pOsdProfile->item_pos[OSD_FLYTIME] |= BLINK_FLAG; SET_BLINK(OSD_FLYTIME);
else else
pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; CLR_BLINK(OSD_FLYTIME);
if (mAhDrawn >= pOsdProfile->cap_alarm) if (mAhDrawn >= pOsdProfile->cap_alarm)
pOsdProfile->item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; SET_BLINK(OSD_MAH_DRAWN);
else else
pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; CLR_BLINK(OSD_MAH_DRAWN);
if (alt >= pOsdProfile->alt_alarm) if (alt >= pOsdProfile->alt_alarm)
pOsdProfile->item_pos[OSD_ALTITUDE] |= BLINK_FLAG; SET_BLINK(OSD_ALTITUDE);
else else
pOsdProfile->item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; CLR_BLINK(OSD_ALTITUDE);
} }
void osdResetAlarms(void) void osdResetAlarms(void)
{ {
osd_profile_t *pOsdProfile = &masterConfig.osdProfile; CLR_BLINK(OSD_RSSI_VALUE);
CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; CLR_BLINK(OSD_MAIN_BATT_WARNING);
pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; CLR_BLINK(OSD_GPS_SATS);
pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; CLR_BLINK(OSD_FLYTIME);
pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; CLR_BLINK(OSD_MAH_DRAWN);
pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; CLR_BLINK(OSD_ALTITUDE);
} }
static void osdResetStats(void) static void osdResetStats(void)

View File

@ -20,11 +20,9 @@
#include "common/time.h" #include "common/time.h"
#define VISIBLE_FLAG 0x0800 #define VISIBLE_FLAG 0x0800
#define BLINK_FLAG 0x0400
#define VISIBLE(x) (x & VISIBLE_FLAG) #define VISIBLE(x) (x & VISIBLE_FLAG)
#define BLINK(x) ((x & BLINK_FLAG) && blinkState)
#define BLINK_OFF(x) (x & ~BLINK_FLAG)
#define OSD_POS_MAX 0x3FF #define OSD_POS_MAX 0x3FF
#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values
typedef enum { typedef enum {
OSD_RSSI_VALUE, OSD_RSSI_VALUE,
@ -47,6 +45,8 @@ typedef enum {
OSD_PITCH_PIDS, OSD_PITCH_PIDS,
OSD_YAW_PIDS, OSD_YAW_PIDS,
OSD_POWER, OSD_POWER,
OSD_PIDRATE_PROFILE,
OSD_MAIN_BATT_WARNING,
OSD_ITEM_COUNT // MUST BE LAST OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e; } osd_items_e;

View File

View File

@ -25,6 +25,19 @@
#include "drivers/dma.h" #include "drivers/dma.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
#ifdef KISSCC
DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED),
DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED),
DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED),
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
DEF_TIM(TIM17,CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM16,CH1N,PA13, TIM_USE_PWM, TIMER_INPUT_ENABLED), // KISSCC new softserial?
#else
DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED),
@ -35,4 +48,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED), DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED), DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED),
DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED), DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED),
#endif
}; };

View File

@ -47,11 +47,17 @@
#define USE_ACC_MPU6050 #define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW180_DEG #define ACC_MPU6050_ALIGN CW180_DEG
#define USE_SOFTSERIAL
#define USE_VCP #define USE_VCP
#define USE_UART1 #define USE_UART1
#define USE_UART2 #define USE_UART2
#define USE_UART3 #define USE_UART3
#ifdef KISSCC
#define USE_SOFTSERIAL1
#define SERIAL_PORT_COUNT 5
#else
#define SERIAL_PORT_COUNT 4 #define SERIAL_PORT_COUNT 4
#endif
#define UART1_TX_PIN PA9 #define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10 #define UART1_RX_PIN PA10
@ -62,6 +68,12 @@
#define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_TX_PIN PB10 // PB10 (AF7)
#define UART3_RX_PIN PB11 // PB11 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7)
#ifdef KISSCC
#define SOFTSERIAL_1_TIMER TIM16
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 11
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 11
#endif
#define USE_I2C #define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
@ -72,7 +84,11 @@
//#define CURRENT_METER_ADC_PIN PA5 //#define CURRENT_METER_ADC_PIN PA5
//#define RSSI_ADC_PIN PB2 //#define RSSI_ADC_PIN PB2
#ifdef KISSCC
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY)
#else
#define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_FEATURES FEATURE_VBAT
#endif
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2 #define SERIALRX_UART SERIAL_PORT_USART2
@ -88,5 +104,9 @@
#define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTF (BIT(4)) #define TARGET_IO_PORTF (BIT(4))
#ifdef KISSCC
#define USABLE_TIMER_CHANNEL_COUNT 11
#else
#define USABLE_TIMER_CHANNEL_COUNT 10 #define USABLE_TIMER_CHANNEL_COUNT 10
#endif
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17))

View File

@ -32,33 +32,29 @@
#define INVERTER_PIN_USART2 PC15 //Sbus on USART 2 of nano. #define INVERTER_PIN_USART2 PC15 //Sbus on USART 2 of nano.
#define MPU9250_CS_PIN PB12 #define MPU6500_CS_PIN PB12
#define MPU9250_SPI_INSTANCE SPI2 #define MPU6500_SPI_INSTANCE SPI2
#define ACC #define ACC
#define USE_ACC_MPU9250 #define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU9250 #define USE_ACC_SPI_MPU6500
#define ACC_MPU9250_ALIGN CW270_DEG #define ACC_MPU6500_ALIGN CW270_DEG
#define GYRO #define GYRO
#define USE_GYRO_MPU9250 #define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU9250 #define USE_GYRO_SPI_MPU6500
#define GYRO_MPU9250_ALIGN CW270_DEG #define GYRO_MPU6500_ALIGN CW270_DEG
//#define MAG
//#define USE_MAG_HMC5883
#define BARO #define BARO
#define USE_BARO_MS5611 #define USE_BARO_MS5611
// MPU9250 interrupts // MPU6500 interrupts
#define USE_EXTI #define USE_EXTI
#define MPU_INT_EXTI PA15 #define MPU_INT_EXTI PA15
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
//#define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_VCP #define USE_VCP
#define VBUS_SENSING_PIN PA9 //#define VBUS_SENSING_PIN PA9
#define USE_UART1 #define USE_UART1
#define UART1_RX_PIN PB7 #define UART1_RX_PIN PB7
@ -68,15 +64,13 @@
#define UART2_RX_PIN PA3 #define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2 #define UART2_TX_PIN PA2
#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 #define SERIAL_PORT_COUNT 3
#define USE_ESCSERIAL #define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define ESCSERIAL_TIMER_TX_HARDWARE 0
#define USE_SPI #define USE_SPI
//#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2 #define USE_SPI_DEVICE_2
//#define USE_SPI_DEVICE_3
#define USE_I2C #define USE_I2C
#define I2C_DEVICE (I2CDEV_3) #define I2C_DEVICE (I2CDEV_3)

View File

@ -2,5 +2,6 @@ F411_TARGETS += $(TARGET)
FEATURES += VCP ONBOARDFLASH FEATURES += VCP ONBOARDFLASH
TARGET_SRC = \ TARGET_SRC = \
drivers/accgyro_spi_mpu9250.c \ drivers/accgyro_mpu6500.c \
drivers/accgyro_spi_mpu6500.c \
drivers/barometer_ms5611.c drivers/barometer_ms5611.c

View File

@ -30,6 +30,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH7 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH7
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH1 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH1
DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, TIMER_OUTPUT_ENABLED) //DMA1_CH2 - LED DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, TIMER_OUTPUT_ENABLED) //DMA1_CH2 - LED
}; };

View File

@ -49,6 +49,8 @@
#ifdef STM32F3 #ifdef STM32F3
#define USE_DSHOT #define USE_DSHOT
#undef GPS
#define MINIMAL_CLI
#endif #endif
#ifdef STM32F1 #ifdef STM32F1
@ -56,7 +58,7 @@
#define USE_UART1_RX_DMA #define USE_UART1_RX_DMA
#define USE_UART1_TX_DMA #define USE_UART1_TX_DMA
#define CLI_MINIMAL_VERBOSITY #define MINIMAL_CLI
#endif #endif
#define SERIAL_RX #define SERIAL_RX
@ -108,7 +110,5 @@
#define VTX_SMARTAUDIO #define VTX_SMARTAUDIO
#define VTX_TRAMP #define VTX_TRAMP
#define USE_SENSOR_NAMES #define USE_SENSOR_NAMES
#else
#define SKIP_CLI_COMMAND_HELP
#endif #endif

View File

@ -25,10 +25,9 @@ MEMORY
FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
} }
REGION_ALIAS("STACKRAM", CCM) REGION_ALIAS("STACKRAM", RAM)
INCLUDE "stm32_flash.ld" INCLUDE "stm32_flash.ld"