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

merge upstream into sirinfpv branch

This commit is contained in:
Evgeny Sychov 2016-06-10 19:42:54 -07:00
parent a38a2da74f ae2e483e0e
commit eb5963809d
77 changed files with 3017 additions and 946 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
Makefile
View File

@ -46,7 +46,7 @@ FORKNAME = betaflight
CC3D_TARGETS = CC3D CC3D_OPBL
VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE SIRINFPV
VALID_TARGETS = NAZE NAZE32PRO OLIMEXINO STM32F3DISCOVERY CHEBUZZF3 $(CC3D_TARGETS) CJMCU EUSTM32F103RC SPRACINGF3 PORT103R SPARKY ALIENFLIGHTF1 ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO IRCFUSIONF3 AFROMINI SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 SIRINFPV
# Valid targets for OP VCP support
VCP_VALID_TARGETS = $(CC3D_TARGETS)
@ -56,9 +56,9 @@ OPBL_VALID_TARGETS = CC3D_OPBL
64K_TARGETS = CJMCU
128K_TARGETS = ALIENFLIGHTF1 $(CC3D_TARGETS) NAZE OLIMEXINO RMDO AFROMINI
256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE SIRINFPV
256K_TARGETS = EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 SIRINFPV
F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE SIRINFPV
F3_TARGETS = STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 IRCFUSIONF3 SPARKY ALIENFLIGHTF3 COLIBRI_RACE LUX_RACE MOTOLAB RMDO SPRACINGF3MINI SPRACINGF3EVO DOGE SINGULARITY FURYF3 SIRINFPV
# note that there is no hardfault debugging startup file assembly handler for other platforms
ifeq ($(DEBUG_HARDFAULTS),F3)
@ -150,6 +150,13 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \
VPATH := $(VPATH):$(FATFS_DIR)
endif
ifeq ($(TARGET),FURY)
INCLUDE_DIRS := $(INCLUDE_DIRS) \
$(FATFS_DIR) \
VPATH := $(VPATH):$(FATFS_DIR)
endif
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld
ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion
@ -255,6 +262,9 @@ endif
ifeq ($(TARGET),$(filter $(TARGET), $(CC3D_TARGETS)))
TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D
ifeq ($(TARGET),CC3D_OPBL)
TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D_OPBL
endif
TARGET_DIR = $(ROOT)/src/main/target/CC3D
endif
@ -719,17 +729,8 @@ IRCFUSIONF3_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
drivers/barometer_bmp085.c \
drivers/barometer_bmp280.c \
drivers/compass_hmc5883l.c \
drivers/display_ug2864hsweg01.h \
drivers/flash_m25p16.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
drivers/serial_softserial.c \
drivers/sonar_hcsr04.c \
io/flashfs.c \
$(HIGHEND_SRC) \
$(COMMON_SRC)
@ -773,7 +774,7 @@ MOTOLAB_SRC = \
$(HIGHEND_SRC) \
$(COMMON_SRC) \
$(VCP_SRC)
SPRACINGF3MINI_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
@ -819,6 +820,46 @@ SIRINFPV_SRC = \
$(VCP_SRC)
# $(FATFS_SRC)
SINGULARITY_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/flash_m25p16.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
drivers/serial_softserial.c \
drivers/serial_usb_vcp.c \
drivers/vtx_rtc6705.c \
io/flashfs.c \
io/vtx.c \
$(HIGHEND_SRC) \
$(COMMON_SRC) \
$(VCP_SRC)
FURYF3_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/barometer_ms5611.c \
drivers/barometer_bmp280.c \
drivers/display_ug2864hsweg01.c \
drivers/accgyro_spi_mpu6000.c \
drivers/accgyro_mpu6500.c \
drivers/accgyro_spi_mpu6500.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
drivers/serial_usb_vcp.c \
drivers/sdcard.c \
drivers/sdcard_standard.c \
drivers/flash_m25p16.c \
drivers/sonar_hcsr04.c \
drivers/serial_softserial.c \
io/asyncfatfs/asyncfatfs.c \
io/asyncfatfs/fat_standard.c \
io/flashfs.c \
$(HIGHEND_SRC) \
$(COMMON_SRC) \
$(VCP_SRC)
# Search path and source files for the ST stdperiph library
VPATH := $(VPATH):$(STDPERIPH_DIR)/src

4
fake_travis_build.sh
View File

@ -16,7 +16,9 @@ targets=("PUBLISHMETA=True" \
"TARGET=IRCFUSIONF3" \
"TARGET=ALIENFLIGHTF1" \
"TARGET=ALIENFLIGHTF3" \
"TARGET=DOGE")
"TARGET=DOGE" \
"TARGET=SINGULARITY" \
"TARGET=FURYF3")
#fake a travis build environment
export TRAVIS_BUILD_NUMBER=$(date +%s)

59
q Normal file
View File

@ -0,0 +1,59 @@
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index fdde2cf..53464ef 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -67,6 +67,7 @@ static pidProfile_t *pidProfile;
static bool isUsingSticksToArm = true;

int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
+int16_t rcCommandSmooth[4]; // Smoothed RcCommand

uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e

diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index eaec277..dd8afaf 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -147,6 +147,7 @@ typedef struct controlRateConfig_s {
} controlRateConfig_t;

extern int16_t rcCommand[4];
+extern int16_t rcCommandSmooth[4]; // Smoothed RcCommand

typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
diff --git a/src/main/mw.c b/src/main/mw.c
index 125674c..5da79cf 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -181,7 +181,7 @@ void filterRc(void)

if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
- deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+ deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}

@@ -194,7 +194,7 @@ void filterRc(void)
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
- rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+ rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
@@ -649,8 +649,11 @@ void subTaskMainSubprocesses(void) {

const uint32_t startTime = micros();

+ filterRc();
+
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
- filterRc();
+ int axis;
+ for (axis = 0; axis <= 4; axis++) rcCommand[axis] = rcCommandSmooth[axis];
}

// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.

289
src/main/blackbox/blackbox.c
View File

@ -61,6 +61,7 @@
#include "io/serial_msp.h"
#include "io/statusindicator.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/msp.h"
@ -1118,6 +1119,15 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v
return xmitState.headerIndex < headerCount;
}
#ifndef BLACKBOX_PRINT_HEADER_LINE
#define BLACKBOX_PRINT_HEADER_LINE(x, ...) case __COUNTER__: \
blackboxPrintfHeaderLine(x, __VA_ARGS__); \
break;
#define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \
{__VA_ARGS__}; \
break;
#endif
/**
* Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
* true iff transmission is complete, otherwise call again later to continue transmission.
@ -1130,212 +1140,97 @@ static bool blackboxWriteSysinfo()
}
switch (xmitState.headerIndex) {
case 0:
blackboxPrintfHeaderLine("Firmware type:Cleanflight");
break;
case 1:
blackboxPrintfHeaderLine("Firmware revision:Betaflight %s (%s)", FC_VERSION_STRING, shortGitRevision);
break;
case 2:
blackboxPrintfHeaderLine("Firmware date:%s %s", buildDate, buildTime);
break;
case 3:
blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
break;
case 4:
blackboxPrintfHeaderLine("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
break;
case 5:
blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
break;
case 6:
blackboxPrintfHeaderLine("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle);
break;
case 7:
blackboxPrintfHeaderLine("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
break;
case 8:
blackboxPrintfHeaderLine("acc_1G:%u", acc_1G);
break;
case 9:
BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight");
BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale);
} else {
xmitState.headerIndex += 2; // Skip the next two vbat fields too
}
break;
case 10:
blackboxPrintfHeaderLine("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage,
masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage);
break;
case 11:
blackboxPrintfHeaderLine("vbatref:%u", vbatReference);
break;
case 12:
);
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage,
masterConfig.batteryConfig.vbatwarningcellvoltage,
masterConfig.batteryConfig.vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
//Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
if (feature(FEATURE_CURRENT_METER)) {
blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale);
}
break;
case 13:
blackboxPrintfHeaderLine("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
break;
case 14:
blackboxPrintfHeaderLine("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8);
break;
case 15:
blackboxPrintfHeaderLine("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8);
break;
case 16:
blackboxPrintfHeaderLine("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8);
break;
case 17:
blackboxPrintfHeaderLine("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID);
break;
case 18:
blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint);
break;
case 19:
blackboxPrintfHeaderLine("superExpoFactor:%d", masterConfig.rxConfig.superExpoFactor);
break;
case 20:
blackboxPrintfHeaderLine("rates:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]);
break;
case 21:
blackboxPrintfHeaderLine("looptime:%d", targetLooptime);
break;
case 22:
blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController);
break;
case 23:
blackboxPrintfHeaderLine("rollPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]);
break;
case 24:
blackboxPrintfHeaderLine("pitchPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]);
break;
case 25:
blackboxPrintfHeaderLine("yawPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]);
break;
case 26:
blackboxPrintfHeaderLine("altPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]);
break;
case 27:
blackboxPrintfHeaderLine("posPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]);
break;
case 28:
blackboxPrintfHeaderLine("posrPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]);
break;
case 29:
blackboxPrintfHeaderLine("navrPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]);
break;
case 30:
blackboxPrintfHeaderLine("levelPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]);
break;
case 31:
blackboxPrintfHeaderLine("magPID:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]);
break;
case 32:
blackboxPrintfHeaderLine("velPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]);
break;
case 33:
blackboxPrintfHeaderLine("yaw_p_limit:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit);
break;
case 34:
blackboxPrintfHeaderLine("yaw_lpf_hz:%d",
(int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f));
break;
case 35:
blackboxPrintfHeaderLine("dterm_average_count:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
break;
case 36:
blackboxPrintfHeaderLine("dterm_differentiator:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator);
break;
case 37:
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermResetRate);
break;
case 38:
blackboxPrintfHeaderLine("yawItermResetRate:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermResetRate);
break;
case 39:
blackboxPrintfHeaderLine("dterm_lpf_hz:%d",
(int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
break;
case 40:
blackboxPrintfHeaderLine("H_sensitivity:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity);
break;
case 41:
blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband);
break;
case 42:
blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
break;
case 43:
blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf);
break;
case 44:
blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
break;
case 45:
blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
break;
case 46:
blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware);
break;
case 47:
blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware);
break;
case 48:
blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware);
break;
case 49:
blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
break;
case 50:
blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
break;
case 51:
blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing);
break;
case 52:
blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures);
break;
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);
BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrMid8);
BLACKBOX_PRINT_HEADER_LINE("thrExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("dynThrPID:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].dynThrPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController);
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]);
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]);
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]);
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]);
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]);
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]);
BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]);
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]);
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]);
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]);
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid);
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
default:
return true;
}

1
src/main/blackbox/blackbox_io.c
View File

@ -37,6 +37,7 @@
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "io/gimbal.h"
#include "io/gps.h"

39
src/main/common/filter.c
View File

@ -28,6 +28,7 @@
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
@ -56,44 +57,36 @@ void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
sn = sinf(omega);
cs = cosf(omega);
alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
//this is wrong, should be hyperbolic sine
//alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
alpha = sn / (2 * BIQUAD_Q);
b0 = (1 - cs) /2;
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) /2;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
/* precompute the coefficients */
newState->b0 = b0 /a0;
newState->b1 = b1 /a0;
newState->b2 = b2 /a0;
newState->a1 = a1 /a0;
newState->a2 = a2 /a0;
newState->b0 = b0 / a0;
newState->b1 = b1 / a0;
newState->b2 = b2 / a0;
newState->a1 = a1 / a0;
newState->a2 = a2 / a0;
/* zero initial samples */
newState->x1 = newState->x2 = 0;
newState->y1 = newState->y2 = 0;
newState->d1 = newState->d2 = 1;
}
/* Computes a biquad_t filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state)
float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed
{
float result;
/* compute result */
result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
state->a1 * state->y1 - state->a2 * state->y2;
/* shift x1 to x2, sample to x1 */
state->x2 = state->x1;
state->x1 = sample;
/* shift y1 to y2, result to y1 */
state->y2 = state->y1;
state->y1 = result;
result = state->b0 * sample + state->d1;
state->d1 = state->b1 * sample - state->a1 * result + state->d2;
state->d2 = state->b2 * sample - state->a2 * result;
return result;
}

2
src/main/common/filter.h
View File

@ -26,7 +26,7 @@ typedef struct filterStatePt1_s {
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
float b0, b1, b2, a1, a2;
float x1, x2, y1, y2;
float d1, d2;
} biquad_t;
float applyBiQuadFilter(float sample, biquad_t *state);

20
src/main/common/maths.c
View File

@ -111,26 +111,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value;
}
inline int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
inline float constrainf(float amt, float low, float high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
void devClear(stdev_t *dev)
{
dev->m_n = 0;

23
src/main/common/maths.h
View File

@ -71,9 +71,6 @@ typedef union {
int32_t applyDeadband(int32_t value, int32_t deadband);
int constrain(int amt, int low, int high);
float constrainf(float amt, float low, float high);
void devClear(stdev_t *dev);
void devPush(stdev_t *dev, float x);
float devVariance(stdev_t *dev);
@ -116,3 +113,23 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
int16_t qPercent(fix12_t q);
int16_t qMultiply(fix12_t q, int16_t input);
fix12_t qConstruct(int16_t num, int16_t den);
static inline int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
static inline float constrainf(float amt, float low, float high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}

5
src/main/common/typeconversion.c
View File

@ -144,11 +144,6 @@ char *itoa(int i, char *a, int base)
#endif
/* Note: the floatString must be at least 13 bytes long to cover all cases.
* This includes up to 10 digits (32 bit ints can hold numbers up to 10
* digits long) + the decimal point + negative sign or space + null
* terminator.
*/
char *ftoa(float x, char *floatString)
{
int32_t value;

10
src/main/common/typeconversion.h
View File

@ -21,17 +21,7 @@ void li2a(long num, char *bf);
void ui2a(unsigned int num, unsigned int base, int uc, char *bf);
void i2a(int num, char *bf);
char a2i(char ch, const char **src, int base, int *nump);
/* Simple conversion of a float to a string. Will display completely
* inaccurate results for floats larger than about 2.15E6 (2^31 / 1000)
* (same thing for negative values < -2.15E6).
* Will always display 3 decimals, so anything smaller than 1E-3 will
* not be displayed.
* The floatString will be filled in with the result and will be
* returned. It must be at least 13 bytes in length to cover all cases!
*/
char *ftoa(float x, char *floatString);
float fastA2F(const char *p);
#ifndef HAVE_ITOA_FUNCTION

138
src/main/config/config.c
View File

@ -55,6 +55,7 @@
#include "io/ledstrip.h"
#include "io/gps.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"
@ -73,6 +74,10 @@
#include "config/config_profile.h"
#include "config/config_master.h"
#ifndef DEFAULT_RX_FEATURE
#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
#endif
#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 400
@ -125,8 +130,14 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
#endif
// use the last flash pages for storage
#ifdef CUSTOM_FLASH_MEMORY_ADDRESS
size_t custom_flash_memory_address = 0;
#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
#else
// use the last flash pages for storage
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
#endif
master_t masterConfig; // master config struct with data independent from profiles
profile_t *currentProfile;
@ -135,7 +146,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 133;
static const uint8_t EEPROM_CONF_VERSION = 140;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -149,14 +160,14 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidController = 1;
pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 18;
pidProfile->I8[ROLL] = 40;
pidProfile->D8[ROLL] = 15;
pidProfile->P8[PITCH] = 45;
pidProfile->I8[PITCH] = 30;
pidProfile->D8[PITCH] = 18;
pidProfile->I8[PITCH] = 40;
pidProfile->D8[PITCH] = 15;
pidProfile->P8[YAW] = 90;
pidProfile->I8[YAW] = 40;
pidProfile->D8[YAW] = 0;
pidProfile->I8[YAW] = 45;
pidProfile->D8[YAW] = 20;
pidProfile->P8[PIDALT] = 50;
pidProfile->I8[PIDALT] = 0;
pidProfile->D8[PIDALT] = 0;
@ -178,14 +189,11 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 75;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 70.0f;
pidProfile->dterm_differentiator = 1;
pidProfile->rollPitchItermResetRate = 200;
pidProfile->rollPitchItermResetAlways = 0;
pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes
pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 45;
pidProfile->dterm_lpf_hz = 110; // filtering ON by default
pidProfile->dynamic_pid = 1;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
@ -234,6 +242,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
escAndServoConfig->maxthrottle = 1850;
escAndServoConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500;
escAndServoConfig->escDesyncProtection = 10000;
}
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@ -306,15 +315,16 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 100;
controlRateConfig->rcExpo8 = 60;
controlRateConfig->rcYawRate8 = 100;
controlRateConfig->rcExpo8 = 10;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0;
controlRateConfig->rcYawExpo8 = 20;
controlRateConfig->tpa_breakpoint = 1500;
controlRateConfig->dynThrPID = 20;
controlRateConfig->rcYawExpo8 = 10;
controlRateConfig->tpa_breakpoint = 1650;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 50;
controlRateConfig->rates[axis] = 70;
}
}
@ -328,7 +338,6 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
void resetMixerConfig(mixerConfig_t *mixerConfig) {
mixerConfig->yaw_motor_direction = 1;
mixerConfig->yaw_jump_prevention_limit = 200;
#ifdef USE_SERVOS
mixerConfig->tri_unarmed_servo = 1;
mixerConfig->servo_lowpass_freq = 400;
@ -378,17 +387,12 @@ static void resetConf(void)
memset(&masterConfig, 0, sizeof(master_t));
setProfile(0);
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.mixerMode = MIXER_QUADX;
featureClearAll();
#if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) || defined(DOGE)
featureSet(FEATURE_RX_PPM);
featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES);
#ifdef DEFAULT_FEATURES
featureSet(DEFAULT_FEATURES);
#endif
//#if defined(SPRACINGF3MINI)
// featureSet(FEATURE_DISPLAY);
//#endif
#ifdef SIRINFPV
featureSet(FEATURE_OSD);
featureSet(FEATURE_RX_SERIAL);
@ -418,18 +422,22 @@ static void resetConf(void)
featureSet(FEATURE_VBAT);
#endif
featureSet(FEATURE_FAILSAFE);
featureSet(FEATURE_ONESHOT125);
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.mixerMode = MIXER_QUADX;
// global settings
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 0; // 256HZ default
#ifdef STM32F10X
masterConfig.gyro_sync_denom = 8;
masterConfig.gyro_soft_lpf_hz = 80.0f;
#else
masterConfig.gyro_sync_denom = 4;
#endif
masterConfig.gyro_soft_lpf_hz = 100;
masterConfig.pid_process_denom = 1;
masterConfig.pid_process_denom = 2;
masterConfig.debug_mode = 0;
@ -459,7 +467,7 @@ static void resetConf(void)
masterConfig.rxConfig.spektrum_sat_bind = 0;
masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1;
masterConfig.rxConfig.midrc = 1500;
masterConfig.rxConfig.mincheck = 1080;
masterConfig.rxConfig.mincheck = 1100;
masterConfig.rxConfig.maxcheck = 1900;
masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection
masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
@ -476,9 +484,7 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.superExpoFactor = 30;
masterConfig.rxConfig.superExpoFactorYaw = 30;
masterConfig.rxConfig.superExpoYawMode = 0;
masterConfig.rxConfig.airModeActivateThreshold = 1350;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
@ -503,7 +509,8 @@ static void resetConf(void)
masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE;
#endif
masterConfig.servo_pwm_rate = 50;
masterConfig.use_oneshot42 = 0;
masterConfig.fast_pwm_protocol = 1;
masterConfig.use_unsyncedPwm = 0;
#ifdef CC3D
masterConfig.use_buzzer_p6 = 0;
#endif
@ -518,11 +525,7 @@ static void resetConf(void)
resetSerialConfig(&masterConfig.serialConfig);
#if defined(STM32F10X) && !defined(CC3D)
masterConfig.emf_avoidance = 1;
#else
masterConfig.emf_avoidance = 0;
#endif
masterConfig.emf_avoidance = 0; // TODO - needs removal
resetPidProfile(&currentProfile->pidProfile);
@ -583,10 +586,17 @@ static void resetConf(void)
#ifdef LED_STRIP
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
applyDefaultLedStripConfig(masterConfig.ledConfigs);
masterConfig.ledstrip_visual_beeper = 0;
#endif
#ifdef VTX
masterConfig.vtx_band = 4; //Fatshark/Airwaves
masterConfig.vtx_channel = 1; //CH1
masterConfig.vtx_mode = 0; //CH+BAND mode
masterConfig.vtx_mhz = 5740; //F0
#endif
#ifdef SPRACINGF3
featureSet(FEATURE_BLACKBOX);
masterConfig.blackbox_device = 1;
#ifdef TRANSPONDER
static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware
@ -608,22 +618,18 @@ static void resetConf(void)
masterConfig.blackbox_rate_denom = 1;
#endif
#if defined(FURYF3)
masterConfig.blackbox_device = 2;
masterConfig.blackbox_rate_num = 1;
masterConfig.blackbox_rate_denom = 1;
#endif
// alternative defaults settings for COLIBRI RACE targets
#if defined(COLIBRI_RACE)
masterConfig.escAndServoConfig.minthrottle = 1025;
masterConfig.escAndServoConfig.maxthrottle = 1980;
masterConfig.batteryConfig.vbatmaxcellvoltage = 45;
masterConfig.batteryConfig.vbatmincellvoltage = 30;
featureSet(FEATURE_VBAT);
featureSet(FEATURE_FAILSAFE);
#endif
#ifdef SPRACINGF3EVO
featureSet(FEATURE_TRANSPONDER);
featureSet(FEATURE_RSSI_ADC);
featureSet(FEATURE_CURRENT_METER);
featureSet(FEATURE_TELEMETRY);
#endif
// alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets
@ -647,7 +653,6 @@ static void resetConf(void)
currentProfile->pidProfile.pidController = 2;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
masterConfig.mixerConfig.yaw_jump_prevention_limit = 500;
currentControlRateProfile->rcRate8 = 100;
currentControlRateProfile->rates[FD_PITCH] = 20;
currentControlRateProfile->rates[FD_ROLL] = 20;
@ -703,6 +708,17 @@ static void resetConf(void)
masterConfig.customMotorMixer[7].yaw = -1.0f;
#endif
// alternative defaults settings for SINGULARITY target
#if defined(SINGULARITY)
masterConfig.blackbox_device = 1;
masterConfig.blackbox_rate_num = 1;
masterConfig.blackbox_rate_denom = 1;
masterConfig.batteryConfig.vbatscale = 77;
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
#endif
// copy first profile into remaining profile
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
@ -744,8 +760,6 @@ static bool isEEPROMContentValid(void)
void activateControlRateConfig(void)
{
generatePitchRollCurve(currentControlRateProfile);
generateYawCurve(currentControlRateProfile);
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
}
@ -818,7 +832,7 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
featureSet(FEATURE_RX_PARALLEL_PWM);
}
if (featureConfigured(FEATURE_RX_PPM)) {
@ -1135,12 +1149,12 @@ void setBeeperOffMask(uint32_t mask)
masterConfig.beeper_off_flags = mask;
}
uint32_t getPreferedBeeperOffMask(void)
uint32_t getPreferredBeeperOffMask(void)
{
return masterConfig.prefered_beeper_off_flags;
return masterConfig.preferred_beeper_off_flags;
}
void setPreferedBeeperOffMask(uint32_t mask)
void setPreferredBeeperOffMask(uint32_t mask)
{
masterConfig.prefered_beeper_off_flags = mask;
masterConfig.preferred_beeper_off_flags = mask;
}

8
src/main/config/config.h
View File

@ -44,7 +44,9 @@ typedef enum {
FEATURE_BLACKBOX = 1 << 19,
FEATURE_CHANNEL_FORWARDING = 1 << 20,
FEATURE_TRANSPONDER = 1 << 21,
FEATURE_OSD = 1 << 22,
FEATURE_AIRMODE = 1 << 22,
FEATURE_SUPEREXPO_RATES = 1 << 23,
FEATURE_OSD = 1 << 24,
} features_e;
void handleOneshotFeatureChangeOnRestart(void);
@ -61,8 +63,8 @@ void beeperOffClear(uint32_t mask);
void beeperOffClearAll(void);
uint32_t getBeeperOffMask(void);
void setBeeperOffMask(uint32_t mask);
uint32_t getPreferedBeeperOffMask(void);
void setPreferedBeeperOffMask(uint32_t mask);
uint32_t getPreferredBeeperOffMask(void);
void setPreferredBeeperOffMask(uint32_t mask);
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex);

20
src/main/config/config_master.h
View File

