mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
Merge remote-tracking branch 'betaflight/master' into betaflight-master
This commit is contained in:
commit
5a360c51bb
98 changed files with 1077 additions and 834 deletions
|
@ -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
|
||||
|
|
67
.travis.yml
67
.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
|
||||
|
|
117
Makefile
117
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.
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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 },
|
||||
|
|
|
@ -40,3 +40,5 @@ typedef enum {
|
|||
} angle_index_t;
|
||||
|
||||
#define ANGLE_INDEX_COUNT 2
|
||||
|
||||
#define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -20,7 +20,7 @@
|
|||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#define EEPROM_CONF_VERSION 158
|
||||
#define EEPROM_CONF_VERSION 159
|
||||
|
||||
bool isEEPROMContentValid(void);
|
||||
bool loadEEPROM(void);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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);
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -141,7 +141,6 @@
|
|||
|
||||
#undef LED_STRIP
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -138,7 +138,6 @@
|
|||
|
||||
#undef LED_STRIP
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
};
|
|
@ -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
|
||||
|
|
0
src/main/target/KIWIF4/PLUMF4.mk
Normal file
0
src/main/target/KIWIF4/PLUMF4.mk
Normal file
|
@ -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
|
||||
};
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
1
src/main/target/OMNIBUSF4/VGOODDHF4.mk
Normal file
1
src/main/target/OMNIBUSF4/VGOODDHF4.mk
Normal file
|
@ -0,0 +1 @@
|
|||
#VGOODDHF4.mk file
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
0
src/main/target/REVO/AIRBOTF4SD.mk
Normal file
0
src/main/target/REVO/AIRBOTF4SD.mk
Normal file
|
@ -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
|
||||
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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 <Type> and <Payload>
|
||||
*/
|
||||
|
||||
/*
|
||||
|
@ -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 ( Nullterminated 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;
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue