Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 09:16:07 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'betaflight/master' into betaflight-master

Browse source
This commit is contained in:
Dominic Clifton 2017-04-01 14:49:34 +01:00
commit 5a360c51bb
98 changed files with 1077 additions and 834 deletions
Download patch file Download diff file Expand all files Collapse all files

8
.travis.sh
View file

@ -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
View file

@ -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
View file

@ -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.

7
make/tools.mk
View file

@ -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

65
src/main/blackbox/blackbox.c
View file

@ -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);

4
src/main/blackbox/blackbox.h
View file

@ -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 {

2
src/main/build/debug.h
View file

@ -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;

20
src/main/cms/cms_menu_blackbox.c
View file

@ -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 },

2
src/main/common/axis.h
View file

@ -40,3 +40,5 @@ typedef enum {
} angle_index_t;
#define ANGLE_INDEX_COUNT 2
#define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1)

22
src/main/common/streambuf.c
View file

@ -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

3
src/main/common/streambuf.h
View file

@ -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);

2
src/main/config/config_eeprom.h
View file

@ -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);

2
src/main/drivers/accgyro.h
View file

@ -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

91
src/main/drivers/accgyro_mpu.c
View file

@ -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)

15
src/main/drivers/accgyro_mpu.h
View file

@ -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);

13
src/main/drivers/accgyro_mpu3050.c
View file

@ -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;
}

16
src/main/drivers/accgyro_mpu6050.c
View file

@ -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
}

22
src/main/drivers/accgyro_mpu6500.c
View file

@ -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);
}

3
src/main/drivers/accgyro_mpu6500.h
View file

@ -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)

125
src/main/drivers/accgyro_spi_bmi160.c
View file

@ -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;
}

6
src/main/drivers/accgyro_spi_bmi160.h
View file

@ -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);

55
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -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);

8
src/main/drivers/accgyro_spi_icm20689.h
View file

@ -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);

55
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -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

8
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -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);

63
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -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;
}

12
src/main/drivers/accgyro_spi_mpu6500.h
View file

@ -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);

73
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -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;
}

14
src/main/drivers/accgyro_spi_mpu9250.h
View file

@ -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);

1
src/main/drivers/compass.h
View file

@ -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;

36
src/main/drivers/compass_ak8963.c
View file

@ -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

8
src/main/drivers/sensor.h
View file

@ -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);

42
src/main/fc/cli.c
View file

@ -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");
}

15
src/main/fc/config.c
View file

@ -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);

4
src/main/fc/config.h
View file

@ -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,

6
src/main/fc/fc_core.c
View file

@ -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

27
src/main/fc/fc_init.c
View file

@ -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();

6
src/main/fc/fc_msp.c
View file

@ -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;

2
src/main/fc/fc_rc.c
View file

@ -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];

366
src/main/fc/rc_adjustments.c
View file

@ -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);
}

17
src/main/fc/rc_adjustments.h
View file

@ -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;

2
src/main/fc/rc_controls.c
View file

@ -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);

2
src/main/fc/rc_controls.h
View file

@ -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);

4
src/main/flight/altitudehold.c
View file

@ -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)

2
src/main/flight/altitudehold.h
View file

@ -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);

4
src/main/flight/mixer.c
View file

@ -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

2
src/main/flight/mixer.h
View file

@ -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);

10
src/main/flight/pid.c
View file

@ -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 {

41
src/main/io/osd.c
View file

@ -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);

2
src/main/io/osd.h
View file

@ -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;

11
src/main/sensors/acceleration.c
View file

@ -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)) {

5
src/main/sensors/acceleration.h
View file

@ -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;

6
src/main/sensors/compass.c
View file

@ -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

5
src/main/sensors/esc_sensor.c
View file

@ -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;
}

47
src/main/sensors/gyro.c
View file

@ -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);

8
src/main/sensors/gyro.h
View file

@ -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;

2
src/main/target/ALIENFLIGHTF4/target.h
View file

@ -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

2
src/main/target/ALIENFLIGHTNGF7/target.h
View file

@ -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

1
src/main/target/ANYFCF7/target.h
View file

@ -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

2
src/main/target/BEEROTORF4/target.h
View file

@ -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

2
src/main/target/BETAFLIGHTF3/target.h
View file

@ -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

6
src/main/target/BLUEJAYF4/config.c
View file

@ -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;
}
}
}

1
src/main/target/BLUEJAYF4/target.h
View file

@ -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

1
src/main/target/COLIBRI/target.h
View file

@ -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

1
src/main/target/F4BY/target.h
View file

@ -141,7 +141,6 @@
#undef LED_STRIP
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

2
src/main/target/FISHDRONEF4/target.h
View file

@ -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

1
src/main/target/FURYF3/target.h
View file

@ -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

1
src/main/target/FURYF4/target.h
View file

@ -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

1
src/main/target/FURYF7/target.h
View file

@ -138,7 +138,6 @@
#undef LED_STRIP
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

1
src/main/target/IMPULSERCF3/target.h
View file

@ -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

1
src/main/target/ISHAPEDF3/target.h
View file

@ -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

4
src/main/target/KAKUTEF4/target.c
View file

@ -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
};

4
src/main/target/KAKUTEF4/target.h
View file

@ -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
View file

4
src/main/target/KIWIF4/target.c
View 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
};

22
src/main/target/KIWIF4/target.h
View file

@ -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

4
src/main/target/LUX_RACE/target.h
View file

@ -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

1
src/main/target/NERO/target.h
View file

@ -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

1
src/main/target/NUCLEOF7/target.h
View file

@ -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

2
src/main/target/OMNIBUS/target.h
View file

@ -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
View file

@ -0,0 +1 @@
#VGOODDHF4.mk file

8
src/main/target/OMNIBUSF4/target.c
View 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

39
src/main/target/OMNIBUSF4/target.h
View file

@ -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
View file

96
src/main/target/REVO/target.h
View 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

6
src/main/target/REVO/target.mk
View file

@ -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 \

1
src/main/target/SINGULARITY/target.h
View file

@ -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

2
src/main/target/SIRINFPV/target.h
View file

@ -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

1
src/main/target/SPARKY2/target.h
View file

@ -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

2
src/main/target/SPRACINGF3/target.h
View file

@ -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

2
src/main/target/SPRACINGF3EVO/target.h
View file

@ -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

1
src/main/target/SPRACINGF3MINI/target.h
View file

@ -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

2
src/main/target/SPRACINGF3NEO/target.h
View file

@ -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

2
src/main/target/TINYFISH/target.h
View file

@ -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

1
src/main/target/X_RACERSPI/target.h
View file

@ -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

10
src/main/target/YUPIF4/target.h
View file

@ -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
src/main/target/YUPIF4/target.mk
View file

@ -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

85
src/main/telemetry/crsf.c
View file

@ -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 ( Null­terminated string )
*/
void crsfFrameFlightMode(sbuf_t *dst)
{
// just do Angle for the moment as a placeholder
// write zero for frame length, since we don't know it yet
uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0);
crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
// use same logic as OSD, so telemetry displays same flight text as OSD
const char *flightMode = "ACRO";
@ -247,9 +225,8 @@ void crsfFrameFlightMode(sbuf_t *dst)
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
}
crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
crsfSerialize8(dst, 0); // zero terminator for string
// write in the length
sbufWriteString(dst, flightMode);
// write in the frame length
*lengthPtr = sbufPtr(dst) - lengthPtr;
}