@ -34,8 +34,8 @@ typedef struct master_t {
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
uint8_t use_oneshot42; // Oneshot42
uint8_t use_multiShot; // multishot
uint8_t fast_pwm_protocol; // Pwm Protocol
uint8_t use_unsyncedPwm; // unsync fast pwm protocol from PID loop
#ifdef USE_SERVOS
servoMixer_t customServoMixer[MAX_SERVO_RULES];
@ -56,9 +56,9 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
float gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
@ -119,6 +119,7 @@ typedef struct master_t {
#ifdef LED_STRIP
ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH];
hsvColor_t colors[CONFIGURABLE_COLOR_COUNT];
uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on
#endif
#ifdef TRANSPONDER
@ -136,6 +137,15 @@ typedef struct master_t {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
#ifdef VTX
uint8_t vtx_band; //1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Raceband
uint8_t vtx_channel; //1-8
uint8_t vtx_mode; //0=ch+band 1=mhz
uint16_t vtx_mhz; //5740
vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
#endif
#ifdef BLACKBOX
uint8_t blackbox_rate_num;
uint8_t blackbox_rate_denom;
@ -143,7 +153,7 @@ typedef struct master_t {
#endif
uint32_t beeper_off_flags;
uint32_t prefered_beeper_off_flags;
uint32_t preferred_beeper_off_flags;
uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum

7
src/main/debug.h
View File

@ -42,10 +42,13 @@ extern uint32_t sectionTimes[2][4];
#endif
typedef enum {
DEBUG_CYCLETIME = 1,
DEBUG_NONE,
DEBUG_CYCLETIME,
DEBUG_BATTERY,
DEBUG_GYRO,
DEBUG_ACCELEROMETER,
DEBUG_MIXER,
DEBUG_AIRMODE
DEBUG_AIRMODE,
DEBUG_PIDLOOP,
DEBUG_COUNT
} debugType_e;

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

@ -30,6 +30,7 @@
#ifndef SOFT_I2C
#if !defined(I2C1_SCL_GPIO)
#define I2C1_SCL_GPIO GPIOB
#define I2C1_SCL_GPIO_AF GPIO_AF_4
#define I2C1_SCL_PIN GPIO_Pin_6
@ -40,6 +41,7 @@
#define I2C1_SDA_PIN GPIO_Pin_7
#define I2C1_SDA_PIN_SOURCE GPIO_PinSource7
#define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB
#endif
#if !defined(I2C2_SCL_GPIO)
#define I2C2_SCL_GPIO GPIOF

125
src/main/drivers/pwm_mapping.c
View File

@ -31,8 +31,7 @@
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42);
void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex);
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse);
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
/*
@ -578,6 +577,62 @@ static const uint16_t airPWM[] = {
};
#endif
#if defined(SINGULARITY)
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8),
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t multiPWM[] = {
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t airPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8),
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_SERVO_OUTPUT << 8),
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
PWM7 | (MAP_TO_SERVO_OUTPUT << 8),
PWM8 | (MAP_TO_SERVO_OUTPUT << 8),
PWM9 | (MAP_TO_SERVO_OUTPUT << 8),
PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
0xFFFF
};
static const uint16_t airPWM[] = {
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_SERVO_OUTPUT << 8),
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
PWM7 | (MAP_TO_SERVO_OUTPUT << 8),
PWM8 | (MAP_TO_SERVO_OUTPUT << 8),
PWM9 | (MAP_TO_SERVO_OUTPUT << 8),
PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
0xFFFF
};
#endif
#ifdef SPRACINGF3MINI
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
@ -723,6 +778,54 @@ static const uint16_t airPWM[] = {
};
#endif
#ifdef FURYF3
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t multiPWM[] = {
PWM1 | (MAP_TO_PWM_INPUT << 8),
PWM2 | (MAP_TO_PWM_INPUT << 8),
PWM3 | (MAP_TO_PWM_INPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t airPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
PWM7 | (MAP_TO_SERVO_OUTPUT << 8),
PWM2 | (MAP_TO_SERVO_OUTPUT << 8),
PWM3 | (MAP_TO_SERVO_OUTPUT << 8),
0xFFFF
};
static const uint16_t airPWM[] = {
PWM1 | (MAP_TO_PWM_INPUT << 8),
PWM2 | (MAP_TO_PWM_INPUT << 8),
PWM3 | (MAP_TO_PWM_INPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
#endif
static const uint16_t * const hardwareMaps[] = {
multiPWM,
multiPPM,
@ -906,6 +1009,12 @@ if (init->useBuzzerP6) {
if (timerIndex == PWM7 || timerIndex == PWM8)
type = MAP_TO_SERVO_OUTPUT;
#endif
#if defined(SINGULARITY)
// remap PWM6+7 as servos
if (timerIndex == PWM6 || timerIndex == PWM7)
type = MAP_TO_SERVO_OUTPUT;
#endif
}
if (init->useChannelForwarding && !init->airplane) {
@ -942,7 +1051,7 @@ if (init->useBuzzerP6) {
if (type == MAP_TO_PPM_INPUT) {
#if defined(SPARKY) || defined(ALIENFLIGHTF3)
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
}
#endif
@ -953,18 +1062,14 @@ if (init->useBuzzerP6) {
} else if (type == MAP_TO_MOTOR_OUTPUT) {
#ifdef CC3D
if (init->useOneshot || isMotorBrushed(init->motorPwmRate)){
if (init->useFastPwm || isMotorBrushed(init->motorPwmRate)){
// Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed
if (timerHardwarePtr->tim == TIM2)
continue;
}
#endif
if (init->useOneshot) {
if (init->useMultiShot) {
pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
} else {
pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->useOneshot42);
}
if (init->useFastPwm) {
pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->useUnsyncedPwm, init->idlePulse);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ;
} else if (isMotorBrushed(init->motorPwmRate)) {
pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);

16
src/main/drivers/pwm_mapping.h
View File

@ -36,9 +36,11 @@
#define MAX_INPUTS 8
#define PWM_TIMER_MHZ 1
#define ONESHOT_TIMER_MHZ 24
#define PWM_BRUSHED_TIMER_MHZ 8
#define MULTISHOT_TIMER_MHZ 12
#define ONESHOT42_TIMER_MHZ 24
#define ONESHOT125_TIMER_MHZ 8
typedef struct sonarGPIOConfig_s {
GPIO_TypeDef *gpio;
@ -59,9 +61,8 @@ typedef struct drv_pwm_config_s {
bool useUART3;
#endif
bool useVbat;
bool useOneshot;
bool useOneshot42;
bool useMultiShot;
bool useFastPwm;
bool useUnsyncedPwm;
bool useSoftSerial;
bool useLEDStrip;
#ifdef SONAR
@ -70,11 +71,12 @@ typedef struct drv_pwm_config_s {
#ifdef USE_SERVOS
bool useServos;
bool useChannelForwarding; // configure additional channels as servos
#ifdef CC3D
bool useBuzzerP6;
#endif
uint8_t pwmProtocolType;
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
#endif
#ifdef CC3D
bool useBuzzerP6;
#endif
bool airplane; // fixed wing hardware config, lots of servos etc
uint16_t motorPwmRate;

55
src/main/drivers/pwm_output.c
View File

@ -135,27 +135,6 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value;
}
#if defined(STM32F10X) && !defined(CC3D)
static void pwmWriteOneshot125(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value * 21 / 6; // 24Mhz -> 8Mhz
}
static void pwmWriteOneshot42(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value * 7 / 6;
}
#else
static void pwmWriteOneshot125(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value * 3; // 24Mhz -> 8Mhz
}
static void pwmWriteOneshot42(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value;
}
#endif
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
{
@ -227,20 +206,30 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42)
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse)
{
motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT_TIMER_MHZ, 0xFFFF, 0);
if (useOneshot42) {
motors[motorIndex]->pwmWritePtr = pwmWriteOneshot42;
} else {
motors[motorIndex]->pwmWritePtr = pwmWriteOneshot125;
}
}
uint32_t timerMhzCounter;
void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex)
{
motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, 0xFFFF, 0);
motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot;
switch (fastPwmProtocolType) {
default:
case (PWM_TYPE_ONESHOT125):
timerMhzCounter = ONESHOT125_TIMER_MHZ;
break;
case (PWM_TYPE_ONESHOT42):
timerMhzCounter = ONESHOT42_TIMER_MHZ;
break;
case (PWM_TYPE_MULTISHOT):
timerMhzCounter = MULTISHOT_TIMER_MHZ;
}
if (useUnsyncedPwm) {
uint32_t hz = timerMhzCounter * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
} else {
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
}
motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : pwmWriteStandard;
}
#ifdef USE_SERVOS

7
src/main/drivers/pwm_output.h
View File

@ -17,6 +17,13 @@
#pragma once
typedef enum {
PWM_TYPE_CONVENTIONAL = 0,
PWM_TYPE_ONESHOT125,
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT
} FastPwmProtocolTypes_e;
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);

91
src/main/drivers/pwm_rx.c
View File

@ -20,8 +20,9 @@
#include <stdlib.h>
#include "platform.h"
#include <platform.h>
#include "build_config.h"
#include "debug.h"
#include "common/utils.h"
@ -35,16 +36,18 @@
#include "pwm_rx.h"
#define DEBUG_PPM_ISR
#define PPM_CAPTURE_COUNT 12
#define PWM_INPUT_PORT_COUNT 8
#if PPM_CAPTURE_COUNT > MAX_PWM_INPUT_PORTS
#if PPM_CAPTURE_COUNT > PWM_INPUT_PORT_COUNT
#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT
#else
#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
#endif
// TODO - change to timer cloks ticks
// TODO - change to timer clocks ticks
#define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
static inputFilteringMode_e inputFilteringMode;
@ -85,7 +88,8 @@ static uint8_t ppmCountShift = 0;
typedef struct ppmDevice_s {
uint8_t pulseIndex;
uint32_t previousTime;
//uint32_t previousTime;
uint32_t currentCapture;
uint32_t currentTime;
uint32_t deltaTime;
uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
@ -95,6 +99,7 @@ typedef struct ppmDevice_s {
uint8_t stableFramesSeenCount;
bool tracking;
bool overflowed;
} ppmDevice_t;
ppmDevice_t ppmDev;
@ -125,10 +130,35 @@ void pwmRxInit(inputFilteringMode_e initialInputFilteringMode)
inputFilteringMode = initialInputFilteringMode;
}
#ifdef DEBUG_PPM_ISR
typedef enum {
SOURCE_OVERFLOW = 0,
SOURCE_EDGE = 1
} eventSource_e;
typedef struct ppmISREvent_s {
eventSource_e source;
uint32_t capture;
} ppmISREvent_t;
static ppmISREvent_t ppmEvents[20];
static uint8_t ppmEventIndex = 0;
void ppmISREvent(eventSource_e source, uint32_t capture)
{
ppmEventIndex = (ppmEventIndex + 1) % (sizeof(ppmEvents) / sizeof(ppmEvents[0]));
ppmEvents[ppmEventIndex].source = source;
ppmEvents[ppmEventIndex].capture = capture;
}
#else
void ppmISREvent(eventSource_e source, uint32_t capture) {}
#endif
static void ppmInit(void)
{
ppmDev.pulseIndex = 0;
ppmDev.previousTime = 0;
ppmDev.currentCapture = 0;
ppmDev.currentTime = 0;
ppmDev.deltaTime = 0;
ppmDev.largeCounter = 0;
@ -136,42 +166,77 @@ static void ppmInit(void)
ppmDev.numChannelsPrevFrame = -1;
ppmDev.stableFramesSeenCount = 0;
ppmDev.tracking = false;
ppmDev.overflowed = false;
}
static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
{
UNUSED(cbRec);
ppmISREvent(SOURCE_OVERFLOW, capture);
ppmDev.largeCounter += capture + 1;
if (capture == PPM_TIMER_PERIOD - 1) {
ppmDev.overflowed = true;
}
}
static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
{
UNUSED(cbRec);
ppmISREvent(SOURCE_EDGE, capture);
int32_t i;
/* Shift the last measurement out */
ppmDev.previousTime = ppmDev.currentTime;
uint32_t previousTime = ppmDev.currentTime;
uint32_t previousCapture = ppmDev.currentCapture;
/* Grab the new count */
ppmDev.currentTime = capture;
uint32_t currentTime = capture;
/* Convert to 32-bit timer result */
ppmDev.currentTime += ppmDev.largeCounter;
currentTime += ppmDev.largeCounter;
if (capture < previousCapture) {
if (ppmDev.overflowed) {
currentTime += PPM_TIMER_PERIOD;
}
}
// Divide by 8 if Oneshot125 is active and this is a CC3D board
ppmDev.currentTime = ppmDev.currentTime >> ppmCountShift;
currentTime = currentTime >> ppmCountShift;
/* Capture computation */
ppmDev.deltaTime = ppmDev.currentTime - ppmDev.previousTime;
if (currentTime > previousTime) {
ppmDev.deltaTime = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD >> ppmCountShift) : 0));
} else {
ppmDev.deltaTime = (PPM_TIMER_PERIOD >> ppmCountShift) + currentTime - previousTime;
}
ppmDev.previousTime = ppmDev.currentTime;
ppmDev.overflowed = false;
#if 0
/* Store the current measurement */
ppmDev.currentTime = currentTime;
ppmDev.currentCapture = capture;
#if 1
static uint32_t deltaTimes[20];
static uint8_t deltaIndex = 0;
deltaIndex = (deltaIndex + 1) % 20;
deltaTimes[deltaIndex] = ppmDev.deltaTime;
UNUSED(deltaTimes);
#endif
#if 1
static uint32_t captureTimes[20];
static uint8_t captureIndex = 0;
captureIndex = (captureIndex + 1) % 20;
captureTimes[captureIndex] = capture;
UNUSED(captureTimes);
#endif
/* Sync pulse detection */

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

@ -83,7 +83,7 @@ void EXTI15_10_IRQHandler(void)
extiHandler(EXTI15_10_IRQn);
}
#if defined(CC3D)
#if defined(CC3D) || defined(FURYF3)
void EXTI3_IRQHandler(void)
{
extiHandler(EXTI3_IRQn);

48
src/main/drivers/timer.c
View File

@ -411,6 +411,54 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
#endif
#ifdef SINGULARITY
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM2, GPIOA, Pin_15, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource15, GPIO_AF_1}, // PPM/SERIAL RX
{ TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PWM1
{ TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // PWM2
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // PWM3
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // PWM4
{ TIM16, GPIOB, Pin_8, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_1}, // PWM5
{ TIM17, GPIOB, Pin_9, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_1}, // PWM6
{ TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // SOFTSERIAL1 RX (NC)
{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // SOFTSERIAL1 TX
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // LED_STRIP
};
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17))
#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3)
#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB)
#endif
#ifdef FURYF3
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM2, GPIOB, Pin_3, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_1}, // PPM IN
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // SS1 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // SS1 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
{ TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_2}, // PWM4 - S1
{ TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_2}, // PWM5 - S2
{ TIM17, GPIOB, Pin_5, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_10}, // PWM6 - S3
{ TIM16, GPIOB, Pin_4, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_1}, // PWM7 - S4
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO TIMER - LED_STRIP
};
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17))
#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB)
#endif
#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS)
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4

191
src/main/drivers/vtx_rtc6705.c Normal file
View File

