diff --git a/.travis.sh b/.travis.sh index 0d6a3cdc1a..1e90137c37 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,10 +1,11 @@ #!/bin/bash +FC_VER=$(make version) REVISION=$(git rev-parse --short HEAD) BRANCH=$(git rev-parse --abbrev-ref HEAD) REVISION=$(git rev-parse --short HEAD) LAST_COMMIT_DATE=$(git log -1 --date=short --format="%cd") -TARGET_FILE=obj/betaflight_${TARGET} +TARGET_FILE=obj/betaflight_${FC_VER}_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} @@ -46,7 +47,8 @@ elif [ $PUBLISHMETA ] ; then fi elif [ $TARGET ] ; then - make $TARGET + make $TARGET || exit $? + if [ $PUBLISH_URL ] ; then if [ -f ${TARGET_FILE}.bin ] ; then TARGET_FILE=${TARGET_FILE}.bin @@ -60,8 +62,10 @@ elif [ $TARGET ] ; then curl -k "${CURL_BASEOPTS[@]}" "${CURL_PUB_BASEOPTS[@]}" --form "file=@${TARGET_FILE}" ${PUBLISH_URL} || true exit 0; fi + elif [ $GOAL ] ; then make V=0 $GOAL + else make V=0 all fi diff --git a/.travis.yml b/.travis.yml index c8e1207206..5e012ae20f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,64 +4,19 @@ env: # - PUBLISHDOCS=True # Specify the main Mafile supported goals. - GOAL=test - - GOAL=all + - GOAL=targets-group-1 + - GOAL=targets-group-2 + - GOAL=targets-group-3 + - GOAL=targets-group-4 + - GOAL=targets-group-rest +# - GOAL=all +# - GOAL=AFROMINI +# - GOAL=AIORACERF3 +# - GOAL=... # Or specify targets to run. # - TARGET=AFROMINI # - TARGET=AIORACERF3 -# - TARGET=AIR32 -# - TARGET=AIRBOTF4 -# - TARGET=AIRHEROF3 -# - TARGET=ALIENFLIGHTF1 -# - TARGET=ALIENFLIGHTF3 -# - TARGET=ALIENFLIGHTF4 -# - TARGET=ANYFCF7 -# - TARGET=BEEBRAIN -# - TARGET=BETAFLIGHTF3 -# - TARGET=BLUEJAYF4 -# - TARGET=CC3D -# - TARGET=CC3D_OPBL -# - TARGET=CHEBUZZF3 -# - TARGET=CJMCU -# - TARGET=COLIBRI -# - TARGET=COLIBRI_OPBL -# - TARGET=COLIBRI_RACE -# - TARGET=DOGE -# - TARGET=F4BY -# - TARGET=FURYF3 -# - TARGET=FURYF4 -# - TARGET=FURYF7 -# - TARGET=IMPULSERCF3 -# - TARGET=IRCFUSIONF3 -# - TARGET=ISHAPEDF3 -# - TARGET=KISSFC -# - TARGET=LUXV2_RACE -# - TARGET=LUX_RACE -# - TARGET=MICROSCISKY -# - TARGET=MOTOLAB -# - TARGET=NAZE -# - TARGET=OMNIBUS -# - TARGET=OMNIBUSF4 -# - TARGET=PIKOBLX -# - TARGET=RACEBASE -# - TARGET=RCEXPLORERF3 -# - TARGET=REVO -# - TARGET=REVOLT -# - TARGET=REVONANO -# - TARGET=REVO_OPBL -# - TARGET=RMDO -# - TARGET=SINGULARITY -# - TARGET=SIRINFPV -# - TARGET=SOULF4 -# - TARGET=SPARKY -# - TARGET=SPARKY2 -# - TARGET=SPRACINGF3 -# - TARGET=SPRACINGF3EVO -# - TARGET=SPRACINGF3MINI -# - TARGET=STM32F3DISCOVERY -# - TARGET=VRRACE -# - TARGET=X_RACERSPI -# - TARGET=YUPIF4 -# - TARGET=ZCOREF3 +# - TARGET=... # use new docker environment sudo: false @@ -73,6 +28,7 @@ addons: apt: packages: - libc6-i386 + - time # We use cpp for unit tests, and c for the main project. language: cpp @@ -100,6 +56,7 @@ cache: - downloads - tools + #notifications: # irc: "chat.freenode.net#cleanflight" # use_notice: true diff --git a/Makefile b/Makefile index 0612dd50e1..311fd88d80 100644 --- a/Makefile +++ b/Makefile @@ -110,6 +110,85 @@ VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS))) VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS) VALID_TARGETS := $(sort $(VALID_TARGETS)) +GROUP_1_TARGETS := \ + AFROMINI \ + AIORACERF3 \ + AIR32 \ + AIRBOTF4 \ + AIRBOTF4SD \ + AIRHEROF3 \ + ALIENFLIGHTF1 \ + ALIENFLIGHTF3 \ + ALIENFLIGHTF4 \ + ALIENFLIGHTNGF7 \ + ANYFCF7 \ + BEEBRAIN \ + BEEROTORF4 \ + BETAFLIGHTF3 \ + BLUEJAYF4 \ + CC3D \ + CC3D_OPBL \ + +GROUP_2_TARGETS := \ + CHEBUZZF3 \ + CJMCU \ + CL_RACINGF4 \ + COLIBRI \ + COLIBRI_OPBL \ + COLIBRI_RACE \ + DOGE \ + ELLE0 \ + F4BY \ + FISHDRONEF4 \ + FLIP32F3OSD \ + FURYF3 \ + FURYF4 \ + FURYF7 \ + IMPULSERCF3 \ + IRCFUSIONF3 \ + ISHAPEDF3 \ + +GROUP_3_TARGETS := \ + KAKUTEF4 \ + KISSCC \ + KIWIF4 \ + LUX_RACE \ + LUXV2_RACE \ + MICROSCISKY \ + MOTOLAB \ + MULTIFLITEPICO \ + NAZE \ + NERO \ + NUCLEOF7 \ + OMNIBUS \ + OMNIBUSF4 \ + OMNIBUSF4SD \ + PIKOBLX \ + PLUMF4 \ + PODIUMF4 \ + +GROUP_4_TARGETS := \ + RCEXPLORERF3 \ + REVO \ + REVO_OPBL \ + REVOLT \ + REVONANO \ + RMDO \ + SINGULARITY \ + SIRINFPV \ + SOULF4 \ + SPARKY \ + SPARKY2 \ + SPRACINGF3 \ + SPRACINGF3EVO \ + SPRACINGF3MINI \ + SPRACINGF3NEO \ + STM32F3DISCOVERY \ + TINYBEEF3 \ + +GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(VALID_TARGETS)) + + ifeq ($(filter $(TARGET),$(ALT_TARGETS)), $(TARGET)) BASE_TARGET := $(firstword $(subst /,, $(subst ./src/main/target/,, $(dir $(wildcard $(ROOT)/src/main/target/*/$(TARGET).mk))))) -include $(ROOT)/src/main/target/$(BASE_TARGET)/$(TARGET).mk @@ -1102,16 +1181,33 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S $(V1) echo "%% $(notdir $<)" "$(STDOUT)" $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< -## official : Build all official (travis) targets -official: $(OFFICIAL_TARGETS) ## all : Build all valid targets all: $(VALID_TARGETS) +## official : Build all official (travis) targets +official: $(OFFICIAL_TARGETS) + +## targets-group-1 : build some targets +targets-group-1: $(GROUP_1_TARGETS) + +## targets-group-2 : build some targets +targets-group-2: $(GROUP_2_TARGETS) + +## targets-group-3 : build some targets +targets-group-3: $(GROUP_3_TARGETS) + +## targets-group-3 : build some targets +targets-group-4: $(GROUP_4_TARGETS) + +## targets-group-rest: build the rest of the targets (not listed in group 1, 2 or 3) +targets-group-rest: $(GROUP_OTHER_TARGETS) + + $(VALID_TARGETS): $(V0) echo "" && \ echo "Building $@" && \ - $(MAKE) binary hex TARGET=$@ && \ + time $(MAKE) binary hex TARGET=$@ && \ echo "Building $@ succeeded." @@ -1189,6 +1285,10 @@ $(TOOLS_DIR): $(BUILD_DIR): mkdir -p $@ +## version : print firmware version +version: + @echo $(FC_VER) + ## help : print this help message and exit help: Makefile make/tools.mk $(V0) @echo "" @@ -1205,9 +1305,14 @@ help: Makefile make/tools.mk ## targets : print a list of all valid target platforms (for consumption by scripts) targets: - $(V0) @echo "Valid targets: $(VALID_TARGETS)" - $(V0) @echo "Target: $(TARGET)" - $(V0) @echo "Base target: $(BASE_TARGET)" + $(V0) @echo "Valid targets: $(VALID_TARGETS)" + $(V0) @echo "Target: $(TARGET)" + $(V0) @echo "Base target: $(BASE_TARGET)" + $(V0) @echo "targets-group-1: $(GROUP_1_TARGETS)" + $(V0) @echo "targets-group-2: $(GROUP_2_TARGETS)" + $(V0) @echo "targets-group-3: $(GROUP_3_TARGETS)" + $(V0) @echo "targets-group-4: $(GROUP_4_TARGETS)" + $(V0) @echo "targets-group-rest: $(GROUP_OTHER_TARGETS)" ## test : run the cleanflight test suite ## junittest : run the cleanflight test suite, producing Junit XML result files. diff --git a/make/tools.mk b/make/tools.mk index 7a889ec3a7..c3f251404d 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -14,7 +14,7 @@ ############################## # Set up ARM (STM32) SDK -ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q1-update +ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q1-update # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) GCC_REQUIRED_VERSION ?= 6.3.1 @@ -286,16 +286,17 @@ zip_clean: # ############################## -GCC_VERSION=$(shell arm-none-eabi-gcc -dumpversion) ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists) ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi- else ifeq (,$(findstring _install,$(MAKECMDGOALS))) + GCC_VERSION = $(shell arm-none-eabi-gcc -dumpversion) ifeq ($(GCC_VERSION),) $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo) else ifneq ($(GCC_VERSION), $(GCC_REQUIRED_VERSION)) $(error **ERROR** your arm-none-eabi-gcc is '$(GCC_VERSION)', but '$(GCC_REQUIRED_VERSION)' is expected. Override with 'GCC_REQUIRED_VERSION' in make/local.mk or run 'make arm_sdk_install' to install the right version automatically in the tools folder of this repo) endif - # not installed, hope it's in the path... + + # ARM tookchain is in the path, and the version is what's required. ARM_SDK_PREFIX ?= arm-none-eabi- endif diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 66144c16bd..58b33cead4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1214,7 +1214,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { - blackboxPrintfHeaderLine("vbatscale", "%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); + blackboxPrintfHeaderLine("vbat_scale", "%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } @@ -1234,13 +1234,13 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom); - BLACKBOX_PRINT_HEADER_LINE("rcRate", "%d", currentControlRateProfile->rcRate8); - BLACKBOX_PRINT_HEADER_LINE("rcExpo", "%d", currentControlRateProfile->rcExpo8); - BLACKBOX_PRINT_HEADER_LINE("rcYawRate", "%d", currentControlRateProfile->rcYawRate8); - BLACKBOX_PRINT_HEADER_LINE("rcYawExpo", "%d", currentControlRateProfile->rcYawExpo8); - BLACKBOX_PRINT_HEADER_LINE("thrMid", "%d", currentControlRateProfile->thrMid8); - BLACKBOX_PRINT_HEADER_LINE("thrExpo", "%d", currentControlRateProfile->thrExpo8); - BLACKBOX_PRINT_HEADER_LINE("dynThrPID", "%d", currentControlRateProfile->dynThrPID); + BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8); + BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8); + BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->rcYawExpo8); + BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); + BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); + BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], currentControlRateProfile->rates[PITCH], @@ -1277,45 +1277,44 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent", "%d", currentPidProfile->itermWindupPointPercent); - BLACKBOX_PRINT_HEADER_LINE("dterm_average_count", "%d", currentPidProfile->dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation", "%d", currentPidProfile->vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("d_notch_cut", "%d", currentPidProfile->dterm_notch_cutoff); + BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); + BLACKBOX_PRINT_HEADER_LINE("anti_gravity_thresh", "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", castFloatBytesToInt(currentPidProfile->itermAcceleratorGain)); - BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio", "%d", currentPidProfile->setpointRelaxRatio); - BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight", "%d", currentPidProfile->dtermSetpointWeight); - BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit", "%d", castFloatBytesToInt(currentPidProfile->yawRateAccelLimit)); - BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit", "%d", castFloatBytesToInt(currentPidProfile->rateAccelLimit)); - BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimit)); - BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimitYaw)); + BLACKBOX_PRINT_HEADER_LINE("setpoint_relax_ratio", "%d", currentPidProfile->setpointRelaxRatio); + BLACKBOX_PRINT_HEADER_LINE("d_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("yaw_accel_limit", "%d", castFloatBytesToInt(currentPidProfile->yawRateAccelLimit)); + BLACKBOX_PRINT_HEADER_LINE("accel_limit", "%d", castFloatBytesToInt(currentPidProfile->rateAccelLimit)); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimit)); + BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", castFloatBytesToInt(currentPidProfile->pidSumLimitYaw)); // End of Betaflight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf); - BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type", "%d", gyroConfig()->gyro_soft_lpf_type); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass", "%d", gyroConfig()->gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, gyroConfig()->gyro_soft_notch_cutoff_2); - BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); - BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); - BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); + BLACKBOX_PRINT_HEADER_LINE("rc_interp", "%d", rxConfig()->rcInterpolation); + BLACKBOX_PRINT_HEADER_LINE("rc_interp_int", "%d", rxConfig()->rcInterpolationInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); - BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); - BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); + BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); + BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); - BLACKBOX_PRINT_HEADER_LINE("digitalIdleOffset", "%d", (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("digital_idle_percent", "%d", (int)(motorConfig()->digitalIdleOffsetPercent * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); @@ -1675,22 +1674,12 @@ void handleBlackbox(timeUs_t currentTimeUs) } } -static bool canUseBlackboxWithCurrentConfiguration(void) -{ -#ifdef USE_SDCARD - return feature(FEATURE_BLACKBOX) && - !(blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD && !feature(FEATURE_SDCARD)); -#else - return feature(FEATURE_BLACKBOX); -#endif -} - /** * Call during system startup to initialize the blackbox. */ void initBlackbox(void) { - if (canUseBlackboxWithCurrentConfiguration()) { + if (blackboxConfig()->device) { blackboxSetState(BLACKBOX_STATE_STOPPED); } else { blackboxSetState(BLACKBOX_STATE_DISABLED); diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 95db665747..44269c5b44 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -24,14 +24,14 @@ #include "config/parameter_group.h" typedef enum BlackboxDevice { - BLACKBOX_DEVICE_SERIAL = 0, + BLACKBOX_DEVICE_NONE = 0, #ifdef USE_FLASHFS BLACKBOX_DEVICE_FLASH = 1, #endif #ifdef USE_SDCARD BLACKBOX_DEVICE_SDCARD = 2, #endif - + BLACKBOX_DEVICE_SERIAL = 3 } BlackboxDevice_e; typedef struct blackboxConfig_s { diff --git a/src/main/build/debug.h b/src/main/build/debug.h index cd6fc1d780..c7a0ea8473 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -62,5 +62,7 @@ typedef enum { DEBUG_ESC_SENSOR, DEBUG_SCHEDULER, DEBUG_STACK, + DEBUG_ESC_SENSOR_RPM, + DEBUG_ESC_SENSOR_TMP, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 1eb19e684a..1429a2a6f8 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -77,14 +77,12 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) #endif // USE_FLASHFS static const char * const cmsx_BlackboxDeviceNames[] = { - "SERIAL", + "NONE", "FLASH ", - "SDCARD" + "SDCARD", + "SERIAL" }; -static bool featureRead = false; - -static uint8_t cmsx_FeatureBlackbox; static uint8_t blackboxConfig_rate_denom; static uint8_t cmsx_BlackboxDevice; @@ -171,10 +169,6 @@ static long cmsx_Blackbox_onEnter(void) cmsx_Blackbox_GetDeviceStatus(); cmsx_BlackboxDevice = blackboxConfig()->device; - if (!featureRead) { - cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; - featureRead = true; - } blackboxConfig_rate_denom = blackboxConfig()->rate_denom; return 0; } @@ -193,20 +187,12 @@ static long cmsx_Blackbox_onExit(const OSD_Entry *self) static long cmsx_Blackbox_FeatureWriteback(void) { - if (featureRead) { - if (cmsx_FeatureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); - } - return 0; } static OSD_Entry cmsx_menuBlackboxEntries[] = { { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, - { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, 0 }, { "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 }, { "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 }, diff --git a/src/main/common/axis.h b/src/main/common/axis.h index 387d96c265..b0921f9e80 100644 --- a/src/main/common/axis.h +++ b/src/main/common/axis.h @@ -40,3 +40,5 @@ typedef enum { } angle_index_t; #define ANGLE_INDEX_COUNT 2 + +#define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1) diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 0f47511e03..910fe99c78 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -39,6 +39,21 @@ void sbufWriteU32(sbuf_t *dst, uint32_t val) sbufWriteU8(dst, val >> 24); } +void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val) +{ + sbufWriteU8(dst, val >> 8); + sbufWriteU8(dst, (uint8_t)val); +} + +void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val) +{ + sbufWriteU8(dst, val >> 24); + sbufWriteU8(dst, val >> 16); + sbufWriteU8(dst, val >> 8); + sbufWriteU8(dst, (uint8_t)val); +} + + void sbufWriteData(sbuf_t *dst, const void *data, int len) { memcpy(dst->ptr, data, len); @@ -47,7 +62,7 @@ void sbufWriteData(sbuf_t *dst, const void *data, int len) void sbufWriteString(sbuf_t *dst, const char *string) { - sbufWriteData(dst, string, strlen(string)); + sbufWriteData(dst, string, strlen(string) + 1); // include zero terminator } uint8_t sbufReadU8(sbuf_t *src) @@ -90,6 +105,11 @@ uint8_t* sbufPtr(sbuf_t *buf) return buf->ptr; } +const uint8_t* sbufConstPtr(const sbuf_t *buf) +{ + return buf->ptr; +} + // advance buffer pointer // reader - skip data // writer - commit written data diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index 7de771ef02..dad54adddb 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -30,6 +30,8 @@ typedef struct sbuf_s { void sbufWriteU8(sbuf_t *dst, uint8_t val); void sbufWriteU16(sbuf_t *dst, uint16_t val); void sbufWriteU32(sbuf_t *dst, uint32_t val); +void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); +void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); void sbufWriteData(sbuf_t *dst, const void *data, int len); void sbufWriteString(sbuf_t *dst, const char *string); @@ -40,6 +42,7 @@ void sbufReadData(sbuf_t *dst, void *data, int len); int sbufBytesRemaining(sbuf_t *buf); uint8_t* sbufPtr(sbuf_t *buf); +const uint8_t* sbufConstPtr(const sbuf_t *buf); void sbufAdvance(sbuf_t *buf, int size); void sbufSwitchToReader(sbuf_t *buf, uint8_t * base); diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index bd586459c9..7b46e94955 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -20,7 +20,7 @@ #include #include -#define EEPROM_CONF_VERSION 158 +#define EEPROM_CONF_VERSION 159 bool isEEPROMContentValid(void); bool loadEEPROM(void); diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index c92b0d1957..586cd98bf3 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -49,6 +49,7 @@ typedef struct gyroDev_s { sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroUpdateFuncPtr update; extiCallbackRec_t exti; + busDevice_t bus; float scale; // scalefactor int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int32_t gyroZero[XYZ_AXIS_COUNT]; @@ -66,6 +67,7 @@ typedef struct gyroDev_s { typedef struct accDev_s { sensorAccInitFuncPtr init; // initialize function sensorAccReadFuncPtr read; // read 3 axis data function + busDevice_t bus; uint16_t acc_1G; int16_t ADCRaw[XYZ_AXIS_COUNT]; char revisionCode; // a revision code for the sensor, if known diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 9e7c3ea98f..9e05b922de 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -41,9 +41,10 @@ #include "accgyro_mpu3050.h" #include "accgyro_mpu6050.h" #include "accgyro_mpu6500.h" +#include "accgyro_spi_bmi160.h" +#include "accgyro_spi_icm20689.h" #include "accgyro_spi_mpu6000.h" #include "accgyro_spi_mpu6500.h" -#include "accgyro_spi_icm20689.h" #include "accgyro_spi_mpu9250.h" #include "accgyro_mpu.h" @@ -75,7 +76,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // determine product ID and accel revision - ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer); + ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, 6, readBuffer); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); if (revision) { /* Congrats, these parts are better. */ @@ -89,7 +90,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { - ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId); + ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { failureMode(FAILURE_ACC_INCOMPATIBLE); @@ -160,14 +161,16 @@ static void mpuIntExtiInit(gyroDev_t *gyro) #endif } -static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) +bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data) { + UNUSED(bus); bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); return ack; } -static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data) +bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data) { + UNUSED(bus); bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); return ack; } @@ -176,7 +179,7 @@ bool mpuAccRead(accDev_t *acc) { uint8_t data[6]; - bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data); + bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data); if (!ack) { return false; } @@ -199,7 +202,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data); + const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, gyro->mpuConfiguration.gyroReadXRegister, 6, data); if (!ack) { return false; } @@ -226,104 +229,112 @@ bool mpuCheckDataReady(gyroDev_t* gyro) #ifdef USE_SPI static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { + UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI #ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiDetect()) { +#ifdef MPU6000_CS_PIN + gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin; +#endif + if (mpu6000SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu6000ReadRegister; - gyro->mpuConfiguration.writeFn = mpu6000WriteRegister; + gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister; + gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister; return true; } #endif #ifdef USE_GYRO_SPI_MPU6500 - uint8_t mpu6500Sensor = mpu6500SpiDetect(); + gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin; + const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus); + // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu6500ReadRegister; - gyro->mpuConfiguration.writeFn = mpu6500WriteRegister; + gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister; + gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister; return true; } #endif #ifdef USE_GYRO_SPI_MPU9250 - if (mpu9250SpiDetect()) { + gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin; + if (mpu9250SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = mpu9250ReadRegister; - gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister; - gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister; - gyro->mpuConfiguration.writeFn = mpu9250WriteRegister; - gyro->mpuConfiguration.resetFn = mpu9250ResetGyro; + gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister; + gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister; + gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister; + gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister; + gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; return true; } #endif #ifdef USE_GYRO_SPI_ICM20689 - if (icm20689SpiDetect()) { + gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin; + if (icm20689SpiDetect(&gyro->bus)) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.readFn = icm20689ReadRegister; - gyro->mpuConfiguration.writeFn = icm20689WriteRegister; + gyro->mpuConfiguration.readFn = icm20689SpiReadRegister; + gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister; + return true; + } +#endif + +#ifdef USE_ACCGYRO_BMI160 + gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin; + if (bmi160Detect(&gyro->bus)) { + gyro->mpuDetectionResult.sensor = BMI_160_SPI; + gyro->mpuConfiguration.readFn = bmi160SpiReadRegister; + gyro->mpuConfiguration.writeFn = bmi160SpiWriteRegister; return true; } #endif - UNUSED(gyro); return false; } #endif -mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) +void mpuDetect(gyroDev_t *gyro) { - // MPU datasheet specifies 30ms. delay(35); -#ifndef USE_I2C uint8_t sig = 0; - bool ack = false; +#ifdef USE_I2C + bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, 1, &sig); #else - uint8_t sig; - bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); + bool ack = false; #endif if (ack) { gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; } else { #ifdef USE_SPI - bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); - UNUSED(detectedSpiSensor); + detectSPISensorsAndUpdateDetectionResult(gyro); #endif - - return &gyro->mpuDetectionResult; + return; } gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; // If an MPU3050 is connected sig will contain 0. uint8_t inquiryResult; - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); + ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_3050; gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; - return &gyro->mpuDetectionResult; + return; } sig &= MPU_INQUIRY_MASK; - if (sig == MPUx0x0_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_60x0; - mpu6050FindRevision(gyro); } else if (sig == MPU6500_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; } - - return &gyro->mpuDetectionResult; } void mpuGyroInit(gyroDev_t *gyro) diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8a44af0464..4ba15bf3f1 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -124,8 +124,8 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data); -typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data); +typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data); +typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data); typedef void(*mpuResetFnPtr)(void); extern mpuResetFnPtr mpuResetFn; @@ -175,9 +175,11 @@ typedef enum { MPU_65xx_I2C, MPU_65xx_SPI, MPU_9250_SPI, - ICM_20689_SPI, + ICM_20601_SPI, + ICM_20602_SPI, ICM_20608_SPI, - ICM_20602_SPI + ICM_20689_SPI, + BMI_160_SPI, } mpuSensor_e; typedef enum { @@ -190,12 +192,15 @@ typedef struct mpuDetectionResult_s { mpu6050Resolution_e resolution; } mpuDetectionResult_t; +bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data); +bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data); + struct gyroDev_s; void mpuGyroInit(struct gyroDev_s *gyro); struct accDev_s; bool mpuAccRead(struct accDev_s *acc); bool mpuGyroRead(struct gyroDev_s *gyro); -mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro); +void mpuDetect(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro); void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 374604b7c1..d5ed95f90a 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -53,21 +53,20 @@ static void mpu3050Init(gyroDev_t *gyro) delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0); + ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0); if (!ack) failureMode(FAILURE_ACC_INIT); - gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); - gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0); - gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET); - gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { - UNUSED(gyro); uint8_t buf[2]; - if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) { + if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, 2, buf)) { return false; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 0a1fe6f336..e57837f373 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) - gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index 30d7caa0fb..995ee26587 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); } diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 7a13bf26f7..a176543e55 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -20,8 +20,9 @@ #define MPU6500_WHO_AM_I_CONST (0x70) #define MPU9250_WHO_AM_I_CONST (0x71) #define MPU9255_WHO_AM_I_CONST (0x73) -#define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20601_WHO_AM_I_CONST (0xAC) #define ICM20602_WHO_AM_I_CONST (0x12) +#define ICM20608G_WHO_AM_I_CONST (0xAF) #define MPU6500_BIT_RESET (0x80) diff --git a/src/main/drivers/accgyro_spi_bmi160.c b/src/main/drivers/accgyro_spi_bmi160.c index 40738e4b76..854bead6cc 100644 --- a/src/main/drivers/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro_spi_bmi160.c @@ -37,24 +37,16 @@ #include "platform.h" -#include "common/axis.h" -#include "common/maths.h" - #include "system.h" #include "io.h" #include "exti.h" #include "nvic.h" #include "bus_spi.h" -#include "gyro_sync.h" - #include "sensor.h" #include "accgyro.h" #include "accgyro_spi_bmi160.h" -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "fc/runtime_config.h" #ifdef USE_ACCGYRO_BMI160 @@ -90,38 +82,36 @@ #define BMI160_REG_CONF_NVM_PROG_EN 0x02 ///* Global Variables */ -static volatile bool BMI160InitDone = false; -static volatile bool BMI160Detected = false; +static volatile bool BMI160InitDone = false; +static volatile bool BMI160Detected = false; static volatile bool bmi160DataReady = false; static volatile bool bmi160ExtiInitDone = false; //! Private functions -static int32_t BMI160_Config(); -static int32_t BMI160_do_foc(); -static uint8_t BMI160_ReadReg(uint8_t reg); -static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data); +static int32_t BMI160_Config(const busDevice_t *bus); +static int32_t BMI160_do_foc(const busDevice_t *bus); +static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg); +static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); -static IO_t bmi160CsPin = IO_NONE; -#define DISABLE_BMI160 IOHi(bmi160CsPin) -#define ENABLE_BMI160 IOLo(bmi160CsPin) +#define DISABLE_BMI160(spiCsnPin) IOHi(spiCsnPin) +#define ENABLE_BMI160(spiCsnPin) IOLo(spiCsnPin) -bool BMI160_Detect() +bool bmi160Detect(const busDevice_t *bus) { if (BMI160Detected) return true; - bmi160CsPin = IOGetByTag(IO_TAG(BMI160_CS_PIN)); - IOInit(bmi160CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(bmi160CsPin, SPI_IO_CS_CFG); + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR); /* Read this address to acticate SPI (see p. 84) */ - BMI160_ReadReg(0x7F); + BMI160_ReadReg(bus, 0x7F); delay(10); // Give SPI some time to start up /* Check the chip ID */ - if (BMI160_ReadReg(BMI160_REG_CHIPID) != 0xd1){ + if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){ return false; } @@ -134,13 +124,13 @@ bool BMI160_Detect() * @brief Initialize the BMI160 6-axis sensor. * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq */ -static void BMI160_Init() +static void BMI160_Init(const busDevice_t *bus) { if (BMI160InitDone || !BMI160Detected) return; /* Configure the BMI160 Sensor */ - if (BMI160_Config() != 0){ + if (BMI160_Config(bus) != 0){ return; } @@ -148,7 +138,7 @@ static void BMI160_Init() /* Perform fast offset compensation if requested */ if (do_foc) { - BMI160_do_foc(); + BMI160_do_foc(bus); } BMI160InitDone = true; @@ -158,69 +148,69 @@ static void BMI160_Init() /** * @brief Configure the sensor */ -static int32_t BMI160_Config() +static int32_t BMI160_Config(const busDevice_t *bus) { // Set normal power mode for gyro and accelerometer - if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){ return -1; } delay(100); // can take up to 80ms - if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){ return -2; } delay(5); // can take up to 3.8ms // Verify that normal power mode was entered - uint8_t pmu_status = BMI160_ReadReg(BMI160_REG_PMU_STAT); + uint8_t pmu_status = BMI160_ReadReg(bus, BMI160_REG_PMU_STAT); if ((pmu_status & 0x3C) != 0x14){ return -3; } // Set odr and ranges // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used - if (BMI160_WriteReg(BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){ return -3; } delay(1); // Set gyr_bwp = 0b010 so only the first filter stage is used - if (BMI160_WriteReg(BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){ return -4; } delay(1); - if (BMI160_WriteReg(BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){ return -5; } delay(1); - if (BMI160_WriteReg(BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){ return -6; } delay(1); // Enable offset compensation - uint8_t val = BMI160_ReadReg(BMI160_REG_OFFSET_0); - if (BMI160_WriteReg(BMI160_REG_OFFSET_0, val | 0xC0) != 0){ + uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0); + if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){ return -7; } // Enable data ready interrupt - if (BMI160_WriteReg(BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){ return -8; } delay(1); // Enable INT1 pin - if (BMI160_WriteReg(BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){ return -9; } delay(1); // Map data ready interrupt to INT1 pin - if (BMI160_WriteReg(BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){ + if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){ return -10; } delay(1); @@ -228,22 +218,22 @@ static int32_t BMI160_Config() return 0; } -static int32_t BMI160_do_foc() +static int32_t BMI160_do_foc(const busDevice_t *bus) { // assume sensor is mounted on top uint8_t val = 0x7D;; - if (BMI160_WriteReg(BMI160_REG_FOC_CONF, val) != 0) { + if (BMI160_WriteReg(bus, BMI160_REG_FOC_CONF, val) != 0) { return -1; } // Start FOC - if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) { + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) { return -2; } // Wait for FOC to complete for (int i=0; i<50; i++) { - val = BMI160_ReadReg(BMI160_REG_STATUS); + val = BMI160_ReadReg(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_FOC_RDY) { break; } @@ -254,18 +244,18 @@ static int32_t BMI160_do_foc() } // Program NVM - val = BMI160_ReadReg(BMI160_REG_CONF); - if (BMI160_WriteReg(BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { + val = BMI160_ReadReg(bus, BMI160_REG_CONF); + if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { return -4; } - if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) { + if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) { return -5; } // Wait for NVM programming to complete for (int i=0; i<50; i++) { - val = BMI160_ReadReg(BMI160_REG_STATUS); + val = BMI160_ReadReg(bus, BMI160_REG_STATUS); if (val & BMI160_REG_STATUS_NVM_RDY) { break; } @@ -283,20 +273,29 @@ static int32_t BMI160_do_foc() * @returns The register value * @param reg[in] Register address to be read */ -static uint8_t BMI160_ReadReg(uint8_t reg) +uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg) { uint8_t data; - ENABLE_BMI160; + ENABLE_BMI160(bus->spi.csnPin); spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response - DISABLE_BMI160; + DISABLE_BMI160(bus->spi.csnPin); return data; } +bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) +{ + ENABLE_BMI160(bus->spi.csnPin); + spiTransferByte(BMI160_SPI_INSTANCE, reg | 0x80); // read transaction + spiTransfer(BMI160_SPI_INSTANCE, data, NULL, length); + ENABLE_BMI160(bus->spi.csnPin); + + return true; +} /** * @brief Writes one byte to the BMI160 register @@ -304,18 +303,22 @@ static uint8_t BMI160_ReadReg(uint8_t reg) * \param[in] data Byte to write * @returns 0 when success */ -static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data) +static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data) { - ENABLE_BMI160; + ENABLE_BMI160(bus->spi.csnPin); spiTransferByte(BMI160_SPI_INSTANCE, 0x7f & reg); spiTransferByte(BMI160_SPI_INSTANCE, data); - DISABLE_BMI160; + DISABLE_BMI160(bus->spi.csnPin); return 0; } +bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + return BMI160_WriteReg(bus, reg, data); +} extiCallbackRec_t bmi160IntCallbackRec; @@ -362,9 +365,9 @@ bool bmi160AccRead(accDev_t *acc) uint8_t bmi160_rec_buf[BUFFER_SIZE]; uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - ENABLE_BMI160; + ENABLE_BMI160(acc->bus.spi.csnPin); spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160; + DISABLE_BMI160(acc->bus.spi.csnPin); acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]); acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]); @@ -390,9 +393,9 @@ bool bmi160GyroRead(gyroDev_t *gyro) uint8_t bmi160_rec_buf[BUFFER_SIZE]; uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; - ENABLE_BMI160; + ENABLE_BMI160(gyro->bus.spi.csnPin); spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response - DISABLE_BMI160; + DISABLE_BMI160(gyro->bus.spi.csnPin); gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]); gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]); @@ -416,13 +419,13 @@ bool checkBMI160DataReady(gyroDev_t* gyro) void bmi160SpiGyroInit(gyroDev_t *gyro) { - BMI160_Init(); + BMI160_Init(gyro->bus.spi.csnPin); bmi160IntExtiInit(gyro); } void bmi160SpiAccInit(accDev_t *acc) { - BMI160_Init(); + BMI160_Init(acc->bus.spi.csnPin); acc->acc_1G = 512 * 8; } @@ -430,7 +433,7 @@ void bmi160SpiAccInit(accDev_t *acc) bool bmi160SpiAccDetect(accDev_t *acc) { - if (!BMI160_Detect()) { + if (!bmi160Detect(acc->bus.spi.csnPin)) { return false; } @@ -443,7 +446,7 @@ bool bmi160SpiAccDetect(accDev_t *acc) bool bmi160SpiGyroDetect(gyroDev_t *gyro) { - if (!BMI160_Detect()) { + if (!bmi160Detect(gyro->bus.spi.csnPin)) { return false; } diff --git a/src/main/drivers/accgyro_spi_bmi160.h b/src/main/drivers/accgyro_spi_bmi160.h index 0ce0c1d970..79dd7e9199 100644 --- a/src/main/drivers/accgyro_spi_bmi160.h +++ b/src/main/drivers/accgyro_spi_bmi160.h @@ -34,6 +34,8 @@ #pragma once +#include "sensor.h" + enum pios_bmi160_orientation { // clockwise rotation from board forward PIOS_BMI160_TOP_0DEG, PIOS_BMI160_TOP_90DEG, @@ -66,5 +68,9 @@ enum bmi160_gyro_range { BMI160_RANGE_2000DPS = 0x00, }; +bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); + +bool bmi160Detect(const busDevice_t *bus); bool bmi160SpiAccDetect(accDev_t *acc); bool bmi160SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 8d0af213df..d265d02d94 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -36,32 +36,30 @@ #include "accgyro_mpu.h" #include "accgyro_spi_icm20689.h" -#define DISABLE_ICM20689 IOHi(icmSpi20689CsPin) -#define ENABLE_ICM20689 IOLo(icmSpi20689CsPin) +#define DISABLE_ICM20689(spiCsnPin) IOHi(spiCsnPin) +#define ENABLE_ICM20689(spiCsnPin) IOLo(spiCsnPin) -static IO_t icmSpi20689CsPin = IO_NONE; - -bool icm20689WriteRegister(uint8_t reg, uint8_t data) +bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { - ENABLE_ICM20689; + ENABLE_ICM20689(bus->spi.csnPin); spiTransferByte(ICM20689_SPI_INSTANCE, reg); spiTransferByte(ICM20689_SPI_INSTANCE, data); - DISABLE_ICM20689; + DISABLE_ICM20689(bus->spi.csnPin); return true; } -bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_ICM20689; + ENABLE_ICM20689(bus->spi.csnPin); spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length); - DISABLE_ICM20689; + DISABLE_ICM20689(bus->spi.csnPin); return true; } -static void icm20689SpiInit(void) +static void icm20689SpiInit(const busDevice_t *bus) { static bool hardwareInitialised = false; @@ -69,30 +67,29 @@ static void icm20689SpiInit(void) return; } - icmSpi20689CsPin = IOGetByTag(IO_TAG(ICM20689_CS_PIN)); - IOInit(icmSpi20689CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG); + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); hardwareInitialised = true; } -bool icm20689SpiDetect(void) +bool icm20689SpiDetect(const busDevice_t *bus) { uint8_t tmp; uint8_t attemptsRemaining = 20; - icm20689SpiInit(); + icm20689SpiInit(bus); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed - icm20689WriteRegister(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + icm20689SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); do { delay(150); - icm20689ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); + icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); if (tmp == ICM20689_WHO_AM_I_CONST) { break; } @@ -130,32 +127,32 @@ void icm20689GyroInit(gyroDev_t *gyro) spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); -// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); +// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0); // delay(100); - gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration -// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN - gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN +// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/accgyro_spi_icm20689.h b/src/main/drivers/accgyro_spi_icm20689.h index bb9451ef4e..5325439328 100644 --- a/src/main/drivers/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro_spi_icm20689.h @@ -16,6 +16,8 @@ */ #pragma once +#include "sensor.h" + #define ICM20689_WHO_AM_I_CONST (0x98) #define ICM20689_BIT_RESET (0x80) @@ -25,10 +27,10 @@ bool icm20689GyroDetect(gyroDev_t *gyro); void icm20689AccInit(accDev_t *acc); void icm20689GyroInit(gyroDev_t *gyro); -bool icm20689SpiDetect(void); +bool icm20689SpiDetect(const busDevice_t *bus); bool icm20689SpiAccDetect(accDev_t *acc); bool icm20689SpiGyroDetect(gyroDev_t *gyro); -bool icm20689WriteRegister(uint8_t reg, uint8_t data); -bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); +bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 20fa4df8db..b44fa4968b 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -99,27 +99,25 @@ static bool mpuSpi6000InitDone = false; #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A -#define DISABLE_MPU6000 IOHi(mpuSpi6000CsPin) -#define ENABLE_MPU6000 IOLo(mpuSpi6000CsPin) +#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin) +#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin) -static IO_t mpuSpi6000CsPin = IO_NONE; - -bool mpu6000WriteRegister(uint8_t reg, uint8_t data) +bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { - ENABLE_MPU6000; + ENABLE_MPU6000(bus->spi.csnPin); spiTransferByte(MPU6000_SPI_INSTANCE, reg); spiTransferByte(MPU6000_SPI_INSTANCE, data); - DISABLE_MPU6000; + DISABLE_MPU6000(bus->spi.csnPin); return true; } -bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU6000; + ENABLE_MPU6000(bus->spi.csnPin); spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU6000; + DISABLE_MPU6000(bus->spi.csnPin); return true; } @@ -133,7 +131,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro) spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting - mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf); + mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf); delayMicroseconds(1); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock @@ -150,25 +148,22 @@ void mpu6000SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 4; } -bool mpu6000SpiDetect(void) +bool mpu6000SpiDetect(const busDevice_t *bus) { uint8_t in; uint8_t attemptsRemaining = 5; -#ifdef MPU6000_CS_PIN - mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN)); -#endif - IOInit(mpuSpi6000CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG); + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); + mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); do { delay(150); - mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in); + mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); if (in == MPU6000_WHO_AM_I_CONST) { break; } @@ -177,7 +172,7 @@ bool mpu6000SpiDetect(void) } } while (attemptsRemaining--); - mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in); + mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in); /* look for a product ID we recognise */ @@ -210,41 +205,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); // Device Reset - mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET); delay(150); - mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); delay(150); // Clock Source PPL with Z axis gyro reference - mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); delayMicroseconds(15); // Disable Primary I2C Interface - mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); delayMicroseconds(15); - mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00); delayMicroseconds(15); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale - mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); delayMicroseconds(15); // Accel +/- 8 G Full Scale - mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); delayMicroseconds(15); - mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR delayMicroseconds(15); #ifdef USE_MPU_DATA_READY_SIGNAL - mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); delayMicroseconds(15); #endif diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index ccd22438d8..9536d6ee3a 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -1,6 +1,8 @@ #pragma once +#include "sensor.h" + #define MPU6000_CONFIG 0x1A #define BITS_DLPF_CFG_256HZ 0x00 @@ -15,10 +17,10 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -bool mpu6000SpiDetect(void); +bool mpu6000SpiDetect(const busDevice_t *bus); bool mpu6000SpiAccDetect(accDev_t *acc); bool mpu6000SpiGyroDetect(gyroDev_t *gyro); -bool mpu6000WriteRegister(uint8_t reg, uint8_t data); -bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); +bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4cd98d824c..e1e82e793a 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -34,32 +34,32 @@ #include "accgyro_mpu6500.h" #include "accgyro_spi_mpu6500.h" -#define DISABLE_MPU6500 IOHi(mpuSpi6500CsPin) -#define ENABLE_MPU6500 IOLo(mpuSpi6500CsPin) +#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin) +#define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin) -static IO_t mpuSpi6500CsPin = IO_NONE; +#define BIT_SLEEP 0x40 -bool mpu6500WriteRegister(uint8_t reg, uint8_t data) +bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { - ENABLE_MPU6500; + ENABLE_MPU6500(bus->spi.csnPin); spiTransferByte(MPU6500_SPI_INSTANCE, reg); spiTransferByte(MPU6500_SPI_INSTANCE, data); - DISABLE_MPU6500; + DISABLE_MPU6500(bus->spi.csnPin); return true; } -bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU6500; + ENABLE_MPU6500(bus->spi.csnPin); spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU6500; + DISABLE_MPU6500(bus->spi.csnPin); return true; } -static void mpu6500SpiInit(void) +static void mpu6500SpiInit(const busDevice_t *bus) { static bool hardwareInitialised = false; @@ -67,24 +67,22 @@ static void mpu6500SpiInit(void) return; } - mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN)); - IOInit(mpuSpi6500CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG); + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); hardwareInitialised = true; } -static uint8_t mpuDetected = MPU_NONE; -uint8_t mpu6500SpiDetect(void) +uint8_t mpu6500SpiDetect(const busDevice_t *bus) { + mpu6500SpiInit(bus); + uint8_t tmp; + mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp); - mpu6500SpiInit(); - - mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); - + uint8_t mpuDetected = MPU_NONE; switch (tmp) { case MPU6500_WHO_AM_I_CONST: mpuDetected = MPU_65xx_SPI; @@ -93,12 +91,15 @@ uint8_t mpu6500SpiDetect(void) case MPU9255_WHO_AM_I_CONST: mpuDetected = MPU_9250_SPI; break; - case ICM20608G_WHO_AM_I_CONST: - mpuDetected = ICM_20608_SPI; + case ICM20601_WHO_AM_I_CONST: + mpuDetected = ICM_20601_SPI; break; case ICM20602_WHO_AM_I_CONST: mpuDetected = ICM_20602_SPI; break; + case ICM20608G_WHO_AM_I_CONST: + mpuDetected = ICM_20608_SPI; + break; default: mpuDetected = MPU_NONE; } @@ -118,7 +119,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro) mpu6500GyroInit(gyro); // Disable Primary I2C Interface - mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); @@ -127,7 +128,14 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro) bool mpu6500SpiAccDetect(accDev_t *acc) { - if (acc->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) { + // MPU6500 is used as a equivalent of other accelerometers by some flight controllers + switch (acc->mpuDetectionResult.sensor) { + case MPU_65xx_SPI: + case MPU_9250_SPI: + case ICM_20608_SPI: + case ICM_20602_SPI: + break; + default: return false; } @@ -139,7 +147,14 @@ bool mpu6500SpiAccDetect(accDev_t *acc) bool mpu6500SpiGyroDetect(gyroDev_t *gyro) { - if (gyro->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) { + // MPU6500 is used as a equivalent of other gyros by some flight controllers + switch (gyro->mpuDetectionResult.sensor) { + case MPU_65xx_SPI: + case MPU_9250_SPI: + case ICM_20608_SPI: + case ICM_20602_SPI: + break; + default: return false; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 25335743f7..01d6b29c13 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -17,13 +17,15 @@ #pragma once -uint8_t mpu6500SpiDetect(void); +#include "sensor.h" -void mpu6500SpiAccInit(accDev_t *acc); -void mpu6500SpiGyroInit(gyroDev_t *gyro); +uint8_t mpu6500SpiDetect(const busDevice_t *bus); bool mpu6500SpiAccDetect(accDev_t *acc); bool mpu6500SpiGyroDetect(gyroDev_t *gyro); -bool mpu6500WriteRegister(uint8_t reg, uint8_t data); -bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); +bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); + +void mpu6500SpiGyroInit(gyroDev_t *gyro); +void mpu6500SpiAccInit(accDev_t *acc); diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 7e71d71541..0859776944 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -50,47 +50,48 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); static bool mpuSpi9250InitDone = false; -static IO_t mpuSpi9250CsPin = IO_NONE; +#define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin) +#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin) -#define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin) -#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin) - -void mpu9250ResetGyro(void) +void mpu9250SpiResetGyro(void) { // Device Reset - mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); +#ifdef MPU9250_CS_PIN + busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } }; + mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(150); +#endif } -bool mpu9250WriteRegister(uint8_t reg, uint8_t data) +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { - ENABLE_MPU9250; + ENABLE_MPU9250(bus->spi.csnPin); delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg); spiTransferByte(MPU9250_SPI_INSTANCE, data); - DISABLE_MPU9250; + DISABLE_MPU9250(bus->spi.csnPin); delayMicroseconds(1); return true; } -bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250(bus->spi.csnPin); spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU9250; + DISABLE_MPU9250(bus->spi.csnPin); return true; } -bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data) { - ENABLE_MPU9250; + ENABLE_MPU9250(bus->spi.csnPin); delayMicroseconds(1); spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); - DISABLE_MPU9250; + DISABLE_MPU9250(bus->spi.csnPin); delayMicroseconds(1); return true; @@ -119,21 +120,21 @@ void mpu9250SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 8; } -bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) +bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { uint8_t in; uint8_t attemptsRemaining = 20; - mpu9250WriteRegister(reg, data); + mpu9250SpiWriteRegister(bus, reg, data); delayMicroseconds(100); do { - mpu9250SlowReadRegister(reg, 1, &in); + mpu9250SpiSlowReadRegister(bus, reg, 1, &in); if (in == data) { return true; } else { debug[3]++; - mpu9250WriteRegister(reg, data); + mpu9250SpiWriteRegister(bus, reg, data); delayMicroseconds(100); } } while (attemptsRemaining--); @@ -148,30 +149,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers - mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); delay(50); - verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); //Fchoice_b defaults to 00 which makes fchoice 11 const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData); + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData); if (gyro->lpf == 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF } else if (gyro->lpf < 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF } else if (gyro->lpf > 4) { - verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); - verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #if defined(USE_MPU_DATA_READY_SIGNAL) - verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. + verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); @@ -179,25 +180,21 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { mpuSpi9250InitDone = true; //init done } -bool mpu9250SpiDetect(void) +bool mpu9250SpiDetect(const busDevice_t *bus) { uint8_t in; uint8_t attemptsRemaining = 20; - /* not the best place for this - should really have an init method */ -#ifdef MPU9250_CS_PIN - mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)); -#endif - IOInit(mpuSpi9250CsPin, OWNER_MPU_CS, 0); - IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG); + IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed - mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); + mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); do { delay(150); - mpu9250ReadRegister(MPU_RA_WHO_AM_I, 1, &in); + mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in); if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) { break; } diff --git a/src/main/drivers/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro_spi_mpu9250.h index 0722b0128d..ab94308adc 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro_spi_mpu9250.h @@ -1,6 +1,8 @@ #pragma once +#include "sensor.h" + #define mpu9250_CONFIG 0x1A /* We should probably use these. :) @@ -24,14 +26,14 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -void mpu9250ResetGyro(void); +void mpu9250SpiResetGyro(void); -bool mpu9250SpiDetect(void); +bool mpu9250SpiDetect(const busDevice_t *bus); bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiGyroDetect(gyroDev_t *gyro); -bool mpu9250WriteRegister(uint8_t reg, uint8_t data); -bool verifympu9250WriteRegister(uint8_t reg, uint8_t data); -bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); -bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data); +bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); +bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h index f1bfbd9d3b..c717365bfc 100644 --- a/src/main/drivers/compass.h +++ b/src/main/drivers/compass.h @@ -22,6 +22,7 @@ typedef struct magDev_s { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function + busDevice_t bus; sensor_align_e magAlign; } magDev_t; diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 53f72e202c..9ba41d7668 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -86,9 +86,9 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f }; // Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same. #if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE) #define MPU9250_SPI_INSTANCE -#define verifympu9250WriteRegister mpu6500WriteRegister -#define mpu9250WriteRegister mpu6500WriteRegister -#define mpu9250ReadRegister mpu6500ReadRegister +#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegister +#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister +#define mpu9250SpiReadRegister mpu6500SpiReadRegister #endif #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) @@ -109,22 +109,22 @@ static queuedReadState_t queuedRead = { false, 0, 0}; static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) { - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes delay(10); __disable_irq(); - mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C + mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C __enable_irq(); return true; } static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) { - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte return true; } @@ -136,9 +136,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_) queuedRead.len = len_; - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register - verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register + mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes queuedRead.readStartedAt = micros(); queuedRead.waiting = true; @@ -173,7 +173,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf) queuedRead.waiting = false; - mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer + mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer return true; } #else @@ -316,13 +316,13 @@ bool ak8963Detect(magDev_t *mag) #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) // initialze I2C master via SPI bus (MPU9250) - verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR + verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR delay(10); - verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz + verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz delay(10); - verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only + verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only delay(10); #endif diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index e39e401b21..288ac3bd93 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -20,6 +20,8 @@ #include #include +#include "io_types.h" + typedef enum { ALIGN_DEFAULT = 0, // driver-provided alignment CW0_DEG = 1, @@ -32,6 +34,12 @@ typedef enum { CW270_DEG_FLIP = 8 } sensor_align_e; +typedef union busDevice_t { + struct deviceSpi_s { + IO_t csnPin; + } spi; +} busDevice_t; + typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorInterruptFuncPtr)(void); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 4c28adaf39..da235fcd10 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -146,7 +146,7 @@ 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", "OSD", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", + "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", NULL }; @@ -170,9 +170,11 @@ static const char * const lookupTableAccHardware[] = { "MPU6000", "MPU6500", "MPU9250", - "ICM20689", + "ICM20601", "ICM20602", - "BMI160", + "ICM20608", + "ICM20689", + "BMI160", "FAKE" }; @@ -207,8 +209,8 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) static const char * const sensorHardwareNames[4][16] = { - { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20689", "ICM20608G", "ICM20602", "BMI160", "FAKE", NULL }, - { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "ICM20689", "MPU9250", "ICM20608G", "ICM20602", "BMI160", "FAKE", NULL }, + { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE", NULL }, + { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, { "", "None", "HMC5883", "AK8975", "AK8963", NULL } }; @@ -321,7 +323,9 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "ANGLERATE", "ESC_SENSOR", "SCHEDULER", - "STACK" + "STACK", + "ESC_SENSOR_RPM", + "ESC_SENSOR_TMP" }; #ifdef OSD @@ -489,7 +493,6 @@ typedef union { cliMinMaxConfig_t minmax; } cliValueConfig_t; -#ifdef USE_PARAMETER_GROUPS typedef struct { const char *name; const uint8_t type; // see cliValueFlag_e @@ -519,6 +522,9 @@ static const clivalue_t valueTable[] = { { "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_isr_update) }, #endif #endif +#ifdef USE_DUAL_GYRO + { "gyro_to_use", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, +#endif // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, @@ -650,7 +656,7 @@ static const clivalue_t valueTable[] = { #endif // PG_MIXER_CONFIG - { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motor_direction) }, + { "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) }, // PG_MOTOR_3D_CONFIG { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, // FIXME upper limit should match code in the mixer, 1500 currently @@ -716,14 +722,14 @@ static const clivalue_t valueTable[] = { #endif // PG_AIRPLANE_CONFIG - { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_dir) }, + { "fixedwing_althold_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_reversed) }, // PG_RC_CONTROLS_CONFIG { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) }, { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_fast_change) }, { "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, - { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_direction) }, + { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, // PG_PID_CONFIG { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, @@ -833,6 +839,8 @@ static const clivalue_t valueTable[] = { { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAH_DRAWN]) }, { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_CRAFT_NAME]) }, { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SPEED]) }, + { "osd_gps_lon", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_LON]) }, + { "osd_gps_lat", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_LAT]) }, { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SATS]) }, { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ALTITUDE]) }, { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_PIDS]) }, @@ -923,6 +931,7 @@ static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS]; static mixerConfig_t mixerConfigCopy; static flight3DConfig_t flight3DConfigCopy; static serialConfig_t serialConfigCopy; +static serialPinConfig_t serialPinConfigCopy; static imuConfig_t imuConfigCopy; static armingConfig_t armingConfigCopy; static rcControlsConfig_t rcControlsConfigCopy; @@ -965,7 +974,6 @@ displayPortProfile_t displayPortProfileMax7456Copy; static pidConfig_t pidConfigCopy; static controlRateConfig_t controlRateProfilesCopy[CONTROL_RATE_PROFILE_COUNT]; static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; -#endif // USE_PARAMETER_GROUPS static void cliPrint(const char *str) { @@ -1238,6 +1246,10 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.currentConfig = &serialConfigCopy; ret.defaultConfig = serialConfig(); break; + case PG_SERIAL_PIN_CONFIG: + ret.currentConfig = &serialPinConfigCopy; + ret.defaultConfig = serialPinConfig(); + break; case PG_IMU_CONFIG: ret.currentConfig = &imuConfigCopy; ret.defaultConfig = imuConfig(); @@ -3902,13 +3914,13 @@ const cliResourceValue_t resourceTable[] = { #endif #ifdef SONAR { OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 }, - { OWNER_SONAR_ECHO, PG_SERIAL_CONFIG, offsetof(sonarConfig_t, echoTag), 0 }, + { OWNER_SONAR_ECHO, PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag), 0 }, #endif #ifdef LED_STRIP { OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ioTag), 0 }, #endif - { OWNER_SERIAL_TX, PG_SERIAL_CONFIG, offsetof(serialPinConfig_t, ioTagTx[0]), SERIAL_PORT_MAX_INDEX }, - { OWNER_SERIAL_RX, PG_SERIAL_CONFIG, offsetof(serialPinConfig_t, ioTagRx[0]), SERIAL_PORT_MAX_INDEX }, + { OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagTx[0]), SERIAL_PORT_MAX_INDEX }, + { OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagRx[0]), SERIAL_PORT_MAX_INDEX }, }; static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) @@ -4022,7 +4034,7 @@ static void cliResource(char *cmdline) cliPrintf("%c%02d: %s ", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); if (ioRecs[i].index > 0) { cliPrintf("%d", ioRecs[i].index); - } + } cliPrintf("\r\n"); } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 7b8f089a40..d55fafefda 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -559,7 +559,7 @@ void createDefaultConfig(master_t *config) config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; #endif - config->rcControlsConfig.yaw_control_direction = 1; + config->rcControlsConfig.yaw_control_reversed = false; // xxx_hardware: 0:default/autodetect, 1: disable config->compassConfig.mag_hardware = 1; @@ -598,7 +598,6 @@ void createDefaultConfig(master_t *config) #ifndef USE_PARAMETER_GROUPS #ifdef USE_SDCARD - intFeatureSet(FEATURE_SDCARD, featuresPtr); resetsdcardConfig(&config->sdcardConfig); #endif @@ -642,7 +641,7 @@ void createDefaultConfig(master_t *config) config->armingConfig.disarm_kill_switch = 1; config->armingConfig.auto_disarm_delay = 5; - config->airplaneConfig.fixedwing_althold_dir = 1; + config->airplaneConfig.fixedwing_althold_reversed = false; // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); @@ -754,10 +753,8 @@ void createDefaultConfig(master_t *config) #ifndef USE_PARAMETER_GROUPS #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) - intFeatureSet(FEATURE_BLACKBOX, featuresPtr); config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) - intFeatureSet(FEATURE_BLACKBOX, featuresPtr); config->blackboxConfig.device = BLACKBOX_DEVICE_SDCARD; #else config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; @@ -785,9 +782,6 @@ void createDefaultConfig(master_t *config) resetStatusLedConfig(&config->statusLedConfig); #endif -#if defined(TARGET_CONFIG) - targetConfiguration(config); -#endif } #endif @@ -797,6 +791,11 @@ void resetConfigs(void) createDefaultConfig(&masterConfig); #endif pgResetAll(MAX_PROFILE_COUNT); + +#if defined(TARGET_CONFIG) + targetConfiguration(); +#endif + pgActivateProfile(0); setPidProfile(0); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ea7d3b5815..e53ad91092 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -52,11 +52,11 @@ typedef enum { FEATURE_LED_STRIP = 1 << 16, FEATURE_DASHBOARD = 1 << 17, FEATURE_OSD = 1 << 18, - FEATURE_BLACKBOX = 1 << 19, + FEATURE_BLACKBOX_UNUSED = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, - FEATURE_SDCARD = 1 << 23, + FEATURE_SDCARD_UNUSED = 1 << 23, FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b7765226e0..57b5c30f12 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -171,7 +171,7 @@ void mwDisarm(void) DISABLE_ARMING_FLAG(ARMED); #ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { + if (blackboxConfig()->device) { finishBlackbox(); } #endif @@ -273,7 +273,7 @@ void updateMagHold(void) dif += 360; if (dif >= +180) dif -= 360; - dif *= -rcControlsConfig()->yaw_control_direction; + dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); if (STATE(SMALL_ANGLE)) rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30; // 18 deg } else @@ -545,7 +545,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) #endif #ifdef BLACKBOX - if (!cliMode && feature(FEATURE_BLACKBOX)) { + if (!cliMode && blackboxConfig()->device) { handleBlackbox(currentTimeUs); } #else diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 334152bf1d..af9fdcc33a 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -427,10 +427,6 @@ void init(void) mspFcInit(); mspSerialInit(); -#if defined(USE_MSP_DISPLAYPORT) && defined(CMS) - cmsDisplayPortRegister(displayPortMspInit()); -#endif - #ifdef USE_CLI cliInit(serialConfig()); #endif @@ -439,19 +435,30 @@ void init(void) rxInit(); + displayPort_t *osdDisplayPort = NULL; + #ifdef OSD //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets + if (feature(FEATURE_OSD)) { #if defined(USE_MAX7456) // if there is a max7456 chip for the OSD then use it, otherwise use MSP - displayPort_t *osdDisplayPort = max7456DisplayPortInit(vcdProfile()); + osdDisplayPort = max7456DisplayPortInit(vcdProfile()); #elif defined(USE_MSP_DISPLAYPORT) - displayPort_t *osdDisplayPort = displayPortMspInit(); + osdDisplayPort = displayPortMspInit(); #endif osdInit(osdDisplayPort); } #endif +#if defined(USE_MSP_DISPLAYPORT) && defined(CMS) + // If BFOSD is active, then register it as CMS device, else register MSP. + if (osdDisplayPort) + cmsDisplayPortRegister(osdDisplayPort); + else + cmsDisplayPortRegister(displayPortMspInit()); +#endif + #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit(); @@ -492,16 +499,14 @@ void init(void) #endif #ifdef USE_FLASHFS - if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) { #if defined(USE_FLASH_M25P16) - m25p16_init(flashConfig()); + m25p16_init(flashConfig()); #endif - flashfsInit(); - } + flashfsInit(); #endif #ifdef USE_SDCARD - if (feature(FEATURE_SDCARD) && blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { + if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { sdcardInsertionDetectInit(); sdcard_init(sdcardConfig()->useDma); afatfs_init(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 105c2a2767..8b06b3989d 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -371,12 +371,10 @@ void initActiveBoxIds(void) #endif #ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; #ifdef USE_FLASHFS - activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE; + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE; #endif - } #endif activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 8b80032971..04647546de 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -284,7 +284,7 @@ void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; + rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); } if (rcData[axis] < rxConfig()->midrc) { rcCommand[axis] = -rcCommand[axis]; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index f553c11a80..521a57c662 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -60,7 +60,7 @@ static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFu UNUSED(adjustmentFunction); UNUSED(newValue); #else - if (feature(FEATURE_BLACKBOX)) { + if (blackboxConfig()->device) { flightLogEvent_inflightAdjustment_t eventData; eventData.adjustmentFunction = adjustmentFunction; eventData.newValue = newValue; @@ -77,7 +77,7 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm UNUSED(adjustmentFunction); UNUSED(newFloatValue); #else - if (feature(FEATURE_BLACKBOX)) { + if (blackboxConfig()->device) { flightLogEvent_inflightAdjustment_t eventData; eventData.adjustmentFunction = adjustmentFunction; eventData.newFloatValue = newFloatValue; @@ -100,117 +100,100 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU { .adjustmentFunction = ADJUSTMENT_RC_RATE, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_RC_EXPO, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_YAW_RATE, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_YAW_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_YAW_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_YAW_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, .mode = ADJUSTMENT_MODE_SELECT, - .data = { .selectConfig = { .switchPositions = 3 }} - }, - { + .data = { .switchPositions = 3 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_RATE, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_ROLL_RATE, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_PITCH_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_ROLL_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_ROLL_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_D_SETPOINT, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { + .data = { .step = 1 } + }, { .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} + .data = { .step = 1 } + }, { + .adjustmentFunction = ADJUSTMENT_HORIZON_STRENGTH, + .mode = ADJUSTMENT_MODE_SELECT, + .data = { .switchPositions = 255 } + } }; @@ -235,146 +218,153 @@ static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, co static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { + + beeperConfirmationBeeps(delta > 0 ? 2 : 1); int newValue; - - if (delta > 0) { - beeperConfirmationBeeps(2); - } else { - beeperConfirmationBeeps(1); - } switch(adjustmentFunction) { - case ADJUSTMENT_RC_RATE: - newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); + case ADJUSTMENT_RC_RATE: + newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); break; - case ADJUSTMENT_RC_EXPO: - newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); + case ADJUSTMENT_RC_EXPO: + newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcExpo8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); break; - case ADJUSTMENT_THROTTLE_EXPO: - newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->thrExpo8 = newValue; - generateThrottleCurve(); - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); + case ADJUSTMENT_THROTTLE_EXPO: + newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->thrExpo8 = newValue; + generateThrottleCurve(); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; - case ADJUSTMENT_PITCH_ROLL_RATE: - case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_PITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); - if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE - case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_ROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + case ADJUSTMENT_PITCH_ROLL_RATE: + case ADJUSTMENT_PITCH_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_PITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { break; - case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - controlRateConfig->rates[FD_YAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_P: - case ADJUSTMENT_PITCH_P: - newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE + case ADJUSTMENT_ROLL_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_ROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + break; + case ADJUSTMENT_YAW_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + controlRateConfig->rates[FD_YAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_P: + case ADJUSTMENT_PITCH_P: + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); - if (adjustmentFunction == ADJUSTMENT_PITCH_P) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_P - case ADJUSTMENT_ROLL_P: - newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_P) { break; - case ADJUSTMENT_PITCH_ROLL_I: - case ADJUSTMENT_PITCH_I: - newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_I) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_I - case ADJUSTMENT_ROLL_I: - newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_P + case ADJUSTMENT_ROLL_P: + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_I: + case ADJUSTMENT_PITCH_I: + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_I) { break; - case ADJUSTMENT_PITCH_ROLL_D: - case ADJUSTMENT_PITCH_D: - newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_D) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_D - case ADJUSTMENT_ROLL_D: - newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); - break; - case ADJUSTMENT_YAW_P: - newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); - break; - case ADJUSTMENT_YAW_I: - newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); - break; - case ADJUSTMENT_YAW_D: - newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); - break; - case ADJUSTMENT_RC_RATE_YAW: - newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcYawRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); - break; - case ADJUSTMENT_D_SETPOINT: - newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c - pidProfile->dtermSetpointWeight = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); - break; - case ADJUSTMENT_D_SETPOINT_TRANSITION: - newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c - pidProfile->setpointRelaxRatio = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); - break; - default: + } + // fall though for combined ADJUSTMENT_PITCH_ROLL_I + case ADJUSTMENT_ROLL_I: + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_D: + case ADJUSTMENT_PITCH_D: + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { break; + } + // fall though for combined ADJUSTMENT_PITCH_ROLL_D + case ADJUSTMENT_ROLL_D: + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + break; + case ADJUSTMENT_YAW_P: + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); + break; + case ADJUSTMENT_YAW_I: + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); + break; + case ADJUSTMENT_YAW_D: + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); + break; + case ADJUSTMENT_RC_RATE_YAW: + newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcYawRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); + break; + case ADJUSTMENT_D_SETPOINT: + newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c + pidProfile->dtermSetpointWeight = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + break; + case ADJUSTMENT_D_SETPOINT_TRANSITION: + newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c + pidProfile->setpointRelaxRatio = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); + break; + default: + break; }; } static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) { - bool applied = false; + uint8_t beeps = 0; switch(adjustmentFunction) { - case ADJUSTMENT_RATE_PROFILE: + case ADJUSTMENT_RATE_PROFILE: + { if (getCurrentControlRateProfileIndex() != position) { changeControlRateProfile(position); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); - applied = true; + beeps = position + 1; } break; + } + case ADJUSTMENT_HORIZON_STRENGTH: + { + uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if(pidProfile->D8[PIDLEVEL] != newValue) { + beeps = ((newValue - pidProfile->D8[PIDLEVEL]) / 8) + 1; + pidProfile->D8[PIDLEVEL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position); + } + break; + } } - if (applied) { - beeperConfirmationBeeps(position + 1); + if (beeps) { + beeperConfirmationBeeps(beeps); } + } #define RESET_FREQUENCY_2HZ (1000 / 2) @@ -413,9 +403,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; if (rcData[channelIndex] > rxConfig()->midrc + 200) { - delta = adjustmentState->config->data.stepConfig.step; + delta = adjustmentState->config->data.step; } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { - delta = 0 - adjustmentState->config->data.stepConfig.step; + delta = -adjustmentState->config->data.step; } else { // returning the switch to the middle immediately resets the ready state MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); @@ -426,10 +416,10 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) continue; } - applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); + applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); pidInitConfig(pidProfile); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.switchPositions); const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; applySelectAdjustment(adjustmentFunction, position); } diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 6067109f8a..3d44a4629a 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -46,6 +46,7 @@ typedef enum { ADJUSTMENT_RC_RATE_YAW, ADJUSTMENT_D_SETPOINT, ADJUSTMENT_D_SETPOINT_TRANSITION, + ADJUSTMENT_HORIZON_STRENGTH, ADJUSTMENT_FUNCTION_COUNT } adjustmentFunction_e; @@ -55,17 +56,9 @@ typedef enum { ADJUSTMENT_MODE_SELECT } adjustmentMode_e; -typedef struct adjustmentStepConfig_s { - uint8_t step; -} adjustmentStepConfig_t; - -typedef struct adjustmentSelectConfig_s { - uint8_t switchPositions; -} adjustmentSelectConfig_t; - typedef union adjustmentConfig_u { - adjustmentStepConfig_t stepConfig; - adjustmentSelectConfig_t selectConfig; + uint8_t step; + uint8_t switchPositions; } adjustmentData_t; typedef struct adjustmentConfig_s { @@ -104,10 +97,6 @@ typedef struct adjustmentState_s { PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); -typedef struct adjustmentProfile_s { - adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; -} adjustmentProfile_t; - void resetAdjustmentStates(void); void updateAdjustmentStates(void); struct controlRateConfig_s; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0f08225de0..5d4ed0d090 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -77,7 +77,7 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .yaw_deadband = 0, .alt_hold_deadband = 40, .alt_hold_fast_change = 1, - .yaw_control_direction = 1, + .yaw_control_reversed = false, ); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 45713ed1b2..c96502dc1f 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -158,7 +158,7 @@ typedef struct rcControlsConfig_s { uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement - int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) + bool yaw_control_reversed; // invert control direction of yaw } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index c6bd68aa14..7797bc4986 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -47,7 +47,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, - .fixedwing_althold_dir = 1 + .fixedwing_althold_reversed = false ); int32_t setVelocity = 0; @@ -114,7 +114,7 @@ static void applyFixedWingAltHold(void) // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir; + rcCommand[PITCH] += altHoldThrottleAdjustment * GET_DIRECTION(airplaneConfig()->fixedwing_althold_reversed); } void applyAltHold(void) diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index f1275a50e0..14e48d2052 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -23,7 +23,7 @@ extern int32_t AltHold; extern int32_t vario; typedef struct airplaneConfig_s { - int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign + bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign } airplaneConfig_t; PG_DECLARE(airplaneConfig_t, airplaneConfig); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b5b5a28c14..aeefaa2307 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -67,7 +67,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #endif PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = TARGET_DEFAULT_MIXER, - .yaw_motor_direction = 1, + .yaw_motors_reversed = false, ); PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); @@ -530,7 +530,7 @@ void mixTable(pidProfile_t *pidProfile) motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction); + scaledAxisPIDf[YAW] * currentMixer[i].yaw * -GET_DIRECTION(mixerConfig()->yaw_motors_reversed); if (vbatCompensationFactor > 1.0f) { motorMix[i] *= vbatCompensationFactor; // Add voltage compensation diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1cfc0620e6..5f88ff3aeb 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -100,7 +100,7 @@ typedef struct mixer_s { typedef struct mixerConfig_s { uint8_t mixerMode; - int8_t yaw_motor_direction; + bool yaw_motors_reversed; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 302e9ec8b9..3700fdd749 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -273,13 +273,13 @@ static float calcHorizonLevelStrength(void) { static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination - float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis); + float angle = pidProfile->levelSensitivity * getRcDeflection(axis); #ifdef GPS - errorAngle += GPS_angle[axis]; + angle += GPS_angle[axis]; #endif - errorAngle = constrainf(errorAngle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); - errorAngle = errorAngle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); - if(FLIGHT_MODE(ANGLE_MODE)) { + angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); + const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); + if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed currentPidSetpoint = errorAngle * levelGain; } else { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 75beff6261..995fea8c3f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -39,8 +39,12 @@ #include "build/debug.h" #include "build/version.h" -#include "common/printf.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_osd.h" + #include "common/maths.h" +#include "common/printf.h" #include "common/typeconversion.h" #include "common/utils.h" @@ -59,10 +63,6 @@ #include "drivers/vtx_rtc6705.h" #endif -#include "cms/cms.h" -#include "cms/cms_types.h" -#include "cms/cms_menu_osd.h" - #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/gps.h" @@ -228,6 +228,30 @@ static void osdDrawSingleElement(uint8_t item) sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10); break; } + + case OSD_GPS_LAT: + case OSD_GPS_LON: + { + int32_t val; + if (item == OSD_GPS_LAT) { + buff[0] = 0xA6; + val = GPS_coord[LAT]; + } else { + buff[0] = 0xA7; + val = GPS_coord[LON]; + } + if (val >= 0) { + val += 1000000000; + } else { + val -= 1000000000; + } + itoa(val, &buff[1], 10); + buff[1] = buff[2]; + buff[2] = buff[3]; + buff[3] = '.'; + break; + } + #endif // GPS case OSD_ALTITUDE: @@ -503,6 +527,8 @@ void osdDrawElements(void) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); + osdDrawSingleElement(OSD_GPS_LAT); + osdDrawSingleElement(OSD_GPS_LON); } #endif // GPS } @@ -532,6 +558,9 @@ void pgResetFn_osdConfig(osdConfig_t *osdProfile) osdProfile->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(9, 10) | VISIBLE_FLAG; osdProfile->item_pos[OSD_AVG_CELL_VOLTAGE] = OSD_POS(12, 2) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_GPS_LAT] = OSD_POS(18, 14) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_GPS_LON] = OSD_POS(18, 15) | VISIBLE_FLAG; + osdProfile->units = OSD_UNIT_METRIC; osdProfile->rssi_alarm = 20; osdProfile->cap_alarm = 2200; @@ -752,7 +781,7 @@ static void osdShowStats(void) sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); displayWrite(osdDisplayPort, 22, top++, buff); - if (feature(FEATURE_BLACKBOX) && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { + if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { displayWrite(osdDisplayPort, 2, top, "BLACKBOX :"); osdGetBlackboxStatusString(buff, 10); displayWrite(osdDisplayPort, 22, top++, buff); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 60fd65963a..ee26fb898a 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -49,6 +49,8 @@ typedef enum { OSD_PIDRATE_PROFILE, OSD_MAIN_BATT_WARNING, OSD_AVG_CELL_VOLTAGE, + OSD_GPS_LON, + OSD_GPS_LAT, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 3381e59748..9539ae8c50 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -214,8 +214,9 @@ retry: #endif ; // fallthrough case ACC_MPU6500: - case ACC_ICM20608G: + case ACC_ICM20601: case ACC_ICM20602: + case ACC_ICM20608G: #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) @@ -230,12 +231,15 @@ retry: case MPU_9250_SPI: accHardware = ACC_MPU9250; break; - case ICM_20608_SPI: - accHardware = ACC_ICM20608G; + case ICM_20601_SPI: + accHardware = ACC_ICM20601; break; case ICM_20602_SPI: accHardware = ACC_ICM20602; break; + case ICM_20608_SPI: + accHardware = ACC_ICM20608G; + break; default: accHardware = ACC_MPU6500; } @@ -300,6 +304,7 @@ bool accInit(uint32_t gyroSamplingInverval) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings + acc.dev.bus = *gyroSensorBus(); acc.dev.mpuConfiguration = *gyroMpuConfiguration(); acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 8ea59f1e01..2560dfd96c 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -32,10 +32,11 @@ typedef enum { ACC_LSM303DLHC, ACC_MPU6000, ACC_MPU6500, - ACC_ICM20689, ACC_MPU9250, - ACC_ICM20608G, + ACC_ICM20601, ACC_ICM20602, + ACC_ICM20608G, + ACC_ICM20689, ACC_BMI160, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 18b2c68f61..07c5d1a60d 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -36,9 +36,10 @@ #include "fc/config.h" #include "fc/runtime_config.h" -#include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -141,9 +142,12 @@ bool compassInit(void) // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. + // copy over SPI bus settings for AK8963 compass + magDev.bus = *gyroSensorBus(); if (!compassDetect(&magDev, compassConfig()->mag_hardware)) { return false; } + const int16_t deg = compassConfig()->mag_declination / 100; const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index c4af56e62d..910c37b717 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -68,7 +68,7 @@ Byte 9: 8-bit CRC DEBUG INFORMATION ----------------- -set debug_mode = DEBUG_ESC_TELEMETRY in cli +set debug_mode = DEBUG_ESC_SENSOR in cli */ @@ -235,6 +235,9 @@ static uint8_t decodeEscFrame(void) combinedDataNeedsUpdate = true; frameStatus = ESC_SENSOR_FRAME_COMPLETE; + + DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, escSensorData[escSensorMotor].rpm); + DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature); } else { frameStatus = ESC_SENSOR_FRAME_FAILED; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ec64f608b5..ac374c7a14 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -87,7 +87,7 @@ static void *notchFilter2[3]; #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 4 @@ -96,19 +96,22 @@ static void *notchFilter2[3]; PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_lpf = GYRO_LPF_256HZ, + .gyro_align = ALIGN_DEFAULT, + .gyroMovementCalibrationThreshold = 48, .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, + .gyro_lpf = GYRO_LPF_256HZ, .gyro_soft_lpf_type = FILTER_PT1, .gyro_soft_lpf_hz = 90, + .gyro_isr_update = false, + .gyro_use_32khz = false, + .gyro_to_use = 0, .gyro_soft_notch_hz_1 = 400, .gyro_soft_notch_cutoff_1 = 300, .gyro_soft_notch_hz_2 = 200, - .gyro_soft_notch_cutoff_2 = 100, - .gyro_align = ALIGN_DEFAULT, - .gyroMovementCalibrationThreshold = 48 + .gyro_soft_notch_cutoff_2 = 100 ); -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -122,6 +125,11 @@ static const extiConfig_t *selectMPUIntExtiConfig(void) } #endif +const busDevice_t *gyroSensorBus(void) +{ + return &gyroDev0.bus; +} + const mpuConfiguration_t *gyroMpuConfiguration(void) { return &gyroDev0.mpuConfiguration; @@ -196,8 +204,9 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) case GYRO_MPU6500: - case GYRO_ICM20608G: + case GYRO_ICM20601: case GYRO_ICM20602: + case GYRO_ICM20608G: #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) { #else @@ -207,12 +216,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) case MPU_9250_SPI: gyroHardware = GYRO_MPU9250; break; - case ICM_20608_SPI: - gyroHardware = GYRO_ICM20608G; + case ICM_20601_SPI: + gyroHardware = GYRO_ICM20601; break; case ICM_20602_SPI: gyroHardware = GYRO_ICM20602; break; + case ICM_20608_SPI: + gyroHardware = GYRO_ICM20608G; + break; default: gyroHardware = GYRO_MPU6500; } @@ -281,10 +293,16 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) bool gyroInit(void) { memset(&gyro, 0, sizeof(gyro)); -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig(); +#ifdef USE_DUAL_GYRO + // set cnsPin using GYRO_n_CS_PIN defined in target.h + gyroDev0.bus.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); +#else + gyroDev0.bus.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h +#endif // USE_DUAL_GYRO mpuDetect(&gyroDev0); - mpuResetFn = gyroDev0.mpuConfiguration.resetFn; + mpuResetFn = gyroDev0.mpuConfiguration.resetFn; // must be set after mpuDetect #endif const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0); if (gyroHardware == GYRO_NONE) { @@ -294,9 +312,10 @@ bool gyroInit(void) switch (gyroHardware) { case GYRO_MPU6500: case GYRO_MPU9250: - case GYRO_ICM20689: - case GYRO_ICM20608G: + case GYRO_ICM20601: case GYRO_ICM20602: + case GYRO_ICM20608G: + case GYRO_ICM20689: // do nothing, as gyro supports 32kHz break; default: @@ -305,7 +324,7 @@ bool gyroInit(void) break; } - // Must set gyro sample rate before initialisation + // Must set gyro targetLooptime before gyroDev.init and initialisation of filters gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); gyroDev0.lpf = gyroConfig()->gyro_lpf; gyroDev0.init(&gyroDev0); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 98a9c585ef..ac46fe2324 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -19,6 +19,7 @@ #include "config/parameter_group.h" #include "common/axis.h" +#include "drivers/io_types.h" #include "drivers/sensor.h" typedef enum { @@ -31,9 +32,10 @@ typedef enum { GYRO_MPU6000, GYRO_MPU6500, GYRO_MPU9250, - GYRO_ICM20689, - GYRO_ICM20608G, + GYRO_ICM20601, GYRO_ICM20602, + GYRO_ICM20608G, + GYRO_ICM20689, GYRO_BMI160, GYRO_FAKE } gyroSensor_e; @@ -54,6 +56,7 @@ typedef struct gyroConfig_s { uint8_t gyro_soft_lpf_hz; bool gyro_isr_update; bool gyro_use_32khz; + uint8_t gyro_to_use; uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; @@ -65,6 +68,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); +const busDevice_t *gyroSensorBus(void); struct mpuConfiguration_s; const struct mpuConfiguration_s *gyroMpuConfiguration(void); struct mpuDetectionResult_s; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 54fc5ed8a8..21c14173f1 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -160,7 +160,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 7d840db126..634bb0610b 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -173,7 +173,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index da211bf964..c6299cae4b 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -142,7 +142,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 7fef249b0a..f4ab811472 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -145,7 +145,7 @@ #define TRANSPONDER -#define DEFAULT_FEATURES ( FEATURE_BLACKBOX | FEATURE_SDCARD | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_AIRMODE ) +#define DEFAULT_FEATURES ( FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_AIRMODE ) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 3ee8fa1b38..4ec9b06927 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -126,7 +126,7 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD) #define SPEKTRUM_BIND_PIN UART2_RX_PIN diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 008badd389..bb507f3c6f 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -45,7 +45,7 @@ void targetConfiguration(void) } if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - featureClear(FEATURE_SDCARD); + blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE; } if (hardwareRevision == BJF4_MINI_REV3A) { @@ -57,10 +57,8 @@ void targetValidateConfiguration(void) { /* make sure the SDCARD cannot be turned on */ if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) { - featureClear(FEATURE_SDCARD); - if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { - blackboxConfigMutable()->device = BLACKBOX_DEVICE_FLASH; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE; } } } diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index fb725f4c04..a807a6f538 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -148,7 +148,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 7d47898118..47f1a60ceb 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -125,7 +125,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 7887779d77..4fed5a79fb 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -141,7 +141,6 @@ #undef LED_STRIP -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index c7c6418a29..e4146ea2c9 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -127,7 +127,7 @@ #define RSSI_ADC_PIN PC1 // *************** FEATURES ************************ -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_BLACKBOX | FEATURE_VTX) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VTX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 161652c940..e408e25033 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -149,7 +149,6 @@ #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN PB0 -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 37934ebb86..28c2990160 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -166,7 +166,6 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 06df3b8622..83de9e286e 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -138,7 +138,6 @@ #undef LED_STRIP -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index aeaaa712b3..275642d368 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -89,7 +89,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h index f6d06a59fd..a72867e8b4 100644 --- a/src/main/target/ISHAPEDF3/target.h +++ b/src/main/target/ISHAPEDF3/target.h @@ -105,7 +105,6 @@ #define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index 9f582df20c..e47da1449c 100644 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -29,7 +29,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S2_OUT - DMA1_ST2 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S3_OUT - DMA1_ST6 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S4_OUT - DMA1_ST1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S5_OUT - DMA1_ST4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // S5_OUT - DMA1_ST2 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // S6_OUT - DMA2_ST4 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0), // LED_STRIP - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0), // LED_STRIP - DMA1_ST4 }; \ No newline at end of file diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index e676308781..059a369a81 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -26,6 +26,7 @@ #define BEEPER PC9 #define BEEPER_INVERTED +#define INVERTER_PIN_USART3 PB15 // ICM20689 interrupt #define USE_EXTI @@ -75,6 +76,7 @@ #define USE_VCP #define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -126,7 +128,7 @@ #define RSSI_ADC_PIN PC1 #define RSSI_ADC_CHANNEL ADC_Channel_11 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/KIWIF4/PLUMF4.mk b/src/main/target/KIWIF4/PLUMF4.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/KIWIF4/target.c b/src/main/target/KIWIF4/target.c index add381e338..896cbfd7d4 100644 --- a/src/main/target/KIWIF4/target.c +++ b/src/main/target/KIWIF4/target.c @@ -29,5 +29,9 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1, 0), DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1, 0), DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1, 0), +#ifdef PLUMF4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0), //LED +#else DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED +#endif }; diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 445749323f..628351d081 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -17,13 +17,26 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "KIWI" +#ifdef PLUMF4 +#define TARGET_BOARD_IDENTIFIER "PLUM" +#define USBD_PRODUCT_STRING "PLUMF4" +#else + +#define TARGET_BOARD_IDENTIFIER "KIWI" #define USBD_PRODUCT_STRING "KIWIF4" +#endif + +#ifdef PLUMF4 +#define LED0 PB4 + +#else + #define LED0 PB5 #define LED1 PB4 +#endif #define BEEPER PA8 @@ -47,6 +60,7 @@ #define USE_ACC_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW180_DEG +#ifdef KIWIF4 #define OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 @@ -54,7 +68,7 @@ //#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 //#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER - +#endif #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -92,11 +106,13 @@ #define USE_SPI_DEVICE_1 +#ifdef KIWIF4 #define USE_SPI_DEVICE_2 #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 +#endif #define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 @@ -119,7 +135,7 @@ #define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 3cdd44561a..c2ca8fed7a 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -152,9 +152,9 @@ #ifdef LUXV2_RACE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES ( FEATURE_BLACKBOX | FEATURE_TELEMETRY ) +#define DEFAULT_FEATURES (FEATURE_TELEMETRY) #else -#define DEFAULT_FEATURES FEATURE_TELEMETRY +#define DEFAULT_FEATURES (FEATURE_TELEMETRY) #endif #define SPEKTRUM_BIND_PIN UART1_RX_PIN diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 53b60d6091..2c5c754431 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -121,7 +121,6 @@ //#define USE_ESC_SENSOR #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index 49c4c5870f..2741fcc0b4 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -143,7 +143,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index acc3894b32..029b2e3687 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -164,7 +164,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) #define BUTTONS #define BUTTON_A_PIN PB1 diff --git a/src/main/target/OMNIBUSF4/VGOODDHF4.mk b/src/main/target/OMNIBUSF4/VGOODDHF4.mk new file mode 100644 index 0000000000..42349f2912 --- /dev/null +++ b/src/main/target/OMNIBUSF4/VGOODDHF4.mk @@ -0,0 +1 @@ +#VGOODDHF4.mk file \ No newline at end of file diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 1c6f2ab377..6f495b5f49 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -29,13 +29,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM #else #if defined(OMNIBUSF4SD) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM DEF_TIM(TIM4, CH4, PB9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN #else DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN #endif - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S4_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S4_IN, UART6_RX DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S5_IN DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S6_IN #endif // CL_RACINGF4 @@ -47,7 +48,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT #elif defined(OMNIBUSF4SD) DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S6_OUT + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #else DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index f083ace06b..735a66982f 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -19,26 +19,34 @@ #define TARGET_BOARD_IDENTIFIER "CLR4" #elif defined(OMNIBUSF4SD) #define TARGET_BOARD_IDENTIFIER "OBSD" +#elif defined(VGOODDHF4) +#define TARGET_BOARD_IDENTIFIER "DHF4" #else #define TARGET_BOARD_IDENTIFIER "OBF4" #endif #if defined(CL_RACINGF4) #define USBD_PRODUCT_STRING "CL_RACINGF4" +#elif defined(VGOODDHF4) +#define USBD_PRODUCT_STRING "VgooddhF4" #else #define USBD_PRODUCT_STRING "OmnibusF4" #endif #ifdef OPBL -#define USBD_SERIALNUMBER_STRING "0x8020000" +#define USBD_SERIALNUMBER_STRING "0x8020000" // Remove this at the next major release (?) #endif #define LED0 PB5 -//#define LED1 PB4 +//#define LED1 PB4 // Remove this at the next major release #define BEEPER PB4 #define BEEPER_INVERTED -#define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO +#ifdef OMNIBUSF4SD +#define INVERTER_PIN_UART6 PC8 // Omnibus F4 V3 and later +#else +#define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO XXX this is not used --- remove it at the next major release +#endif #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -69,8 +77,8 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW90_DEG -//#define USE_MAG_NAZA -//#define MAG_NAZA_ALIGN CW180_DEG_FLIP +//#define USE_MAG_NAZA // Delete this on next major release +//#define MAG_NAZA_ALIGN CW180_DEG_FLIP // Ditto #define BARO #define USE_BARO_MS5611 @@ -107,6 +115,12 @@ #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 +#elif defined(VGOODDHF4) +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #else #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define M25P16_CS_PIN SPI3_NSS_PIN @@ -150,7 +164,7 @@ #define USE_SPI #define USE_SPI_DEVICE_1 -#if defined(OMNIBUSF4SD) || defined(CL_RACINGF4) +#if defined(OMNIBUSF4SD) || defined(CL_RACINGF4) || defined(VGOODDHF4) #define USE_SPI_DEVICE_2 #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 @@ -181,9 +195,9 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define AVOID_UART1_FOR_PWM_PPM #if defined(CL_RACINGF4) -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD ) #else -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_OSD) #endif #define SPEKTRUM_BIND_PIN UART1_RX_PIN @@ -194,5 +208,14 @@ #define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) #define TARGET_IO_PORTD BIT(2) +#ifdef CL_RACINGF4 +#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS ( TIM_N(4) | TIM_N(8) ) +#else +#ifdef OMNIBUSF4SD +#define USABLE_TIMER_CHANNEL_COUNT 13 +#else #define USABLE_TIMER_CHANNEL_COUNT 12 +#endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#endif diff --git a/src/main/target/REVO/AIRBOTF4SD.mk b/src/main/target/REVO/AIRBOTF4SD.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 3b558a70cf..665cc54ffa 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -21,6 +21,10 @@ #define TARGET_BOARD_IDENTIFIER "AIR4" #define USBD_PRODUCT_STRING "AirbotF4" +#elif defined(AIRBOTF4SD) +#define TARGET_BOARD_IDENTIFIER "A4SD" +#define USBD_PRODUCT_STRING "AirbotF4SD" + #elif defined(REVOLT) #define TARGET_BOARD_IDENTIFIER "RVLT" #define USBD_PRODUCT_STRING "Revolt" @@ -53,7 +57,7 @@ #endif // Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper -#if defined(AIRBOTF4) +#if defined(AIRBOTF4) || defined(AIRBOTF4SD) #define BEEPER PB4 #define BEEPER_INVERTED #elif defined(REVOLT) @@ -68,7 +72,11 @@ #endif // PC0 used as inverter select GPIO +#ifdef AIRBOTF4SD +#define INVERTER_PIN_UART6 PD2 +#else #define INVERTER_PIN_UART1 PC0 +#endif #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -76,42 +84,53 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#if defined(SOULF4) +#define GYRO #define ACC -#define USE_ACC_SPI_MPU6000 + +#ifdef AIRBOTF4SD +#undef MPU6000_CS_PIN +#define MPU6000_CS_PIN PB13 +#define USE_GYRO_SPI_ICM20601 +#define ICM20601_CS_PIN PA4 // served through MPU6500 code +#define ICM20601_SPI_INSTANCE SPI1 +#define USE_DUAL_GYRO +#define GYRO_0_CS_PIN MPU6000_CS_PIN +#define GYRO_1_CS_PIN ICM20601_CS_PIN +#endif + +#if defined(SOULF4) +#define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG -#define GYRO -#define USE_GYRO_SPI_MPU6000 +#define USE_ACC_SPI_MPU6000 #define ACC_MPU6000_ALIGN CW180_DEG #elif defined(REVOLT) || defined(PODIUMF4) -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW0_DEG - #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW0_DEG -#else -#define ACC -#define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG - #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW0_DEG + +#else -#define GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW270_DEG +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG + +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + #endif // MPU6000 interrupts @@ -126,15 +145,39 @@ #define BARO #define USE_BARO_MS5611 - #endif +#if defined(AIRBOTF4SD) +// SDCARD support for AIRBOTF4SD +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define USE_SDCARD +#define USE_SDCARD_SPI3 + +#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PC0 +#define SDCARD_SPI_INSTANCE SPI3 +#define SDCARD_SPI_CS_PIN SPI3_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz + +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF5 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 + +#else + #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 - #define USE_FLASHFS #define USE_FLASH_M25P16 +#endif // AIRBOTF4SD + + #define USE_VCP #if defined(PODIUMF4) #define VBUS_SENSING_PIN PA8 @@ -186,13 +229,15 @@ #define VBAT_ADC_CHANNEL ADC_Channel_13 #endif +#if defined(AIRBOTF4SD) +#define RSSI_ADC_PIN PA0 +#endif + #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(PODIUMF4) #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 #define DEFAULT_FEATURES FEATURE_TELEMETRY -#else -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #endif #define SPEKTRUM_BIND_PIN UART3_RX_PIN @@ -207,12 +252,11 @@ #ifdef REVOLT #define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(12) ) -#else -#define USABLE_TIMER_CHANNEL_COUNT 12 -#ifdef AIRBOTF4 +#elif defined(AIRBOTF4) || defined(AIRBOTF4SD) +#define USABLE_TIMER_CHANNEL_COUNT 13 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(12) ) #else +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(12) ) -#endif // AIRBOTF4 -#endif // REVOLT +#endif diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 3d8b57a8a6..c1844c7f13 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -1,5 +1,9 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +ifeq ($(TARGET), AIRBOTF4SD) +FEATURES = VCP SDCARD +else +FEATURES = VCP ONBOARDFLASH +endif TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 8e942e45f4..bddc8e6bb5 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -89,7 +89,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 98e42f7674..f7f761ea43 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -150,7 +150,7 @@ #define SPEKTRUM_BIND_PIN UART3_RX_PIN #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) +#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 46a7be6dce..5b9f570310 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -121,7 +121,6 @@ #undef LED_STRIP -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index a564b9c5ef..332b06ec83 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -182,7 +182,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 15c1ced144..0ecff9cdca 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -149,7 +149,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 9572c421f4..ea3cd18c6d 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -176,7 +176,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #ifndef TINYBEEF3 -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define BUTTONS #define BUTTON_A_PIN PB1 diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 2da896bfd7..3aa363b92e 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -160,7 +160,7 @@ #define OSD #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY) #define BUTTONS #define BUTTON_A_PIN PD2 diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index 8d5544513e..90b31b31b6 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -117,7 +117,7 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES ( FEATURE_TELEMETRY ) #define TARGET_CONFIG #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index b382b15702..90fd89d5ca 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -114,7 +114,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND_PIN UART3_RX_PIN diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 17e04007b2..4cb7f333bf 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -127,6 +127,14 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +// OSD +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PA14 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + #define USE_I2C #define USE_I2C_DEVICE_1 @@ -142,9 +150,9 @@ // Default configuration #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 // Target IO and timers #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk index b4565c621b..d484463b26 100644 --- a/src/main/target/YUPIF4/target.mk +++ b/src/main/target/YUPIF4/target.mk @@ -4,5 +4,7 @@ FEATURES += SDCARD VCP TARGET_SRC = \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_spi_icm20689.c + drivers/accgyro_spi_icm20689.c\ + drivers/max7456.c \ + io/osd.c diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d99f45cdf4..69f9eb73bb 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -56,50 +56,29 @@ #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz static bool crsfTelemetryEnabled; -static uint8_t crsfCrc; static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) { - crsfCrc = 0; dst->ptr = crsfFrame; dst->end = ARRAYEND(crsfFrame); sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST); } -static void crsfSerialize8(sbuf_t *dst, uint8_t v) +static void crsfWriteCrc(sbuf_t *dst, uint8_t *start) { - sbufWriteU8(dst, v); - crsfCrc = crc8_dvb_s2(crsfCrc, v); -} - -static void crsfSerialize16(sbuf_t *dst, uint16_t v) -{ - // Use BigEndian format - crsfSerialize8(dst, (v >> 8)); - crsfSerialize8(dst, (uint8_t)v); -} - -static void crsfSerialize32(sbuf_t *dst, uint32_t v) -{ - // Use BigEndian format - crsfSerialize8(dst, (v >> 24)); - crsfSerialize8(dst, (v >> 16)); - crsfSerialize8(dst, (v >> 8)); - crsfSerialize8(dst, (uint8_t)v); -} - -static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len) -{ - for (int ii = 0; ii< len; ++ii) { - crsfSerialize8(dst, data[ii]); + uint8_t crc = 0; + uint8_t *end = sbufPtr(dst); + for (uint8_t *ptr = start; ptr < end; ++ptr) { + crc = crc8_dvb_s2(crc, *ptr); } + sbufWriteU8(dst, crc); } static void crsfFinalize(sbuf_t *dst) { - sbufWriteU8(dst, crsfCrc); + crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length sbufSwitchToReader(dst, crsfFrame); // write the telemetry frame to the receiver. crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); @@ -107,7 +86,7 @@ static void crsfFinalize(sbuf_t *dst) static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame) { - sbufWriteU8(dst, crsfCrc); + crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length sbufSwitchToReader(dst, crsfFrame); const int frameSize = sbufBytesRemaining(dst); for (int ii = 0; sbufBytesRemaining(dst); ++ii) { @@ -122,7 +101,7 @@ CRSF frame has the structure: Device address: (uint8_t) Frame length: length in bytes including Type (uint8_t) Type: (uint8_t) -CRC: (uint8_t) +CRC: (uint8_t), crc of and */ /* @@ -139,15 +118,15 @@ void crsfFrameGps(sbuf_t *dst) { // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); - crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees - crsfSerialize32(dst, GPS_coord[LON]); - crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s - crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 + sbufWriteU8(dst, CRSF_FRAMETYPE_GPS); + sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees + sbufWriteU32BigEndian(dst, GPS_coord[LON]); + sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s + sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 //Send real GPS altitude only if it's reliable (there's a GPS fix) const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; - crsfSerialize16(dst, altitude); - crsfSerialize8(dst, GPS_numSat); + sbufWriteU16BigEndian(dst, altitude); + sbufWriteU8(dst, GPS_numSat); } /* @@ -162,24 +141,24 @@ void crsfFrameBatterySensor(sbuf_t *dst) { // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - crsfSerialize16(dst, getBatteryVoltage()); // vbat is in units of 0.1V + sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); + sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V #ifdef CLEANFLIGHT const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource); const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A - crsfSerialize16(dst, amperage); // amperage is in units of 0.1A + sbufWriteU16BigEndian(dst, amperage); // amperage is in units of 0.1A const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else - crsfSerialize16(dst, getAmperage() / 10); + sbufWriteU16BigEndian(dst, getAmperage() / 10); const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining(); #endif - crsfSerialize8(dst, (batteryCapacity >> 16)); - crsfSerialize8(dst, (batteryCapacity >> 8)); - crsfSerialize8(dst, (uint8_t)batteryCapacity); + sbufWriteU8(dst, (batteryCapacity >> 16)); + sbufWriteU8(dst, (batteryCapacity >> 8)); + sbufWriteU8(dst, (uint8_t)batteryCapacity); - crsfSerialize8(dst, batteryRemainingPercentage); + sbufWriteU8(dst, batteryRemainingPercentage); } typedef enum { @@ -216,10 +195,10 @@ int16_t Yaw angle ( rad / 10000 ) void crsfFrameAttitude(sbuf_t *dst) { sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE); - crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch)); - crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll)); - crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw)); + sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE); + sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch)); + sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll)); + sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw)); } /* @@ -229,11 +208,10 @@ char[] Flight mode ( Null­terminated string ) */ void crsfFrameFlightMode(sbuf_t *dst) { - // just do Angle for the moment as a placeholder // write zero for frame length, since we don't know it yet uint8_t *lengthPtr = sbufPtr(dst); sbufWriteU8(dst, 0); - crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); + sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); // use same logic as OSD, so telemetry displays same flight text as OSD const char *flightMode = "ACRO"; @@ -247,9 +225,8 @@ void crsfFrameFlightMode(sbuf_t *dst) } else if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = "HOR"; } - crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode)); - crsfSerialize8(dst, 0); // zero terminator for string - // write in the length + sbufWriteString(dst, flightMode); + // write in the frame length *lengthPtr = sbufPtr(dst) - lengthPtr; }