diff --git a/Makefile b/Makefile index a1000e796..b6243c777 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 9d1fe6616..4babddd21 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -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) diff --git a/q b/q new file mode 100644 index 000000000..04586033b --- /dev/null +++ b/q @@ -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. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e7513a588..4b3aa51cc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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; } diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 1f3a97496..82c105eb4 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -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" diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 7d26d2ca8..21fd2948f 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -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; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 500995602..809063f4b 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -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); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 3be8eff72..888847e07 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4a32e282c..3251c237a 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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; +} diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 3a3730fd5..4e3f8b4c2 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -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; diff --git a/src/main/common/typeconversion.h b/src/main/common/typeconversion.h index f2811d65b..62b437875 100644 --- a/src/main/common/typeconversion.h +++ b/src/main/common/typeconversion.h @@ -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 diff --git a/src/main/config/config.c b/src/main/config/config.c index 0d777c1ac..165ee4ee3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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(¤tProfile->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; } diff --git a/src/main/config/config.h b/src/main/config/config.h index ff43e5a14..219cc31be 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -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); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b00e449aa..ab58d68b0 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/debug.h b/src/main/debug.h index facb9982e..4f6250b75 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -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; diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 8b33e7e46..b45f6bd6b 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -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 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1df96dd25..e7b32413c 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -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); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 9d8377f17..aaaf4ac6b 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -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; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 97a8fef65..56759cbd4 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -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 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 11036d527..060d7f388 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -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); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index b87fecd11..df91273f2 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -20,8 +20,9 @@ #include -#include "platform.h" +#include #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 */ diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 1cc192121..af25ef0c9 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -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); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 3638ad471..1c7c71fe7 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -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 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c new file mode 100644 index 000000000..4bdaf9931 --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.c @@ -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 . + */ + +/* + * 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 +#include +#include + +#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 diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h new file mode 100644 index 000000000..d72116659 --- /dev/null +++ b/src/main/drivers/vtx_rtc6705.h @@ -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 . + */ + +/* + * 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 + +#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); \ No newline at end of file diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d72331ddd..0906c4ac4 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -419,11 +419,6 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims) } } -void imuUpdateGyro(void) -{ - gyroUpdate(); -} - void imuUpdateAttitude(void) { if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 3f64b5c50..6dc0a1f6b 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 802e7e780..100aeb62f 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f27c2909c..1e883716a 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0246fa71f..b9d706dd7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 94addfb55..1103f7bd9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e7ec34964..f1ac9e352 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -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 diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 7d3ef38ef..0f9f488ef 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -53,3 +53,4 @@ uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); int beeperTableEntryCount(void); +bool isBeeperOn(void); diff --git a/src/main/io/display.c b/src/main/io/display.c index 55c4d1528..ed394d9c9 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -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 = ¤tProfile->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)) { diff --git a/src/main/io/display.h b/src/main/io/display.h index b4bac0e08..8149a35ad 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -//#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 diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index 66cd7db59..b73f9920f 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -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; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index a52ed8df0..afb78f352 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -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 +#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; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 3264b0269..0bbcadc09 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -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: diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 7756aed07..a5e96e15c 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -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); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index e6abf1d86..ee2fe326f 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -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; +} + diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index f79b50599..747a934df 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -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); + diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4e6721b67..e03c31d0a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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; profileCountactiveRateProfile; + uint8_t rateCount; for (rateCount=0; rateCountactiveRateProfile); @@ -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) { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index abb6cbcda..1a2bd757e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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(); } } diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c new file mode 100644 index 000000000..3a48bc1a6 --- /dev/null +++ b/src/main/io/vtx.c @@ -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 . + */ + +#include +#include +#include + +#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 \ No newline at end of file diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h new file mode 100644 index 000000000..ddcd7492d --- /dev/null +++ b/src/main/io/vtx.h @@ -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 . + */ + +#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(); \ No newline at end of file diff --git a/src/main/main.c b/src/main/main.c index 54912c495..d5f537039 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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/ diff --git a/src/main/mw.c b/src/main/mw.c index cd54285d3..0bd6751ee 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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( ¤tProfile->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 diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6e2dd2124..b76873fb1 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -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; diff --git a/src/main/scheduler.h b/src/main/scheduler.h index 807ffeb84..c55bbaeb0 100755 --- a/src/main/scheduler.h +++ b/src/main/scheduler.h @@ -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 diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c index cddb7ae1b..59ee0ac9e 100644 --- a/src/main/scheduler_tasks.c +++ b/src/main/scheduler_tasks.c @@ -22,14 +22,15 @@ #include #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", diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index d2d892fd6..38006a04f 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index de3412760..b22955cea 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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]); + } + } + } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 48e5e677e..05f78706f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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 { diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2c65a6d90..6a49a50e3 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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
© COPYRIGHT 2014 STMicroelectronics
+ * + * 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****/ \ No newline at end of file diff --git a/src/main/target/FURYF3/system_stm32f30x.h b/src/main/target/FURYF3/system_stm32f30x.h new file mode 100644 index 000000000..57e5e05d7 --- /dev/null +++ b/src/main/target/FURYF3/system_stm32f30x.h @@ -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 + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * 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****/ \ No newline at end of file diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h new file mode 100644 index 000000000..99d7340d7 --- /dev/null +++ b/src/main/target/FURYF3/target.h @@ -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 . + */ + +#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 \ No newline at end of file diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index ccc9fa997..c3b99b5c7 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -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 - diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index a3b370df7..2d04a2fee 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -154,6 +154,7 @@ #define TELEMETRY #define SERIAL_RX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 9f168587f..23e54ab3b 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -121,6 +121,7 @@ #define TELEMETRY #define BLACKBOX #define SERIAL_RX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM //#define GPS //#define GTUNE #define DISPLAY diff --git a/src/main/target/SINGULARITY/system_stm32f30x.c b/src/main/target/SINGULARITY/system_stm32f30x.c new file mode 100644 index 000000000..fca696982 --- /dev/null +++ b/src/main/target/SINGULARITY/system_stm32f30x.c @@ -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 + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * 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****/ + diff --git a/src/main/target/SINGULARITY/system_stm32f30x.h b/src/main/target/SINGULARITY/system_stm32f30x.h new file mode 100644 index 000000000..4f999d305 --- /dev/null +++ b/src/main/target/SINGULARITY/system_stm32f30x.h @@ -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 + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * 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****/ diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h new file mode 100644 index 000000000..fbc58bae3 --- /dev/null +++ b/src/main/target/SINGULARITY/target.h @@ -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 . + */ + +#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 + diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 8ff56c86c..5e69ef108 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -126,6 +126,7 @@ #define TELEMETRY #define USE_SERVOS #define USE_CLI +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP #if 1 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index c5a0e5265..0e0ede72c 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -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 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 4586dbc2d..73ea225dc 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -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 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 5459d4f9a..6151d3549 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -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 diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index d4dd78ae8..fd775c877 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -63,6 +63,7 @@ #include "io/ledstrip.h" #include "io/beeper.h" #include "io/osd.h" +#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 002d073ae..36b7c9556 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -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" diff --git a/src/main/version.h b/src/main/version.h index 587b442c7..3cd436cc2 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -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) diff --git a/top_makefile b/top_makefile index d765dcf35..bbc1f43f1 100644 --- a/top_makefile +++ b/top_makefile @@ -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