@ -0,0 +1,191 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#ifdef VTX
#include "common/maths.h"
#include "drivers/vtx_rtc6705.h"
#include "drivers/bus_spi.h"
#include "drivers/system.h"
#define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400
#define RTC6705_SET_A1 0x8F3031 //5865
#define RTC6705_SET_A2 0x8EB1B1 //5845
#define RTC6705_SET_A3 0x8E3331 //5825
#define RTC6705_SET_A4 0x8DB4B1 //5805
#define RTC6705_SET_A5 0x8D3631 //5785
#define RTC6705_SET_A6 0x8CB7B1 //5765
#define RTC6705_SET_A7 0x8C4131 //5745
#define RTC6705_SET_A8 0x8BC2B1 //5725
#define RTC6705_SET_B1 0x8BF3B1 //5733
#define RTC6705_SET_B2 0x8C6711 //5752
#define RTC6705_SET_B3 0x8CE271 //5771
#define RTC6705_SET_B4 0x8D55D1 //5790
#define RTC6705_SET_B5 0x8DD131 //5809
#define RTC6705_SET_B6 0x8E4491 //5828
#define RTC6705_SET_B7 0x8EB7F1 //5847
#define RTC6705_SET_B8 0x8F3351 //5866
#define RTC6705_SET_E1 0x8B4431 //5705
#define RTC6705_SET_E2 0x8AC5B1 //5685
#define RTC6705_SET_E3 0x8A4731 //5665
#define RTC6705_SET_E4 0x89D0B1 //5645
#define RTC6705_SET_E5 0x8FA6B1 //5885
#define RTC6705_SET_E6 0x902531 //5905
#define RTC6705_SET_E7 0x90A3B1 //5925
#define RTC6705_SET_E8 0x912231 //5945
#define RTC6705_SET_F1 0x8C2191 //5740
#define RTC6705_SET_F2 0x8CA011 //5760
#define RTC6705_SET_F3 0x8D1691 //5780
#define RTC6705_SET_F4 0x8D9511 //5800
#define RTC6705_SET_F5 0x8E1391 //5820
#define RTC6705_SET_F6 0x8E9211 //5840
#define RTC6705_SET_F7 0x8F1091 //5860
#define RTC6705_SET_F8 0x8F8711 //5880
#define RTC6705_SET_R1 0x8A2151 //5658
#define RTC6705_SET_R2 0x8B04F1 //5695
#define RTC6705_SET_R3 0x8BF091 //5732
#define RTC6705_SET_R4 0x8CD431 //5769
#define RTC6705_SET_R5 0x8DB7D1 //5806
#define RTC6705_SET_R6 0x8EA371 //5843
#define RTC6705_SET_R7 0x8F8711 //5880
#define RTC6705_SET_R8 0x9072B1 //5917
#define RTC6705_SET_R 400 //Reference clock
#define RTC6705_SET_FDIV 1024 //128*(fosc/1000000)
#define RTC6705_SET_NDIV 16 //Remainder divider to get 'A' part of equation
#define RTC6705_SET_WRITE 0x11 //10001b to write to register
#define RTC6705_SET_DIVMULT 1000000 //Division value (to fit into a uint32_t) (Hz to MHz)
#define DISABLE_RTC6705 GPIO_SetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
#define ENABLE_RTC6705 GPIO_ResetBits(RTC6705_CS_GPIO, RTC6705_CS_PIN)
// Define variables
static const uint32_t channelArray[RTC6705_BAND_MAX][RTC6705_CHANNEL_MAX] = {
{ RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 },
{ RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 },
{ RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 },
{ RTC6705_SET_F1, RTC6705_SET_F2, RTC6705_SET_F3, RTC6705_SET_F4, RTC6705_SET_F5, RTC6705_SET_F6, RTC6705_SET_F7, RTC6705_SET_F8 },
{ RTC6705_SET_R1, RTC6705_SET_R2, RTC6705_SET_R3, RTC6705_SET_R4, RTC6705_SET_R5, RTC6705_SET_R6, RTC6705_SET_R7, RTC6705_SET_R8 },
};
/**
* Send a command and return if good
* TODO chip detect
*/
static bool rtc6705IsReady()
{
// Sleep a little bit to make sure it has booted
delay(50);
// TODO Do a read and get current config (note this would be reading on MOSI (data) line)
return true;
}
/**
* Reverse a uint32_t (LSB to MSB)
* This is easier for when generating the frequency to then
* reverse the bits afterwards
*/
static uint32_t reverse32(uint32_t in) {
uint32_t out = 0;
for (uint8_t i = 0 ; i < 32 ; i++)
{
out |= ((in>>i) & 1)<<(31-i);
}
return out;
}
/**
* Start chip if available
*/
bool rtc6705Init()
{
DISABLE_RTC6705;
spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
return rtc6705IsReady();
}
/**
* Transfer a 25bit packet to RTC6705
* This will just send it as a 32bit packet LSB meaning
* extra 0's get truncated on RTC6705 end
*/
static void rtc6705Transfer(uint32_t command)
{
command = reverse32(command);
ENABLE_RTC6705;
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 24) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 16) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 8) & 0xFF);
spiTransferByte(RTC6705_SPI_INSTANCE, (command >> 0) & 0xFF);
DISABLE_RTC6705;
}
/**
* Set a band and channel
*/
void rtc6705SetChannel(uint8_t band, uint8_t channel)
{
band = constrain(band, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
channel = constrain(channel, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
rtc6705Transfer(RTC6705_SET_HEAD);
rtc6705Transfer(channelArray[band-1][channel-1]);
}
/**
* Set a freq in mhz
* Formula derived from datasheet
*/
void rtc6705SetFreq(uint16_t freq)
{
freq = constrain(freq, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX);
uint32_t val_hex = 0;
uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
val_hex |= RTC6705_SET_WRITE;
val_hex |= (val_a << 5);
val_hex |= (val_n << 12);
rtc6705Transfer(RTC6705_SET_HEAD);
rtc6705Transfer(val_hex);
}
#endif

38
src/main/drivers/vtx_rtc6705.h Normal file
View File

@ -0,0 +1,38 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Giles Burgess (giles@multiflite.co.uk)
*
* This source code is provided as is and can be used/modified so long
* as this header is maintained with the file at all times.
*/
#pragma once
#include <stdint.h>
#define RTC6705_BAND_MIN 1
#define RTC6705_BAND_MAX 5
#define RTC6705_CHANNEL_MIN 1
#define RTC6705_CHANNEL_MAX 8
#define RTC6705_FREQ_MIN 5600
#define RTC6705_FREQ_MAX 5950
bool rtc6705Init();
void rtc6705SetChannel(uint8_t band, uint8_t channel);
void rtc6705SetFreq(uint16_t freq);

5
src/main/flight/imu.c
View File

@ -419,11 +419,6 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
}
}
void imuUpdateGyro(void)
{
gyroUpdate();
}
void imuUpdateAttitude(void)
{
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {

2
src/main/flight/imu.h
View File

@ -79,7 +79,6 @@ void imuConfigure(
float getCosTiltAngle(void);
void calculateEstimatedAltitude(uint32_t currentTime);
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
void imuUpdateGyro(void);
void imuUpdateAttitude(void);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
@ -88,5 +87,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
int16_t imuCalculateHeading(t_fp_vector *vec);
void imuResetAccelerationSum(void);
void imuUpdateGyro(void);
void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims);

37
src/main/flight/mixer.c
View File

@ -633,7 +633,7 @@ void writeServos(void)
}
#endif
void writeMotors(void)
void writeMotors(uint8_t fastPwmProtocol, uint8_t unsyncedPwm)
{
uint8_t i;
@ -641,7 +641,7 @@ void writeMotors(void)
pwmWriteMotor(i, motor[i]);
if (feature(FEATURE_ONESHOT125)) {
if (fastPwmProtocol && !unsyncedPwm) {
pwmCompleteOneshotMotorUpdate(motorCount);
}
}
@ -653,7 +653,7 @@ void writeAllMotors(int16_t mc)
// Sends commands to all motors
for (i = 0; i < motorCount; i++)
motor[i] = mc;
writeMotors();
writeMotors(1,1);
}
void stopMotors(void)
@ -751,22 +751,22 @@ STATIC_UNIT_TESTED void servoMixer(void)
void mixTable(void)
{
uint32_t i;
fix12_t vbatCompensationFactor;
fix12_t vbatCompensationFactor = 0;
static fix12_t mixReduction;
bool use_vbat_compensation = false;
if (batteryConfig && batteryConfig->vbatPidCompensation) {
use_vbat_compensation = true;
vbatCompensationFactor = calculateVbatPidCompensation();
}
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
@ -775,7 +775,7 @@ void mixTable(void)
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
@ -824,7 +824,7 @@ void mixTable(void)
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
} else {
@ -856,6 +856,19 @@ void mixTable(void)
motor[i] = escAndServoConfig->mincommand;
}
}
// Experimental Code. Anti Desync feature for ESC's
if (escAndServoConfig->escDesyncProtection) {
const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000);
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
}
}
}
// Disarmed mode

10
src/main/flight/mixer.h
View File

@ -19,9 +19,6 @@
#define MAX_SUPPORTED_MOTORS 12
#define MAX_SUPPORTED_SERVOS 8
#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode
@ -71,10 +68,9 @@ typedef struct mixer_s {
typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100)
#ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
int8_t servo_lowpass_enable; // enable/disable lowpass filter
#endif
} mixerConfig_t;
@ -195,7 +191,7 @@ bool motorLimitReached;
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
bool motorLimitReached;
struct escAndServoConfig_s;
struct rxConfig_s;
@ -219,6 +215,6 @@ int servoDirection(int servoIndex, int fromChannel);
#endif
void mixerResetDisarmedMotors(void);
void mixTable(void);
void writeMotors(void);
void writeMotors(uint8_t fastPwmProtocol, uint8_t unsyncedPwm);
void stopMotors(void);
void StopPwmAllMotors(void);

276
src/main/flight/pid.c
View File

@ -62,16 +62,12 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
uint8_t PIDweight[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3], errorGyroIfLimit[3];
static int32_t errorAngleI[2];
static float errorAngleIf[2];
static bool lowThrottlePidReduction;
#endif
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
@ -79,43 +75,52 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
float propFactor;
float superExpoFactor;
float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) {
float angleRate;
if (axis == YAW && !rxConfig->superExpoYawMode) {
propFactor = 1.0f;
if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcYawRate8 / 100.0f))) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f);
} else {
superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
propFactor = 1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f;
}
return propFactor;
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
}
void pidResetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
uint16_t dynamicKp;
errorAngleIf[ROLL] = 0.0f;
errorAngleIf[PITCH] = 0.0f;
uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);
dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;
return dynamicKp;
}
void pidResetErrorGyroState(uint8_t resetOption)
{
if (resetOption >= RESET_ITERM) {
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
uint16_t dynamicKi;
uint16_t resetRate;
if (resetOption == RESET_ITERM_AND_REDUCE_PID) {
lowThrottlePidReduction = true;
} else {
lowThrottlePidReduction = false;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint32_t dynamicFactor = ((resetRate << 16) / (resetRate + ABS(gyroADC[axis]))) >> 8;
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi;
}
void pidResetErrorGyroState(void)
{
int axis;
for (axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
#ifndef SKIP_PID_LUXFLOAT
errorGyroIf[axis] = 0.0f;
#endif
}
}
@ -131,12 +136,13 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
#ifndef SKIP_PID_LUXFLOAT
static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastRate[3][PID_LAST_RATE_COUNT];
static float lastRate[3];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -164,35 +170,30 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((rate + 47) * rcCommand[YAW]) / 32.0f;
} else {
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = calculateRate(axis, controlRateConfig);
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#else
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
gyroRate = gyroADC[axis] / 4.0f; // gyro output scaled to rewrite scale
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
@ -200,12 +201,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
}
PTerm = luxPTermScale * RateError * kP * tpaFactor;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -213,17 +212,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f);
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
}
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
if (axis == YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
}
if (antiWindupProtection || motorLimitReached) {
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
@ -236,22 +229,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->dterm_differentiator) {
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
axisPID[axis] = lrintf(PTerm + ITerm);
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
lastRate[axis][0] = gyroRate;
DTerm = 0.0f; // needed for blackbox
} else {
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
@ -260,13 +251,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
}
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -280,15 +269,14 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#endif
}
}
#endif
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis;
int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRate[3][PID_LAST_RATE_COUNT];
static int32_t lastRate[3];
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
@ -307,31 +295,26 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5;
} else {
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to max configured inclination
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig);
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to max configured inclination
#ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
} else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
}
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
} else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
}
}
@ -342,12 +325,10 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else {
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
}
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -359,21 +340,15 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis];
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
}
if (axis == YAW) {
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
}
if (antiWindupProtection || motorLimitReached) {
if (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
@ -384,22 +359,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->dterm_differentiator) {
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
lastRate[axis][0] = gyroRate;
DTerm = 0; // needed for blackbox
} else {
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
@ -408,12 +381,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -436,8 +408,10 @@ void pidSetController(pidControllerType_e type)
case PID_CONTROLLER_MWREWRITE:
pid_controller = pidMultiWiiRewrite;
break;
#ifndef SKIP_PID_LUXFLOAT
case PID_CONTROLLER_LUX_FLOAT:
pid_controller = pidLuxFloat;
#endif
}
}

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

@ -20,11 +20,11 @@
#define GYRO_I_MAX 256 // Gyro I limiter
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
#define YAW_JUMP_PREVENTION_LIMIT_HIGH 400
#define PID_LAST_RATE_COUNT 7
#define ITERM_RESET_THRESHOLD 20
#define ITERM_RESET_THRESHOLD_YAW 10
#define DYNAMIC_PTERM_STICK_THRESHOLD 400
typedef enum {
PIDROLL,
@ -51,12 +51,6 @@ typedef enum {
DELTA_FROM_MEASUREMENT
} pidDeltaType_e;
typedef enum {
RESET_DISABLE = 0,
RESET_ITERM,
RESET_ITERM_AND_REDUCE_PID
} pidErrorResetOption_e;
typedef enum {
SUPEREXPO_YAW_OFF = 0,
SUPEREXPO_YAW_ON,
@ -72,16 +66,13 @@ typedef struct pidProfile_s {
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
uint8_t H_sensitivity;
float dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t dterm_lpf_hz; // Delta Filter in hz
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t dterm_differentiator;
uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I)
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
@ -92,13 +83,18 @@ typedef struct pidProfile_s {
#endif
} pidProfile_t;
struct controlRateConfig_s;
union rollAndPitchTrims_u;
struct rxConfig_s;
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig,
uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool antiWindupProtection;
bool airmodeWasActivated;
extern uint32_t targetPidLooptime;
void pidSetController(pidControllerType_e type);
void pidResetErrorAngle(void);
void pidResetErrorGyroState(uint8_t resetOption);
void pidResetErrorGyroState(void);
void setTargetPidLooptime(uint8_t pidProcessDenom);

11
src/main/io/beeper.c
View File

@ -31,6 +31,7 @@
#include "sensors/sensors.h"
#include "io/statusindicator.h"
#include "io/vtx.h"
#ifdef GPS
#include "io/gps.h"
@ -172,7 +173,7 @@ typedef struct beeperTableEntry_s {
{ BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") },
{ BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERED") },
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERRED") },
};
static const beeperTableEntry_t *currentBeeperEntry = NULL;
@ -386,6 +387,13 @@ int beeperTableEntryCount(void)
return (int)BEEPER_TABLE_ENTRY_COUNT;
}
/*
* Returns true if the beeper is on, false otherwise
*/
bool isBeeperOn(void) {
return beeperIsOn;
}
#else
// Stub out beeper functions if #BEEPER not defined
@ -397,5 +405,6 @@ uint32_t getArmingBeepTimeMicros(void) {return 0;}
beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;}
const char *beeperNameForTableIndex(int idx) {UNUSED(idx); return NULL;}
int beeperTableEntryCount(void) {return 0;}
bool isBeeperOn(void) {return false;}
#endif

1
src/main/io/beeper.h
View File

@ -53,3 +53,4 @@ uint32_t getArmingBeepTimeMicros(void);
beeperMode_e beeperModeForTableIndex(int idx);
const char *beeperNameForTableIndex(int idx);
int beeperTableEntryCount(void);
bool isBeeperOn(void);

66
src/main/io/display.c
View File

@ -22,6 +22,7 @@
#include "platform.h"
#include "version.h"
#include "debug.h"
#include "build_config.h"
@ -58,12 +59,16 @@
#include "flight/navigation.h"
#endif
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/runtime_config.h"
#include "config/config_profile.h"
#include "display.h"
#include "scheduler.h"
extern profile_t *currentProfile;
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
#define MICROSECONDS_IN_A_SECOND (1000 * 1000)
@ -90,6 +95,9 @@ static const char* const pageTitles[] = {
"SENSORS",
"RX",
"PROFILE"
#ifndef SKIP_TASK_STATISTICS
,"TASKS"
#endif
#ifdef GPS
,"GPS"
#endif
@ -108,6 +116,9 @@ const pageId_e cyclePageIds[] = {
PAGE_RX,
PAGE_BATTERY,
PAGE_SENSORS
#ifndef SKIP_TASK_STATISTICS
,PAGE_TASKS
#endif
#ifdef ENABLE_DEBUG_OLED_PAGE
,PAGE_DEBUG,
#endif
@ -309,13 +320,26 @@ void showProfilePage(void)
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
const pidProfile_t *pidProfile = &currentProfile->pidProfile;
for (int axis = 0; axis < 3; ++axis) {
tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d",
axisTitles[axis],
pidProfile->P8[axis],
pidProfile->I8[axis],
pidProfile->D8[axis]
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
const uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
const controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
controlRateConfig->rcExpo8,
controlRateConfig->rcRate8
@ -514,6 +538,33 @@ void showSensorsPage(void)
}
#ifndef SKIP_TASK_STATISTICS
void showTasksPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%2d%6d%5d%4d%4d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string("Task max avg mx% av%");
cfTaskInfo_t taskInfo;
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &taskInfo);
if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo
const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 10000;
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
break;
}
}
}
}
#endif
#ifdef ENABLE_DEBUG_OLED_PAGE
void showDebugPage(void)
@ -605,6 +656,11 @@ void updateDisplay(void)
case PAGE_PROFILE:
showProfilePage();
break;
#ifndef SKIP_TASK_STATISTICS
case PAGE_TASKS:
showTasksPage();
break;
#endif
#ifdef GPS
case PAGE_GPS:
if (feature(FEATURE_GPS)) {

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

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
//#define ENABLE_DEBUG_OLED_PAGE
#define ENABLE_DEBUG_OLED_PAGE
typedef enum {
PAGE_WELCOME,
@ -24,6 +24,9 @@ typedef enum {
PAGE_SENSORS,
PAGE_RX,
PAGE_PROFILE,
#ifndef SKIP_TASK_STATISTICS
PAGE_TASKS,
#endif
#ifdef GPS
PAGE_GPS,
#endif

1
src/main/io/escservo.h
View File

@ -24,4 +24,5 @@ typedef struct escAndServoConfig_s {
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
uint16_t escDesyncProtection; // Value that a motor is allowed to increase or decrease in a period of 1ms
} escAndServoConfig_t;

27
src/main/io/ledstrip.c
View File

@ -34,19 +34,44 @@
#include "drivers/light_ws2811strip.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include <common/printf.h>
#include "common/axis.h"
#include "io/rc_controls.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "io/ledstrip.h"
#include "io/beeper.h"
#include "io/escservo.h"
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/gps.h"
#include "io/vtx.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
static bool ledStripInitialised = false;
static bool ledStripEnabled = true;
@ -916,7 +941,7 @@ void updateLedStrip(void)
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;

34
src/main/io/rc_controls.c
View File

@ -49,6 +49,7 @@
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "io/vtx.h"
#include "io/display.h"
@ -70,6 +71,13 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isSuperExpoActive(void) {
return (feature(FEATURE_SUPEREXPO_RATES));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
@ -124,15 +132,6 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
return THROTTLE_HIGH;
}
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
{
if (((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND)))
&& ((rcData[ROLL] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODEDEADBAND))))
return CENTERED;
return NOT_CENTERED;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@ -306,6 +305,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
#endif
#ifdef VTX
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
}
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
@ -489,13 +503,11 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcRate8 = newValue;
generatePitchRollCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue;
generatePitchRollCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:

6
src/main/io/rc_controls.h
View File

@ -49,7 +49,6 @@ typedef enum {
BOXBLACKBOX,
BOXFAILSAFE,
BOXAIRMODE,
BOXSUPEREXPO,
BOX3DDISABLESWITCH,
CHECKBOX_ITEM_COUNT
} boxId_e;
@ -137,6 +136,7 @@ typedef struct modeActivationCondition_s {
typedef struct controlRateConfig_s {
uint8_t rcRate8;
uint8_t rcYawRate8;
uint8_t rcExpo8;
uint8_t thrMid8;
uint8_t thrExpo8;
@ -161,6 +161,7 @@ bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
@ -243,6 +244,8 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15
bool isAirmodeActive(void);
bool isSuperExpoActive(void);
void resetAdjustmentStates(void);
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
@ -252,4 +255,3 @@ bool isUsingSticksForArming(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig);

36
src/main/io/rc_curves.c
View File

@ -26,26 +26,8 @@
#include "config/config.h"
int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;
for (i = 0; i < PITCH_LOOKUP_LENGTH; i++)
lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (i * i - 25)) * i * (int32_t) controlRateConfig->rcRate8 / 2500;
}
void generateYawCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;
for (i = 0; i < YAW_LOOKUP_LENGTH; i++)
lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (i * i - 25)) * i / 25;
}
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig)
{
@ -63,3 +45,17 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
}
}
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate)
{
float tmpf = tmp / 100.0f;
return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f );
}
int16_t rcLookupThrottle(int32_t tmp)
{
const int32_t tmp2 = tmp / 100;
// [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;
}

13
src/main/io/rc_curves.h
View File

@ -17,13 +17,8 @@
#pragma once
#define PITCH_LOOKUP_LENGTH 7
#define YAW_LOOKUP_LENGTH 7
#define THROTTLE_LOOKUP_LENGTH 12
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateYawCurve(controlRateConfig_t *controlRateConfig);
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate);
int16_t rcLookupThrottle(int32_t tmp);

160
src/main/io/serial_cli.c
View File

@ -26,6 +26,7 @@
#include "platform.h"
#include "scheduler.h"
#include "version.h"
#include "debug.h"
#include "build_config.h"
@ -60,6 +61,7 @@
#include "io/beeper.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
@ -164,6 +166,10 @@ static void cliFlashRead(char *cmdline);
#endif
#endif
#ifdef VTX
static void cliVtx(char *cmdline);
#endif
#ifdef USE_SDCARD
static void cliSdInfo(char *cmdline);
#endif
@ -194,7 +200,8 @@ static const char * const featureNames[] = {
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", NULL
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES",
NULL
};
// sync this with rxFailsafeChannelMode_e
@ -325,6 +332,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#ifdef VTX
CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
#endif
};
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@ -427,14 +437,15 @@ static const char * const lookupTableMagHardware[] = {
"AK8963"
};
static const char * const lookupTableDebug[] = {
static const char * const lookupTableDebug[DEBUG_COUNT] = {
"NONE",
"CYCLETIME",
"BATTERY",
"GYRO",
"ACCELEROMETER",
"MIXER",
"AIRMODE"
"AIRMODE",
"PIDLOOP",
};
#ifdef OSD
static const char * const lookupTableOsdType[] = {
@ -448,6 +459,10 @@ static const char * const lookupTableSuperExpoYaw[] = {
"OFF", "ON", "ALWAYS"
};
static const char * const lookupTableFastPwm[] = {
"OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT"
};
typedef struct lookupTableEntry_s {
const char * const *values;
const uint8_t valueCount;
@ -474,6 +489,7 @@ typedef enum {
TABLE_MAG_HARDWARE,
TABLE_DEBUG,
TABLE_SUPEREXPO_YAW,
TABLE_FAST_PWM,
#ifdef OSD
TABLE_OSD,
#endif
@ -500,6 +516,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
{ lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) },
#ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif
@ -571,6 +588,7 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
@ -578,12 +596,12 @@ const clivalue_t valueTable[] = {
{ "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "use_oneshot42", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_oneshot42, .config.lookup = { TABLE_OFF_ON } },
{ "use_multishot", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_multiShot, .config.lookup = { TABLE_OFF_ON } },
{ "unsynced_fast_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_unsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
{ "fast_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.fast_pwm_protocol, .config.lookup = { TABLE_FAST_PWM } },
#ifdef CC3D
{ "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 300, 32000 } },
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 200, 32000 } },
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
@ -667,7 +685,7 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
@ -683,15 +701,15 @@ const clivalue_t valueTable[] = {
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } },
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } },
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } },
{ "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } },
{ "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } },
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } },
{ "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
@ -701,9 +719,7 @@ const clivalue_t valueTable[] = {
{ "yaw_rate", 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_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
{ "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } },
{ "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } },
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -735,12 +751,11 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dynamic_pid", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
@ -773,9 +788,19 @@ const clivalue_t valueTable[] = {
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
#endif
#ifdef VTX
{ "vtx_band", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_band, .config.minmax = { 1, 5 } },
{ "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 1, 8 } },
{ "vtx_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_mode, .config.minmax = { 0, 2 } },
{ "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } },
#endif
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } },
#ifdef LED_STRIP
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
#endif
#ifdef USE_RTC6705
{ "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } },
#endif
@ -1790,6 +1815,67 @@ static void cliFlashRead(char *cmdline)
#endif
#endif
#ifdef VTX
static void cliVtx(char *cmdline)
{
int i, val = 0;
char *ptr;
if (isEmpty(cmdline)) {
// print out vtx channel settings
for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) {
vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i];
printf("vtx %u %u %u %u %u %u\r\n",
i,
cac->auxChannelIndex,
cac->band,
cac->channel,
MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep)
);
}
} else {
ptr = cmdline;
i = atoi(ptr++);
if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) {
vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i];
uint8_t validArgumentCount = 0;
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) {
cac->auxChannelIndex = val;
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) {
cac->band = val;
validArgumentCount++;
}
}
ptr = strchr(ptr, ' ');
if (ptr) {
val = atoi(++ptr);
if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) {
cac->channel = val;
validArgumentCount++;
}
}
ptr = processChannelRangeArgs(ptr, &cac->range, &validArgumentCount);
if (validArgumentCount != 5) {
memset(cac, 0, sizeof(vtxChannelActivationCondition_t));
}
} else {
cliShowArgumentRangeError("index", 0, MAX_CHANNEL_ACTIVATION_CONDITION_COUNT - 1);
}
}
}
#endif
static void dumpValues(uint16_t valueSection)
{
uint32_t i;
@ -1980,6 +2066,12 @@ static void cliDump(char *cmdline)
}
#endif
#ifdef VTX
cliPrint("\r\n# vtx\r\n");
cliVtx("");
#endif
printSectionBreak();
dumpValues(MASTER_VALUE);
@ -1988,17 +2080,28 @@ static void cliDump(char *cmdline)
if (dumpMask & DUMP_ALL) {
uint8_t activeProfile = masterConfig.current_profile_index;
uint8_t currentRateIndex = currentProfile->activeRateProfile;
uint8_t profileCount;
uint8_t rateCount;
for (profileCount=0; profileCount<MAX_PROFILE_COUNT;profileCount++) {
cliDumpProfile(profileCount);
uint8_t currentRateIndex = currentProfile->activeRateProfile;
uint8_t rateCount;
for (rateCount=0; rateCount<MAX_RATEPROFILES; rateCount++)
cliDumpRateProfile(rateCount);
cliPrint("\r\n# restore original rateprofile selection\r\n");
changeControlRateProfile(currentRateIndex);
cliRateProfile("");
}
cliPrint("\r\n# restore original profile selection\r\n");
changeProfile(activeProfile);
changeControlRateProfile(currentRateIndex);
cliProfile("");
printSectionBreak();
cliPrint("\r\n# save configuration\r\nsave\r\n");
} else {
cliDumpProfile(masterConfig.current_profile_index);
cliDumpRateProfile(currentProfile->activeRateProfile);
@ -2023,24 +2126,25 @@ void cliDumpProfile(uint8_t profileIndex) {
cliPrint("\r\n# profile\r\n");
cliProfile("");
cliPrintf("############################# PROFILE VALUES ####################################\r\n");
cliProfile("");
printSectionBreak();
dumpValues(PROFILE_VALUE);
cliRateProfile("");
}
void cliDumpRateProfile(uint8_t rateProfileIndex) {
if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values
return;
changeControlRateProfile(rateProfileIndex);
cliPrint("\r\n# rateprofile\r\n");
cliRateProfile("");
cliRateProfile("");
printSectionBreak();
dumpValues(PROFILE_RATE_VALUE);
}
void cliEnter(serialPort_t *serialPort)
{
cliMode = 1;
@ -2187,7 +2291,7 @@ static void cliBeeper(char *cmdline)
beeperOffSetAll(beeperCount-2);
else
if (i == BEEPER_PREFERENCE-1)
setBeeperOffMask(getPreferedBeeperOffMask());
setBeeperOffMask(getPreferredBeeperOffMask());
else {
mask = 1 << i;
beeperOffSet(mask);
@ -2199,7 +2303,7 @@ static void cliBeeper(char *cmdline)
beeperOffClearAll();
else
if (i == BEEPER_PREFERENCE-1)
setPreferedBeeperOffMask(getBeeperOffMask());
setPreferredBeeperOffMask(getBeeperOffMask());
else {
mask = 1 << i;
beeperOffClear(mask);
@ -2417,7 +2521,7 @@ static void cliRateProfile(char *cmdline) {
i = atoi(cmdline);
if (i >= 0 && i < MAX_RATEPROFILES) {
changeControlRateProfile(i);
cliRateProfile("");
cliRateProfile("");
}
}
}
@ -2477,7 +2581,7 @@ static void cliWrite(uint8_t ch)
static void cliPrintVar(const clivalue_t *var, uint32_t full)
{
int32_t value = 0;
char buf[13];
char buf[8];
void *ptr = var->ptr;
if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {

20
src/main/io/serial_msp.c
View File

@ -60,6 +60,7 @@
#include "io/transponder_ir.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "telemetry/telemetry.h"
@ -127,7 +128,7 @@ void setGyroSamplingSpeed(uint16_t looptime) {
masterConfig.mag_hardware = 1;
masterConfig.pid_process_denom = 2;
} else if (looptime < 375) {
#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) || defined(DOGE)
#if defined(LUX_RACE) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(ALIENFLIGHTF3) || defined(SPRACINGF3EVO) || defined(DOGE) || defined(FURYF3)
masterConfig.acc_hardware = 0;
#else
masterConfig.acc_hardware = 1;
@ -155,7 +156,7 @@ void setGyroSamplingSpeed(uint16_t looptime) {
masterConfig.pid_process_denom = 1;
if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2;
if (looptime < 250) {
masterConfig.pid_process_denom = 3;
masterConfig.pid_process_denom = 4;
} else if (looptime < 375) {
if (currentProfile->pidProfile.pidController == 2) {
masterConfig.pid_process_denom = 3;
@ -174,8 +175,6 @@ void setGyroSamplingSpeed(uint16_t looptime) {
masterConfig.pid_process_denom = 1;
}
#endif
if (!(masterConfig.use_multiShot || masterConfig.use_oneshot42) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125)) masterConfig.pid_process_denom = 3;
}
}
@ -220,8 +219,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOXSUPEREXPO, "SUPER EXPO;", 29 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30},
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -547,8 +545,7 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
if (sensors(SENSOR_BARO)) {
@ -654,8 +651,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));
@ -1892,6 +1888,10 @@ void mspProcess(void)
waitForSerialPortToFinishTransmitting(candidatePort->port);
stopMotors();
handleOneshotFeatureChangeOnRestart();
// On real flight controllers, systemReset() will do a soft reset of the device,
// reloading the program. But to support offline testing this flag needs to be
// cleared so that the software doesn't continuously attempt to reboot itself.
isRebootScheduled = false;
systemReset();
}
}

146
src/main/io/vtx.c Normal file
View File

@ -0,0 +1,146 @@
/*
* 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 <string.h>
#include "platform.h"
#ifdef VTX
#include "common/color.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/serial.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "io/ledstrip.h"
#include "io/gps.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/navigation.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/runtime_config.h"
static uint8_t locked = 0;
void vtxInit()
{
rtc6705Init();
if (masterConfig.vtx_mode == 0) {
rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel);
} else if (masterConfig.vtx_mode == 1) {
rtc6705SetFreq(masterConfig.vtx_mhz);
}
}
static void setChannelSaveAndNotify(uint8_t *bandOrChannel, uint8_t step, int32_t min, int32_t max)
{
if (ARMING_FLAG(ARMED)) {
locked = 1;
}
if (masterConfig.vtx_mode == 0 && !locked) {
uint8_t temp = (*bandOrChannel) + step;
temp = constrain(temp, min, max);
*bandOrChannel = temp;
rtc6705SetChannel(masterConfig.vtx_band, masterConfig.vtx_channel);
writeEEPROM();
readEEPROM();
beeperConfirmationBeeps(temp);
}
}
void vtxIncrementBand()
{
setChannelSaveAndNotify(&(masterConfig.vtx_band), 1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
}
void vtxDecrementBand()
{
setChannelSaveAndNotify(&(masterConfig.vtx_band), -1, RTC6705_BAND_MIN, RTC6705_BAND_MAX);
}
void vtxIncrementChannel()
{
setChannelSaveAndNotify(&(masterConfig.vtx_channel), 1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
}
void vtxDecrementChannel()
{
setChannelSaveAndNotify(&(masterConfig.vtx_channel), -1, RTC6705_CHANNEL_MIN, RTC6705_CHANNEL_MAX);
}
void vtxUpdateActivatedChannel()
{
if (ARMING_FLAG(ARMED)) {
locked = 1;
}
if (masterConfig.vtx_mode == 2 && !locked) {
static uint8_t lastIndex = -1;
uint8_t index;
for (index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) {
vtxChannelActivationCondition_t *vtxChannelActivationCondition = &masterConfig.vtxChannelActivationConditions[index];
if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range)
&& index != lastIndex) {
lastIndex = index;
rtc6705SetChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel);
break;
}
}
}
}
#endif

40
src/main/io/vtx.h Normal file
View File

@ -0,0 +1,40 @@
/*
* 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 "drivers/vtx_rtc6705.h"
#define VTX_BAND_MIN 1
#define VTX_BAND_MAX 5
#define VTX_CHANNEL_MIN 1
#define VTX_CHANNEL_MAX 8
#define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10
typedef struct vtxChannelActivationCondition_s {
uint8_t auxChannelIndex;
uint8_t band;
uint8_t channel;
channelRange_t range;
} vtxChannelActivationCondition_t;
void vtxInit();
void vtxIncrementBand();
void vtxDecrementBand();
void vtxIncrementChannel();
void vtxDecrementChannel();
void vtxUpdateActivatedChannel();

50
src/main/main.c
View File

@ -26,7 +26,6 @@
#include "common/axis.h"
#include "common/color.h"
#include "common/atomic.h"
#include "common/maths.h"
#include "drivers/nvic.h"
@ -45,6 +44,7 @@
#include "drivers/compass.h"
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_rx.h"
#include "drivers/pwm_output.h"
#include "drivers/adc.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_bst.h"
@ -71,6 +71,7 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/transponder_ir.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
@ -179,7 +180,7 @@ void init(void)
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
SetSysClock(0); // TODO - Remove from config in the future
#endif
//i2cSetOverclock(masterConfig.i2c_overclock);
@ -314,18 +315,20 @@ void init(void)
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
if (masterConfig.use_oneshot42) {
pwm_params.useOneshot42 = masterConfig.use_oneshot42 ? true : false;
masterConfig.use_multiShot = false;
if (masterConfig.fast_pwm_protocol == PWM_TYPE_ONESHOT125) {
featureSet(FEATURE_ONESHOT125);
} else {
pwm_params.useMultiShot = masterConfig.use_multiShot ? true : false;
featureClear(FEATURE_ONESHOT125);
}
pwm_params.useFastPwm = (masterConfig.fast_pwm_protocol != PWM_TYPE_CONVENTIONAL) ? true : false; // Configurator feature abused for enabling Fast PWM
pwm_params.pwmProtocolType = masterConfig.fast_pwm_protocol;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
pwm_params.useUnsyncedPwm = masterConfig.use_unsyncedPwm;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
if (pwm_params.motorPwmRate > 500 && !pwm_params.useFastPwm)
pwm_params.idlePulse = 0; // brushed motors
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
@ -397,6 +400,10 @@ void init(void)
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
@ -421,6 +428,11 @@ void init(void)
}
#endif
#if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
@ -486,7 +498,7 @@ void init(void)
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getPreferedBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
@ -666,7 +678,7 @@ void processLoopback(void) {
#define processLoopback()
#endif
int main(void) {
void main_init(void) {
init();
/* Setup scheduler */
@ -740,12 +752,24 @@ int main(void) {
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
}
while (1) {
scheduler();
processLoopback();
void main_step(void)
{
scheduler();
processLoopback();
}
#ifndef NOMAIN
int main(void)
{
main_init();
while(1) {
main_step();
}
}
#endif
#ifdef DEBUG_HARDFAULTS
//from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/

194
src/main/mw.c
View File

@ -67,6 +67,7 @@
#include "io/transponder_ir.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "rx/msp.h"
@ -102,6 +103,8 @@ enum {
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t magHold;
@ -114,15 +117,11 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
extern uint32_t currentTime;
extern uint8_t PIDweight[3];
extern bool antiWindupProtection;
uint16_t filteredCycleTime;
static bool isRXDataNew;
static bool armingCalibrationWasInitialised;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
extern pidControllerFuncPtr pid_controller;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
@ -170,7 +169,8 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void filterRc(void){
void filterRc(void)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
@ -221,12 +221,10 @@ void scaleRcCommandToFpvCamAngle(void) {
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
void annexCode(void)
static void updateRcCommands(void)
{
int32_t tmp, tmp2;
int32_t axis, prop;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
} else {
@ -237,38 +235,32 @@ void annexCode(void)
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (masterConfig.rcControlsConfig.deadband) {
if (tmp > masterConfig.rcControlsConfig.deadband) {
tmp -= masterConfig.rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
} else if (axis == YAW) {
if (masterConfig.rcControlsConfig.yaw_deadband) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
PIDweight[axis] = prop;
if (rcData[axis] < masterConfig.rxConfig.midrc)
int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > masterConfig.rcControlsConfig.deadband) {
tmp -= masterConfig.rcControlsConfig.deadband;
} else {
tmp = 0;
}
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8);
} else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;;
}
if (rcData[axis] < masterConfig.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);
@ -276,8 +268,7 @@ void annexCode(void)
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
}
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
@ -285,10 +276,10 @@ void annexCode(void)
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
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;
}
@ -297,8 +288,10 @@ void annexCode(void)
if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
}
static void updateLEDs(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
@ -323,10 +316,6 @@ void annexCode(void)
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void mwDisarm(void)
@ -467,6 +456,7 @@ void updateMagHold(void)
void processRx(void)
{
static bool armedBeeperOn = false;
static bool wasAirmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTime);
@ -488,28 +478,17 @@ void processRx(void)
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset
} else {
wasAirmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
if (rollPitchStatus == CENTERED) {
antiWindupProtection = true;
} else {
antiWindupProtection = false;
}
} else {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
pidResetErrorGyroState(RESET_ITERM);
} else {
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
}
pidResetErrorAngle();
}
} else {
pidResetErrorGyroState(RESET_DISABLE);
antiWindupProtection = false;
if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) {
pidResetErrorGyroState();
}
// When armed and motors aren't spinning, do beeps and then disarm
@ -652,15 +631,14 @@ void processRx(void)
}
#endif
#ifdef VTX
vtxUpdateActivatedChannel();
#endif
}
#if defined(BARO) || defined(SONAR)
static bool haveProcessedAnnexCodeOnce = false;
#endif
void taskMainPidLoop(void)
void subTaskPidController(void)
{
const uint32_t startTime = micros();
// PID - note this is function pointer set by setPIDController()
pid_controller(
&currentProfile->pidProfile,
@ -669,19 +647,21 @@ void taskMainPidLoop(void)
&masterConfig.accelerometerTrims,
&masterConfig.rxConfig
);
mixTable();
if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
}
void subTasksMainPidLoop(void) {
void subTaskMainSubprocesses(void) {
const uint32_t startTime = micros();
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
filterRc();
}
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) {
gyro.temperature(&telemTemperature1);
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
@ -742,25 +722,31 @@ void subTasksMainPidLoop(void) {
#ifdef TRANSPONDER
updateTransponder();
#endif
if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
}
void taskMotorUpdate(void) {
void subTaskMotorUpdate(void)
{
const uint32_t startTime = micros();
if (debugMode == DEBUG_CYCLETIME) {
static uint32_t previousMotorUpdateTime;
uint32_t currentDeltaTime = micros() - previousMotorUpdateTime;
const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
debug[2] = currentDeltaTime;
debug[3] = currentDeltaTime - targetPidLooptime;
previousMotorUpdateTime = micros();
previousMotorUpdateTime = startTime;
}
mixTable();
#ifdef USE_SERVOS
filterServos();
writeServos();
#endif
if (motorControlEnable) {
writeMotors();
writeMotors(masterConfig.fast_pwm_protocol, masterConfig.use_unsyncedPwm);
}
if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
}
uint8_t setPidUpdateCountDown(void) {
@ -771,23 +757,13 @@ uint8_t setPidUpdateCountDown(void) {
}
}
// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime
bool shouldUpdateMotorsAfterPIDLoop(void) {
if (targetPidLooptime > 375 ) {
return true;
} else if ((masterConfig.use_multiShot || masterConfig.use_oneshot42) && feature(FEATURE_ONESHOT125)) {
return true;
} else {
return false;
}
}
// Function for loop trigger
void taskMainPidLoopCheck(void) {
void taskMainPidLoopCheck(void)
{
static uint32_t previousTime;
static bool runTaskMainSubprocesses;
uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
cycleTime = micros() - previousTime;
previousTime = micros();
@ -797,24 +773,25 @@ void taskMainPidLoopCheck(void) {
debug[1] = averageSystemLoadPercent;
}
const uint32_t startTime = micros();
while (true) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
static uint8_t pidUpdateCountdown;
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
if (runTaskMainSubprocesses) {
if (!shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
subTasksMainPidLoop();
subTaskMainSubprocesses();
runTaskMainSubprocesses = false;
}
imuUpdateGyro();
gyroUpdate();
if (pidUpdateCountdown) {
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
taskMainPidLoop();
if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
subTaskPidController();
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}
@ -875,24 +852,21 @@ void taskUpdateRxMain(void)
processRx();
isRXDataNew = true;
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
updateLEDs();
#ifdef BARO
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
#endif
#ifdef SONAR
// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
if (haveProcessedAnnexCodeOnce) {
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
}
if (sensors(SENSOR_SONAR)) {
updateSonarAltHoldState();
}
#endif
annexCode();
}
#ifdef GPS

4
src/main/rx/rx.h
View File

@ -124,9 +124,7 @@ typedef struct rxConfig_s {
uint8_t rcSmoothing;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t superExpoFactor; // Super Expo Factor
uint8_t superExpoFactorYaw; // Super Expo Factor Yaw
uint8_t superExpoYawMode; // Seperate Super expo for yaw
uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated
uint16_t rx_min_usec;
uint16_t rx_max_usec;

6
src/main/scheduler.h
View File

@ -44,14 +44,14 @@ typedef enum {
/* Actual tasks */
TASK_SYSTEM = 0,
TASK_GYROPID,
TASK_ATTITUDE,
TASK_ACCEL,
TASK_ATTITUDE,
TASK_RX,
TASK_SERIAL,
TASK_BATTERY,
#ifdef BEEPER
TASK_BEEPER,
#endif
TASK_BATTERY,
TASK_RX,
#ifdef GPS
TASK_GPS,
#endif

52
src/main/scheduler_tasks.c
View File

@ -22,14 +22,15 @@
#include <platform.h>
#include "scheduler.h"
void taskSystem(void);
void taskMainPidLoopCheck(void);
void taskUpdateAccelerometer(void);
void taskHandleSerial(void);
void taskUpdateAttitude(void);
void taskUpdateBeeper(void);
void taskUpdateBattery(void);
bool taskUpdateRxCheck(uint32_t currentDeltaTime);
void taskUpdateRxMain(void);
void taskHandleSerial(void);
void taskUpdateBattery(void);
void taskUpdateBeeper(void);
void taskProcessGPS(void);
void taskUpdateCompass(void);
void taskUpdateBaro(void);
@ -39,7 +40,6 @@ void taskUpdateDisplay(void);
void taskTelemetry(void);
void taskLedStrip(void);
void taskTransponder(void);
void taskSystem(void);
#ifdef OSD
void taskUpdateOsd(void);
#endif
@ -64,13 +64,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_REALTIME,
},
[TASK_ATTITUDE] = {
.taskName = "ATTITUDE",
.taskFunc = taskUpdateAttitude,
.desiredPeriod = 1000000 / 100,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_ACCEL] = {
.taskName = "ACCEL",
.taskFunc = taskUpdateAccelerometer,
@ -78,6 +71,21 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_ATTITUDE] = {
.taskName = "ATTITUDE",
.taskFunc = taskUpdateAttitude,
.desiredPeriod = 1000000 / 100,
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_RX] = {
.taskName = "RX",
.checkFunc = taskUpdateRxCheck,
.taskFunc = taskUpdateRxMain,
.desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
.staticPriority = TASK_PRIORITY_HIGH,
},
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
@ -85,6 +93,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_LOW,
},
[TASK_BATTERY] = {
.taskName = "BATTERY",
.taskFunc = taskUpdateBattery,
.desiredPeriod = 1000000 / 50, // 50 Hz
.staticPriority = TASK_PRIORITY_LOW,
},
#ifdef BEEPER
[TASK_BEEPER] = {
.taskName = "BEEPER",
@ -94,21 +109,6 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
[TASK_BATTERY] = {
.taskName = "BATTERY",
.taskFunc = taskUpdateBattery,
.desiredPeriod = 1000000 / 50, // 50 Hz
.staticPriority = TASK_PRIORITY_LOW,
},
[TASK_RX] = {
.taskName = "RX",
.checkFunc = taskUpdateRxCheck,
.taskFunc = taskUpdateRxMain,
.desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
.staticPriority = TASK_PRIORITY_HIGH,
},
#ifdef GPS
[TASK_GPS] = {
.taskName = "GPS",

2
src/main/sensors/acceleration.h
View File

@ -45,7 +45,7 @@ typedef struct rollAndPitchTrims_s {
int16_t pitch;
} rollAndPitchTrims_t_def;
typedef union {
typedef union rollAndPitchTrims_u {
int16_t raw[2];
rollAndPitchTrims_t_def values;
} rollAndPitchTrims_t;

20
src/main/sensors/gyro.c
View File

@ -38,6 +38,7 @@
uint16_t calibratingG = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
@ -151,17 +152,20 @@ void gyroUpdate(void)
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++){
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
}
}
}

1
src/main/sensors/gyro.h
View File

@ -33,6 +33,7 @@ extern gyro_t gyro;
extern sensor_align_e gyroAlign;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
extern float gyroADCf[XYZ_AXIS_COUNT];
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct gyroConfig_s {

39
src/main/sensors/initialisation.c
View File

@ -195,6 +195,19 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
return &MotolabF3MPU6050Config;
#endif
#ifdef SINGULARITY
static const extiConfig_t singularityMPU6050Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
.exti_port_source = EXTI_PortSourceGPIOC,
.exti_pin_source = EXTI_PinSource13,
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &singularityMPU6050Config;
#endif
#ifdef ALIENFLIGHTF3
// MPU_INT output on V1 PA15
static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = {
@ -223,10 +236,24 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
}
#endif
#if defined(FURYF3)
static const extiConfig_t FURYF3MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_3,
.exti_port_source = EXTI_PortSourceGPIOA,
.exti_pin_source = EXTI_PinSource3,
.exti_line = EXTI_Line3,
.exti_irqn = EXTI3_IRQn
};
return &FURYF3MPUIntExtiConfig;
#endif
return NULL;
}
#ifdef USE_FAKE_GYRO
int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 };
static void fakeGyroInit(uint16_t lpf)
{
UNUSED(lpf);
@ -234,7 +261,10 @@ static void fakeGyroInit(uint16_t lpf)
static bool fakeGyroRead(int16_t *gyroADC)
{
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
for (int i = 0; i < XYZ_AXIS_COUNT; ++i) {
gyroADC[i] = fake_gyro_values[i];
}
return true;
}
@ -249,14 +279,19 @@ bool fakeGyroDetect(gyro_t *gyro)
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
gyro->scale = 1.0f / 16.4f;
return true;
}
#endif
#ifdef USE_FAKE_ACC
int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0};
static void fakeAccInit(void) {}
static bool fakeAccRead(int16_t *accData) {
memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
for(int i=0;i<XYZ_AXIS_COUNT;++i) {
accData[i] = fake_acc_values[i];
}
return true;
}

2
src/main/sensors/sonar.c
View File

@ -101,7 +101,7 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.exti_irqn = EXTI0_IRQn
};
return &sonarHardware;
#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI)
#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_0, // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )

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

@ -157,6 +157,7 @@
//#define DISPLAY
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define SPEKTRUM_BIND
// USART2, PA3

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

@ -71,10 +71,6 @@
#define MAG
#define USE_MAG_HMC5883
#define INVERTER
#define BEEPER
#define DISPLAY
#define USE_VCP
#define USE_USART1
#define USE_USART3
@ -117,28 +113,32 @@
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define SONAR
#define USE_SERVOS
#define USE_CLI
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#undef DISPLAY
#undef SONAR
//#if defined(OPBL) && defined(USE_SERIAL_1WIRE)
#undef BARO
//#undef BLACKBOX
#undef GPS
//#endif
#define SKIP_CLI_COMMAND_HELP
#define SPEKTRUM_BIND
// USART3, PB11 (Flexport)
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
#define USE_QUATERNION
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define INVERTER
#define BEEPER
#define DISPLAY
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define SONAR
//#define GPS
#undef BARO
#ifdef CC3D_OPBL
#define SKIP_CLI_COMMAND_HELP
//#define SKIP_PID_LUXFLOAT
#undef DISPLAY
#undef SONAR
#undef GPS
#endif

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

@ -60,6 +60,7 @@
#define SERIAL_RX
//#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define SPEKTRUM_BIND
// USART2, PA3

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

@ -192,5 +192,6 @@
#define SERIAL_RX
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

8
src/main/target/DOGE/target.h
View File

@ -37,12 +37,7 @@
#define BEEP_GPIO GPIOB
#define BEEP_PIN Pin_2
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOB
//#define BEEPER_INVERTED
// #define BEEP_GPIO GPIOB
// #define BEEP_PIN Pin_13
// #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOB
// #define BEEPER_INVERTED
#define BEEPER_INVERTED
// tqfp48 pin 3
#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOC
@ -195,6 +190,7 @@
#define SERIAL_RX
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define SPEKTRUM_BIND
// Use UART3 for speksat

371
src/main/target/FURYF3/system_stm32f30x.c Normal file
View File

@ -0,0 +1,371 @@
/**
******************************************************************************
* @file system_stm32f30x.c
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F30x devices,
* and is generated by the clock configuration tool
* stm32f30x_Clock_Configuration_V1.0.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f30x.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (8 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f30x.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
* in "stm32f30x.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
* Supported STM32F30x device
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* APB1 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*----------------------------------------------------------------------------
* PLLMUL | 9
*-----------------------------------------------------------------------------
* PREDIV | 1
*-----------------------------------------------------------------------------
* USB Clock | ENABLE
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 2
*-----------------------------------------------------------------------------
* Prefetch Buffer | ON
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/** @addtogroup STM32F30x_System_Private_Includes
* @{
*/
#include "stm32f30x.h"
uint32_t hse_value = HSE_VALUE;
/**
* @}
*/
/* Private typedef -----------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Defines
* @{
*/
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
/**
* @}
*/
/* Private macro -------------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Variables
* @{
*/
uint32_t SystemCoreClock = 72000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_FunctionPrototypes
* @{
*/
void SetSysClock(void);
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR &= 0xF87FC00C;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
RCC->CFGR &= (uint32_t)0xFF80FFFF;
/* Reset PREDIV1[3:0] bits */
RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
/* Reset USARTSW[1:0], I2CSW and TIMs bits */
RCC->CFGR3 &= (uint32_t)0xFF00FCCC;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
//SetSysClock(); // called from main()
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate (void)
{
uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock */
/* Get PLL clock source and multiplication factor ----------------------*/
pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
pllmull = ( pllmull >> 18) + 2;
if (pllsource == 0x00)
{
/* HSI oscillator clock divided by 2 selected as PLL clock entry */
SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
}
else
{
prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
/* HSE oscillator clock selected as PREDIV1 clock entry */
SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;
}
break;
default: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK clock frequency ----------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
void SetSysClock(void)
{
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
/* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Enable Prefetch Buffer and set Flash Latency */
FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1;
/* HCLK = SYSCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 2 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2;
/* PLL configuration */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9);
/* Enable PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Select PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
/* Wait till PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

76
src/main/target/FURYF3/system_stm32f30x.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @file system_stm32f30x.h
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/**
* @brief Define to prevent recursive inclusion
*/
#ifndef __SYSTEM_STM32F30X_H
#define __SYSTEM_STM32F30X_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
/** @addtogroup STM32F30x_System_Exported_Functions
* @{
*/
extern void SystemInit(void);
extern void SystemCoreClockUpdate(void);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /*__SYSTEM_STM32F30X_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

223
src/main/target/FURYF3/target.h Normal file
View File

@ -0,0 +1,223 @@
/*
* 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
#define TARGET_BOARD_IDENTIFIER "FURY"
#define LED0
#define LED0_GPIO GPIOC
#define LED0_PIN Pin_14
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER_INVERTED
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define GYRO
#define ACC
#define MPU6000_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA
#define MPU6000_CS_GPIO GPIOA
#define MPU6000_CS_PIN GPIO_Pin_4
#define MPU6000_SPI_INSTANCE SPI1
#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA
#define MPU6500_CS_GPIO GPIOA
#define MPU6500_CS_PIN GPIO_Pin_4
#define MPU6500_SPI_INSTANCE SPI1
#define USE_GYRO_SPI_MPU6000
#define GYRO_MPU6000_ALIGN CW180_DEG // changedkb 270
#define USE_ACC_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG // changedkb 270
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW90_DEG // changedkb 270
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW90_DEG // changedkb 270
#define BARO
#define USE_BARO_MS5611
#define USE_BARO_BMP280
#define USE_SPI
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define SPI2_GPIO GPIOB
#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
#define SPI2_NSS_PIN Pin_12
#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
#define SPI2_SCK_PIN Pin_13
#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
#define SPI2_MISO_PIN Pin_14
#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
#define SPI2_MOSI_PIN Pin_15
#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
//#define USE_FLASHFS
//#define USE_FLASH_M25P16
//#define M25P16_CS_GPIO GPIOB
//#define M25P16_CS_PIN GPIO_Pin_12
//#define M25P16_SPI_INSTANCE SPI2
#define USE_SDCARD
#define USE_SDCARD_SPI2
#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN GPIO_Pin_2
#define SDCARD_DETECT_EXTI_LINE EXTI_Line2
#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2
#define SDCARD_DETECT_GPIO_PORT GPIOB
#define SDCARD_DETECT_GPIO_CLK RCC_AHBPeriph_GPIOB
#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn
#define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_SPI_CS_GPIO SPI2_GPIO
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN
// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init:
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
// Divide to under 25MHz for normal operation:
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2
// Note, this is the same DMA channel as USART1_RX. Luckily we don't use DMA for USART Rx.
#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
// Performance logging for SD card operations:
// #define AFATFS_USE_INTROSPECTIVE_LOGGING
#define USABLE_TIMER_CHANNEL_COUNT 8
#define USB_IO
#define USE_VCP
#define USE_USART1
#define USE_USART2
#define USE_USART3
#define USE_SOFTSERIAL1
#define SERIAL_PORT_COUNT 5
#ifndef UART1_GPIO
#define UART1_TX_PIN GPIO_Pin_9 // PA9
#define UART1_RX_PIN GPIO_Pin_10 // PA10
#define UART1_GPIO GPIOA
#define UART1_GPIO_AF GPIO_AF_7
#define UART1_TX_PINSOURCE GPIO_PinSource9
#define UART1_RX_PINSOURCE GPIO_PinSource10
#endif
#define UART2_TX_PIN GPIO_Pin_14 // PA14
#define UART2_RX_PIN GPIO_Pin_15 // PA15
#define UART2_GPIO GPIOA
#define UART2_GPIO_AF GPIO_AF_7
#define UART2_TX_PINSOURCE GPIO_PinSource14
#define UART2_RX_PINSOURCE GPIO_PinSource15
#ifndef UART3_GPIO
#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
#define UART3_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
#endif
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 1
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 2
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4)
#define I2C1_SCL_GPIO GPIOB
#define I2C1_SCL_GPIO_AF GPIO_AF_4
#define I2C1_SCL_PIN GPIO_Pin_8
#define I2C1_SCL_PIN_SOURCE GPIO_PinSource8
#define I2C1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB
#define I2C1_SDA_GPIO GPIOB
#define I2C1_SDA_GPIO_AF GPIO_AF_4
#define I2C1_SDA_PIN GPIO_Pin_9
#define I2C1_SDA_PIN_SOURCE GPIO_PinSource9
#define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define ADC_INSTANCE ADC1
#define ADC_DMA_CHANNEL DMA1_Channel1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
#define VBAT_ADC_CHANNEL ADC_Channel_1
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_2
#define CURRENT_METER_ADC_GPIO GPIOA
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_2
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_3
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_GPIO GPIOA
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
#define WS2811_GPIO_AF GPIO_AF_6
#define WS2811_PIN GPIO_Pin_8
#define WS2811_PIN_SOURCE GPIO_PinSource8
#define WS2811_TIMER TIM1
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define BLACKBOX
#define DISPLAY
#define GPS
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI
#define SONAR
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define SPEKTRUM_BIND
// USART3,
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

53
src/main/target/IRCFUSIONF3/target.h
View File

@ -19,26 +19,18 @@
#define TARGET_BOARD_IDENTIFIER "IFF3"
#define LED0
#define LED0_GPIO GPIOB
#define LED0_PIN Pin_3
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 17
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define GYRO
#define USE_GYRO_MPU6050
#define GYRO_MPU6050_ALIGN CW270_DEG
@ -48,27 +40,15 @@
#define ACC_MPU6050_ALIGN CW270_DEG
#define BARO
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define MAG
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW270_DEG
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define SONAR
#define BEEPER
#define LED0
#define USE_USART1
#define USE_USART2
#define USE_USART3
#define USE_SOFTSERIAL1
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 5
#define SERIAL_PORT_COUNT 3
#ifndef UART1_GPIO
#define UART1_TX_PIN GPIO_Pin_9 // PA9
@ -95,13 +75,6 @@
#define UART3_RX_PINSOURCE GPIO_PinSource11
#endif
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
#define SOFTSERIAL_2_TIMER TIM3
#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
@ -132,29 +105,12 @@
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_12
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_GPIO GPIOA
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
#define WS2811_GPIO_AF GPIO_AF_6
#define WS2811_PIN GPIO_Pin_8
#define WS2811_PIN_SOURCE GPIO_PinSource8
#define WS2811_TIMER TIM1
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define BLACKBOX
#define DISPLAY
#define GPS
//#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
#define TELEMETRY
#define USE_CLI
#define SPEKTRUM_BIND
@ -180,4 +136,3 @@
#define S1W_RX_PIN GPIO_Pin_10
#endif

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

@ -154,6 +154,7 @@
#define TELEMETRY
#define SERIAL_RX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define USE_SERVOS
#define USE_CLI

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

@ -121,6 +121,7 @@
#define TELEMETRY
#define BLACKBOX
#define SERIAL_RX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
//#define GPS
//#define GTUNE
#define DISPLAY

372
src/main/target/SINGULARITY/system_stm32f30x.c Normal file
View File

@ -0,0 +1,372 @@
/**
******************************************************************************
* @file system_stm32f30x.c
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F30x devices,
* and is generated by the clock configuration tool
* stm32f30x_Clock_Configuration_V1.0.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f30x.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (8 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f30x.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
* in "stm32f30x.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
* Supported STM32F30x device
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* APB1 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*----------------------------------------------------------------------------
* PLLMUL | 9
*-----------------------------------------------------------------------------
* PREDIV | 1
*-----------------------------------------------------------------------------
* USB Clock | ENABLE
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 2
*-----------------------------------------------------------------------------
* Prefetch Buffer | ON
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/** @addtogroup STM32F30x_System_Private_Includes
* @{
*/
#include "stm32f30x.h"
uint32_t hse_value = HSE_VALUE;
/**
* @}
*/
/* Private typedef -----------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Defines
* @{
*/
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
/**
* @}
*/
/* Private macro -------------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Variables
* @{
*/
uint32_t SystemCoreClock = 72000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_FunctionPrototypes
* @{
*/
void SetSysClock(void);
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR &= 0xF87FC00C;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
RCC->CFGR &= (uint32_t)0xFF80FFFF;
/* Reset PREDIV1[3:0] bits */
RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
/* Reset USARTSW[1:0], I2CSW and TIMs bits */
RCC->CFGR3 &= (uint32_t)0xFF00FCCC;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
//SetSysClock(); // called from main()
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate (void)
{
uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock */
/* Get PLL clock source and multiplication factor ----------------------*/
pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
pllmull = ( pllmull >> 18) + 2;
if (pllsource == 0x00)
{
/* HSI oscillator clock divided by 2 selected as PLL clock entry */
SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
}
else
{
prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
/* HSE oscillator clock selected as PREDIV1 clock entry */
SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;
}
break;
default: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK clock frequency ----------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
void SetSysClock(void)
{
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
/* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Enable Prefetch Buffer and set Flash Latency */
FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1;
/* HCLK = SYSCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 2 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2;
/* PLL configuration */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9);
/* Enable PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Select PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
/* Wait till PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

76
src/main/target/SINGULARITY/system_stm32f30x.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @file system_stm32f30x.h
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/**
* @brief Define to prevent recursive inclusion
*/
#ifndef __SYSTEM_STM32F30X_H
#define __SYSTEM_STM32F30X_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
/** @addtogroup STM32F30x_System_Exported_Functions
* @{
*/
extern void SystemInit(void);
extern void SystemCoreClockUpdate(void);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /*__SYSTEM_STM32F30X_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

139
src/main/target/SINGULARITY/target.h Normal file
View File

@ -0,0 +1,139 @@
/*
* 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
#define TARGET_BOARD_IDENTIFIER "SING"
#define LED0_GPIO GPIOB
#define LED0_PIN Pin_3
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define USABLE_TIMER_CHANNEL_COUNT 10
#define USE_MPU_DATA_READY_SIGNAL
#define GYRO
#define USE_GYRO_MPU6050
#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP
#define ACC
#define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW0_DEG_FLIP
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define BEEPER
#define LED0
#define USE_VCP
#define USE_USART1 // JST-SH Serial - TX (PA9) RX (PA10)
#define USE_USART2 // Input - TX (NC) RX (PA15)
#define USE_USART3 // Solder Pads - TX (PB10) RX (PB11)
#define USE_SOFTSERIAL1 // Telemetry
#define SERIAL_PORT_COUNT 5
#define UART1_TX_PIN GPIO_Pin_9
#define UART1_RX_PIN GPIO_Pin_10
#define UART1_GPIO GPIOA
#define UART1_GPIO_AF GPIO_AF_7
#define UART1_TX_PINSOURCE GPIO_PinSource9
#define UART1_RX_PINSOURCE GPIO_PinSource10
#define UART2_TX_PIN GPIO_Pin_14 //Not connected
#define UART2_RX_PIN GPIO_Pin_15
#define UART2_GPIO GPIOA
#define UART2_GPIO_AF GPIO_AF_7
#define UART2_TX_PINSOURCE GPIO_PinSource14
#define UART2_RX_PINSOURCE GPIO_PinSource15
#define UART3_TX_PIN GPIO_Pin_10
#define UART3_RX_PIN GPIO_Pin_11
#define UART3_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
#define SOFTSERIAL_1_TIMER TIM15
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 7 //Not connected
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 8
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
#define USE_SPI
#define USE_SPI_DEVICE_1 // PA4, 5, 6, 7
#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define VTX
#define RTC6705_CS_GPIO GPIOA
#define RTC6705_CS_PIN GPIO_Pin_4
#define RTC6705_SPI_INSTANCE SPI1
#define M25P16_CS_GPIO GPIOB
#define M25P16_CS_PIN GPIO_Pin_12
#define M25P16_SPI_INSTANCE SPI2
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define ADC_INSTANCE ADC2
#define ADC_DMA_CHANNEL DMA2_Channel1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2
#define VBAT_ADC_GPIO GPIOB
#define VBAT_ADC_GPIO_PIN GPIO_Pin_2
#define VBAT_ADC_CHANNEL ADC_Channel_12
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_GPIO GPIOA
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
#define WS2811_GPIO_AF GPIO_AF_6
#define WS2811_PIN GPIO_Pin_8
#define WS2811_PIN_SOURCE GPIO_PinSource8
#define WS2811_TIMER TIM1
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define AUTOTUNE
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define GPS
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL)
#define SPEKTRUM_BIND
// USART2, PA15
#define BIND_PORT GPIOA
#define BIND_PIN Pin_15
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

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

@ -126,6 +126,7 @@
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define LED_STRIP
#if 1

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

@ -158,6 +158,9 @@
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define SPEKTRUM_BIND
// USART3,
#define BIND_PORT GPIOB

5
src/main/target/SPRACINGF3EVO/target.h
View File

@ -206,8 +206,6 @@
#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2
#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define GPS
#define BLACKBOX
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@ -217,6 +215,9 @@
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY)
#define SPEKTRUM_BIND
// USART3,
#define BIND_PORT GPIOB

2
src/main/target/SPRACINGF3MINI/target.h
View File

@ -212,10 +212,10 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define USE_SERVOS
#define USE_CLI
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define BUTTONS
#define BUTTON_A_PORT GPIOB

1
src/main/telemetry/ltm.c
View File

@ -63,6 +63,7 @@
#include "io/ledstrip.h"
#include "io/beeper.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "rx/rx.h"

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

@ -38,6 +38,7 @@
#include "io/serial.h"
#include "io/ledstrip.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"

4
src/main/version.h
View File

@ -16,8 +16,8 @@
*/
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed
#define FC_VERSION_MINOR 8 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)

4
top_makefile
View File

@ -14,6 +14,8 @@ ALL_TARGETS += rmdo
ALL_TARGETS += ircfusionf3
ALL_TARGETS += afromini
ALL_TARGETS += doge
ALL_TARGETS += singularity
ALL_TARGETS += furyf3
CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS))
@ -33,6 +35,8 @@ clean_rmdo rmdo : opts := TARGET=RMDO
clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3
clean_afromini afromini : opts := TARGET=AFROMINI
clean_doge doge : opts := TARGET=DOGE
clean_singularity singularity : opts := TARGET=SINGULARITY
clean_furyf3 furyf3 : opts := TARGET=FURYF3
.PHONY: all clean