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

Merge pull request #681 from betaflight/development 

Development into master
This commit is contained in:
borisbstyle 2016-07-05 22:35:22 +02:00 committed by GitHub
parent c1af7ab878 a3ca1c59db
commit 1c67a9414b
178 changed files with 3114 additions and 2126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
Makefile
View File

@ -600,6 +600,7 @@ LDFLAGS = -lm \
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
-Wl,-L$(LINKER_DIR) \
-Wl,--cref \
-Wl,--no-wchar-size-warning \
-T$(LD_SCRIPT)
###############################################################################
@ -664,27 +665,39 @@ all: $(VALID_TARGETS)
$(VALID_TARGETS):
echo "" && \
echo "Building $@" && \
$(MAKE) -j binary hex TARGET=$@ && \
$(MAKE) binary hex TARGET=$@ && \
echo "Building $@ succeeded."
## clean : clean up all temporary / machine-generated files
CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) )
TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) )
## clean : clean up temporary / machine-generated files
clean:
echo "Cleaning $(TARGET)"
rm -f $(CLEAN_ARTIFACTS)
rm -rf $(OBJECT_DIR)/$(TARGET)
echo "Cleaning $(TARGET) succeeded."
## clean_test : clean up all temporary / machine-generated files (tests)
## clean_test : clean up temporary / machine-generated files (tests)
clean_test:
cd src/test && $(MAKE) clean || true
## clean_all_targets : clean all valid target platforms
clean_all:
for clean_target in $(VALID_TARGETS); do \
echo "" && \
echo "Cleaning $$clean_target" && \
$(MAKE) -j TARGET=$$clean_target clean || \
break; \
echo "Cleaning $$clean_target succeeded."; \
done
## clean_<TARGET> : clean up one specific target
$(CLEAN_TARGETS) :
$(MAKE) -j TARGET=$(subst clean_,,$@) clean
## <TARGET>_clean : clean up one specific target (alias for above)
$(TARGETS_CLEAN) :
$(MAKE) -j TARGET=$(subst _clean,,$@) clean
## clean_all : clean all valid targets
clean_all:$(CLEAN_TARGETS)
## all_clean : clean all valid targets (alias for above)
all_clean:$(TARGETS_CLEAN)
flash_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
@ -700,8 +713,11 @@ st-flash_$(TARGET): $(TARGET_BIN)
## st-flash : flash firmware (.bin) onto flight controller
st-flash: st-flash_$(TARGET)
binary: $(TARGET_BIN)
hex: $(TARGET_HEX)
binary:
$(MAKE) -j $(TARGET_BIN)
hex:
$(MAKE) -j $(TARGET_HEX)
unbrick_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon

2
README.md
View File

@ -1,6 +1,6 @@
# Betaflight
![Betaflight](https://dl.dropboxusercontent.com/u/31537757/betaflight%20logo.jpg)
![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067)
Clean-code version of baseflight flight-controller - flight controllers are used to fly multi-rotor craft and fixed wing craft.

11
fake_travis_build.sh
View File

@ -20,7 +20,10 @@ targets=("PUBLISHMETA=True" \
"TARGET=ALIENFLIGHTF3" \
"TARGET=DOGE" \
"TARGET=SINGULARITY" \
"TARGET=SIRINFPV")
"TARGET=SIRINFPV" \
"TARGET=X_RACERSPI")
#fake a travis build environment
export TRAVIS_BUILD_NUMBER=$(date +%s)
@ -30,9 +33,9 @@ export TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/simulated}
for target in "${targets[@]}"
do
unset RUNTESTS PUBLISHMETA TARGET
echo
echo
echo "BUILDING '$target'"
echo
echo
echo "BUILDING '$target'"
eval "export $target"
make -f Makefile clean
./.travis.sh

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

@ -325,9 +325,6 @@ extern uint32_t currentTime;
//From rx.c:
extern uint16_t rssi;
//From gyro.c
extern uint32_t targetLooptime;
//From rc_controls.c
extern uint32_t rcModeActivationMask;
@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo()
}
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);

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

@ -22,7 +22,6 @@
#include "drivers/accgyro.h"
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"

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

@ -17,76 +17,81 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
// PT1 Low Pass filter
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
}
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
float pt1FilterApply(pt1Filter_t *filter, float input)
{
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
}
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
/* sets up a biquad Filter */
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
{
float sampleRate;
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
sampleRate = 1 / ((float)refreshRate * 0.000001f);
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
/* setup variables */
omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
sn = sinf(omega);
cs = cosf(omega);
// setup variables
const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
const float sn = sinf(omega);
const float cs = cosf(omega);
//this is wrong, should be hyperbolic sine
//alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
alpha = sn / (2 * BIQUAD_Q);
const float alpha = sn / (2 * BIQUAD_Q);
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
const float b0 = (1 - cs) / 2;
const float b1 = 1 - cs;
const float b2 = (1 - cs) / 2;
const float a0 = 1 + alpha;
const float a1 = -2 * cs;
const float a2 = 1 - alpha;
/* precompute the coefficients */
newState->b0 = b0 / a0;
newState->b1 = b1 / a0;
newState->b2 = b2 / a0;
newState->a1 = a1 / a0;
newState->a2 = a2 / a0;
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
/* zero initial samples */
newState->d1 = newState->d2 = 1;
// zero initial samples
filter->d1 = filter->d2 = 0;
}
/* Computes a biquad_t filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed
float biquadFilterApply(biquadFilter_t *filter, float input)
{
float result;
result = state->b0 * sample + state->d1;
state->d1 = state->b1 * sample - state->a1 * result + state->d2;
state->d2 = state->b2 * sample - state->a2 * result;
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}

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

@ -17,20 +17,26 @@
#define DELTA_MAX_SAMPLES 12
typedef struct filterStatePt1_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;
typedef struct pt1Filter_s {
float state;
float RC;
float dT;
} pt1Filter_t;
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
float d1, d2;
} biquad_t;
} biquadFilter_t;
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
float biquadFilterApply(biquadFilter_t *filter, float input);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
float applyBiQuadFilter(float sample, biquad_t *state);
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

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

@ -38,7 +38,6 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/serial.h"
#include "drivers/gyro_sync.h"
#include "drivers/pwm_output.h"
#include "drivers/max7456.h"
@ -90,6 +89,7 @@
#endif
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
void targetConfiguration(void);
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
@ -269,8 +269,13 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
{
#ifdef BRUSHED_MOTORS
escAndServoConfig->minthrottle = 1000;
escAndServoConfig->maxthrottle = 2000;
#else
escAndServoConfig->minthrottle = 1150;
escAndServoConfig->maxthrottle = 1850;
#endif
escAndServoConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500;
escAndServoConfig->escDesyncProtection = 0;
@ -416,8 +421,6 @@ uint16_t getCurrentMinthrottle(void)
// Default settings
static void resetConf(void)
{
int i;
// Clear all configuration
memset(&masterConfig, 0, sizeof(master_t));
setProfile(0);
@ -478,7 +481,11 @@ static void resetConf(void)
resetTelemetryConfig(&masterConfig.telemetryConfig);
#ifdef SERIALRX_PROVIDER
masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER;
#else
masterConfig.rxConfig.serialrx_provider = 0;
#endif
masterConfig.rxConfig.sbus_inversion = 1;
masterConfig.rxConfig.spektrum_sat_bind = 0;
masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1;
@ -488,7 +495,7 @@ static void resetConf(void)
masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection
masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i];
channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
@ -550,8 +557,7 @@ static void resetConf(void)
resetPidProfile(&currentProfile->pidProfile);
uint8_t rI;
for (rI = 0; rI<MAX_RATEPROFILES; rI++) {
for (int rI = 0; rI<MAX_RATEPROFILES; rI++) {
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]);
}
resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
@ -582,7 +588,7 @@ static void resetConf(void)
#ifdef USE_SERVOS
// servos
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
masterConfig.servoConf[i].min = DEFAULT_SERVO_MIN;
masterConfig.servoConf[i].max = DEFAULT_SERVO_MAX;
masterConfig.servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
@ -601,8 +607,9 @@ static void resetConf(void)
#endif
// custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
masterConfig.customMotorMixer[i].throttle = 0.0f;
}
#ifdef LED_STRIP
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
@ -639,62 +646,19 @@ static void resetConf(void)
masterConfig.blackbox_rate_denom = 1;
#endif // BLACKBOX
// alternative defaults settings for COLIBRI RACE targets
#if defined(COLIBRI_RACE)
masterConfig.escAndServoConfig.minthrottle = 1025;
masterConfig.escAndServoConfig.maxthrottle = 1980;
masterConfig.batteryConfig.vbatmaxcellvoltage = 45;
masterConfig.batteryConfig.vbatmincellvoltage = 30;
#ifdef SERIALRX_UART
if (featureConfigured(FEATURE_RX_SERIAL)) {
masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL;
}
#endif
#if defined(TARGET_CONFIG)
targetConfiguration(&masterConfig);
#endif
#if defined(ALIENFLIGHT)
featureClear(FEATURE_ONESHOT125);
#ifdef ALIENFLIGHTF1
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
#else
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
#endif
#ifdef ALIENFLIGHTF3
masterConfig.batteryConfig.vbatscale = 20;
masterConfig.mag_hardware = MAG_NONE; // disabled by default
#endif
masterConfig.rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048;
masterConfig.rxConfig.spektrum_sat_bind = 5;
masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1;
masterConfig.escAndServoConfig.minthrottle = 1000;
masterConfig.escAndServoConfig.maxthrottle = 2000;
masterConfig.motor_pwm_rate = 32000;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
currentControlRateProfile->rates[FD_PITCH] = 40;
currentControlRateProfile->rates[FD_ROLL] = 40;
currentControlRateProfile->rates[FD_YAW] = 40;
parseRcChannels("TAER1234", &masterConfig.rxConfig);
masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif
#endif
#if defined(SINGULARITY)
// alternative defaults settings for SINGULARITY target
masterConfig.batteryConfig.vbatscale = 77;
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
targetConfiguration();
#endif
// copy first profile into remaining profile
for (i = 1; i < MAX_PROFILE_COUNT; i++) {
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
}
@ -751,7 +715,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
@ -806,7 +770,7 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
featureSet(DEFAULT_RX_FEATURE);
}
if (featureConfigured(FEATURE_RX_PPM)) {
@ -848,10 +812,10 @@ void validateAndFixConfig(void)
if (featureConfigured(FEATURE_SOFTSERIAL) && (
0
#ifdef USE_SOFTSERIAL1
|| (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
|| (WS2811_TIMER == SOFTSERIAL_1_TIMER)
#endif
#ifdef USE_SOFTSERIAL2
|| (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
|| (WS2811_TIMER == SOFTSERIAL_2_TIMER)
#endif
)) {
// led strip needs the same timer as softserial
@ -901,15 +865,11 @@ void validateAndFixConfig(void)
#if defined(COLIBRI_RACE)
masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP);
featureSet(FEATURE_RX_PPM);
}
if(featureConfigured(FEATURE_RX_SERIAL)) {
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
//masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
}
#endif
useRxConfig(&masterConfig.rxConfig);

1
src/main/drivers/accgyro.h
View File

@ -27,6 +27,7 @@ typedef struct gyro_s {
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;
float scale; // scalefactor
uint32_t targetLooptime;
} gyro_t;
typedef struct acc_s {

8
src/main/drivers/accgyro_bma280.c
View File

@ -40,7 +40,7 @@ bool bma280Detect(acc_t *acc)
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xFB)
return false;
@ -51,8 +51,8 @@ bool bma280Detect(acc_t *acc)
static void bma280Init(acc_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc->acc_1G = 512 * 8;
}
@ -61,7 +61,7 @@ static bool bma280Read(int16_t *accelData)
{
uint8_t buf[6];
if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
return false;
}

8
src/main/drivers/accgyro_l3g4200d.c
View File

@ -63,7 +63,7 @@ bool l3g4200dDetect(gyro_t *gyro)
delay(25);
i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
if (deviceid != L3G4200D_ID)
return false;
@ -100,12 +100,12 @@ static void l3g4200dInit(uint8_t lpf)
delay(100);
ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
if (!ack)
failureMode(FAILURE_ACC_INIT);
delay(5);
i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
@ -113,7 +113,7 @@ static bool l3g4200dRead(int16_t *gyroADC)
{
uint8_t buf[6];
if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
return false;
}

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

@ -62,7 +62,7 @@
#define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00)
#define BLE_MSB ((uint8_t)0x40)
#define BLE_MSB ((uint8_t)0x40)
#define BOOT ((uint8_t)0x80)

8
src/main/drivers/accgyro_lsm303dlhc.c
View File

@ -26,7 +26,7 @@
#include "common/axis.h"
#include "system.h"
#include "gpio.h"
#include "io.h"
#include "bus_i2c.h"
#include "sensor.h"
@ -115,15 +115,15 @@ int32_t accelSummedSamples500Hz[3];
void lsm303dlhcAccInit(acc_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
delay(100);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
delay(10);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
delay(100);

57
src/main/drivers/accgyro_lsm303dlhc.h
View File

@ -22,8 +22,7 @@
*/
/* LSM303DLHC ACC struct */
typedef struct
{
typedef struct {
uint8_t Power_Mode; /* Power-down/Normal Mode */
uint8_t AccOutput_DataRate; /* OUT data rate */
uint8_t Axes_Enable; /* Axes enable */
@ -31,25 +30,23 @@ typedef struct
uint8_t BlockData_Update; /* Block Data Update */
uint8_t Endianness; /* Endian Data selection */
uint8_t AccFull_Scale; /* Full Scale selection */
}LSM303DLHCAcc_InitTypeDef;
} LSM303DLHCAcc_InitTypeDef;
/* LSM303DLHC Acc High Pass Filter struct */
typedef struct
{
typedef struct {
uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */
uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */
uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */
uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */
}LSM303DLHCAcc_FilterConfigTypeDef;
} LSM303DLHCAcc_FilterConfigTypeDef;
/* LSM303DLHC Mag struct */
typedef struct
{
typedef struct {
uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */
uint8_t MagOutput_DataRate; /* OUT data rate */
uint8_t Working_Mode; /* operating mode */
uint8_t MagFull_Scale; /* Full Scale selection */
}LSM303DLHCMag_InitTypeDef;
} LSM303DLHCMag_InitTypeDef;
/**
* @}
*/
@ -78,43 +75,11 @@ typedef struct
* @brief LSM303DLHC I2C Interface pins
*/
#define LSM303DLHC_I2C I2C1
#define LSM303DLHC_I2C_CLK RCC_APB1Periph_I2C1
#define LSM303DLHC_I2C_SCK_PIN GPIO_Pin_6 /* PB.06 */
#define LSM303DLHC_I2C_SCK_GPIO_PORT GPIOB /* GPIOB */
#define LSM303DLHC_I2C_SCK_GPIO_CLK RCC_AHBPeriph_GPIOB
#define LSM303DLHC_I2C_SCK_SOURCE GPIO_PinSource6
#define LSM303DLHC_I2C_SCK_AF GPIO_AF_4
#define LSM303DLHC_I2C_SDA_PIN GPIO_Pin_7 /* PB.7 */
#define LSM303DLHC_I2C_SDA_GPIO_PORT GPIOB /* GPIOB */
#define LSM303DLHC_I2C_SDA_GPIO_CLK RCC_AHBPeriph_GPIOB
#define LSM303DLHC_I2C_SDA_SOURCE GPIO_PinSource7
#define LSM303DLHC_I2C_SDA_AF GPIO_AF_4
#define LSM303DLHC_DRDY_PIN GPIO_Pin_2 /* PE.02 */
#define LSM303DLHC_DRDY_GPIO_PORT GPIOE /* GPIOE */
#define LSM303DLHC_DRDY_GPIO_CLK RCC_AHBPeriph_GPIOE
#define LSM303DLHC_DRDY_EXTI_LINE EXTI_Line2
#define LSM303DLHC_DRDY_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE
#define LSM303DLHC_DRDY_EXTI_PIN_SOURCE EXTI_PinSource2
#define LSM303DLHC_DRDY_EXTI_IRQn EXTI2_TS_IRQn
#define LSM303DLHC_I2C_INT1_PIN GPIO_Pin_4 /* PE.04 */
#define LSM303DLHC_I2C_INT1_GPIO_PORT GPIOE /* GPIOE */
#define LSM303DLHC_I2C_INT1_GPIO_CLK RCC_AHBPeriph_GPIOE
#define LSM303DLHC_I2C_INT1_EXTI_LINE EXTI_Line4
#define LSM303DLHC_I2C_INT1_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE
#define LSM303DLHC_I2C_INT1_EXTI_PIN_SOURCE EXTI_PinSource4
#define LSM303DLHC_I2C_INT1_EXTI_IRQn EXTI4_IRQn
#define LSM303DLHC_I2C_INT2_PIN GPIO_Pin_5 /* PE.05 */
#define LSM303DLHC_I2C_INT2_GPIO_PORT GPIOE /* GPIOE */
#define LSM303DLHC_I2C_INT2_GPIO_CLK RCC_AHBPeriph_GPIOE
#define LSM303DLHC_I2C_INT2_EXTI_LINE EXTI_Line5
#define LSM303DLHC_I2C_INT2_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE
#define LSM303DLHC_I2C_INT2_EXTI_PIN_SOURCE EXTI_PinSource5ss
#define LSM303DLHC_I2C_INT2_EXTI_IRQn EXTI9_5_IRQn
#define LSM303DLHC_I2C_SCK_PIN PB6 /* PB.06 */
#define LSM303DLHC_I2C_SDA_PIN PB7 /* PB.7 */
#define LSM303DLHC_DRDY_PIN PE2 /* PE.02 */
#define LSM303DLHC_I2C_INT1_PIN PE4 /* PE.04 */
#define LSM303DLHC_I2C_INT2_PIN PE5 /* PE.05 */
/******************************************************************************/
/*************************** START REGISTER MAPPING **************************/

8
src/main/drivers/accgyro_mma845x.c
View File

@ -89,7 +89,7 @@ bool mma8452Detect(acc_t *acc)
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
return false;
@ -109,9 +109,9 @@ static inline void mma8451ConfigureInterrupt(void)
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
#endif
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}
static void mma8452Init(acc_t *acc)

46
src/main/drivers/accgyro_mpu.c
View File

@ -32,7 +32,6 @@
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
@ -228,43 +227,43 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
void mpuIntExtiInit(void)
{
static bool mpuExtiInitDone = false;
static bool mpuExtiInitDone = false;
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = IORead(mpuIntIO);
if (status) {
return;
}
uint8_t status = IORead(mpuIntIO);
if (status) {
return;
}
#endif
IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
#endif
mpuExtiInitDone = true;
mpuExtiInitDone = true;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
{
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
return ack;
}
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
{
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
return ack;
}
@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC)
return true;
}
void checkMPUDataReady(bool *mpuDataReadyPtr) {
bool checkMPUDataReady(void)
{
bool ret;
if (mpuDataReady) {
*mpuDataReadyPtr = true;
ret = true;
mpuDataReady= false;
} else {
*mpuDataReadyPtr = false;
ret = false;
}
return ret;
}

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

@ -17,6 +17,8 @@
#pragma once
#include "exti.h"
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
@ -120,12 +122,12 @@ typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void);
typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
mpuReadRegisterFunc slowread;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
mpuReadRegisterFunc slowread;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
} mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration;
@ -185,4 +187,4 @@ void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(int16_t *gyroADC);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
void checkMPUDataReady(bool *mpuDataReadyPtr);
bool checkMPUDataReady(void);

1
src/main/drivers/accgyro_mpu3050.c
View File

@ -30,7 +30,6 @@
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu3050.h"
#include "gyro_sync.h"
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68

4
src/main/drivers/accgyro_mpu6050.c
View File

@ -1,4 +1,4 @@
/*
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
@ -38,8 +38,6 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu6050.h"
extern uint8_t mpuLowPassFilter;
//#define DEBUG_MPU_DATA_READY_INTERRUPT
// MPU6050, Standard address 0x68

16
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -49,12 +49,12 @@ static bool mpuSpi6000InitDone = false;
// Bits
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
@ -74,9 +74,9 @@ static bool mpuSpi6000InitDone = false;
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
#define BIT_INT_STATUS_DATA 0x01
#define BIT_GYRO 3
#define BIT_ACC 2
#define BIT_TEMP 1

2
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -1,7 +1,7 @@
#pragma once
#define MPU6000_CONFIG 0x1A
#define MPU6000_CONFIG 0x1A
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01

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

@ -72,7 +72,7 @@ static void mpu6500SpiInit(void)
IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI);
IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
hardwareInitialised = true;
}

64
src/main/drivers/accgyro_spi_mpu9250.c
View File

@ -64,7 +64,7 @@ void mpu9250ResetGyro(void)
bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU9250;
ENABLE_MPU9250;
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
spiTransferByte(MPU9250_SPI_INSTANCE, data);
@ -76,7 +76,7 @@ bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250;
ENABLE_MPU9250;
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250;
@ -86,7 +86,7 @@ bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250;
ENABLE_MPU9250;
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
@ -98,7 +98,7 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
void mpu9250SpiGyroInit(uint8_t lpf)
{
(void)(lpf);
(void)(lpf);
mpuIntExtiInit();
@ -126,55 +126,55 @@ void mpu9250SpiAccInit(acc_t *acc)
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
uint8_t in;
uint8_t attemptsRemaining = 20;
mpu9250WriteRegister(reg, data);
delayMicroseconds(100);
mpu9250WriteRegister(reg, data);
delayMicroseconds(100);
do {
mpu9250SlowReadRegister(reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250WriteRegister(reg, data);
delayMicroseconds(100);
}
mpu9250SlowReadRegister(reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250WriteRegister(reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
return false;
}
static void mpu9250AccAndGyroInit(uint8_t lpf) {
if (mpuSpi9250InitDone) {
return;
}
if (mpuSpi9250InitDone) {
return;
}
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
delay(50);
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
if (lpf == 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (lpf < 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (lpf > 4) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
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
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
#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.
verifympu9250WriteRegister(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);
@ -191,10 +191,10 @@ bool mpu9250SpiDetect(void)
#ifdef MPU9250_CS_PIN
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
#endif
IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI);
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI);
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
do {

2
src/main/drivers/accgyro_spi_mpu9250.h
View File

@ -1,7 +1,7 @@
#pragma once
#define mpu9250_CONFIG 0x1A
#define mpu9250_CONFIG 0x1A
/* We should probably use these. :)
#define BITS_DLPF_CFG_256HZ 0x00

10
src/main/drivers/adc.c
View File

@ -35,11 +35,11 @@ volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
uint8_t adcChannelByTag(ioTag_t ioTag)
{
for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
if (ioTag == adcTagMap[i].tag)
return adcTagMap[i].channel;
}
return 0;
for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
if (ioTag == adcTagMap[i].tag)
return adcTagMap[i].channel;
}
return 0;
}
uint16_t adcGetChannel(uint8_t channel)

3
src/main/drivers/adc.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "io.h"
typedef enum {
ADC_BATTERY = 0,
ADC_RSSI = 1,
@ -28,6 +30,7 @@ typedef enum {
#define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1)
typedef struct adc_config_s {
ioTag_t tag;
uint8_t adcChannel; // ADC1_INxx channel number
uint8_t dmaIndex; // index into DMA buffer in case of sparse channels
bool enabled;

46
src/main/drivers/adc_stm32f10x.c
View File

@ -81,7 +81,7 @@ void adcInit(drv_adc_config_t *init)
{
#if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN)
UNUSED(init);
UNUSED(init);
#endif
uint8_t i;
@ -91,60 +91,50 @@ void adcInit(drv_adc_config_t *init)
#ifdef VBAT_ADC_PIN
if (init->enableVBat) {
IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN));
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_BATTERY].enabled = true;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5;
adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN);
}
#endif
#ifdef RSSI_ADC_PIN
if (init->enableRSSI) {
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN));
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_RSSI].enabled = true;
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5;
adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN);
}
#endif
#ifdef EXTERNAL1_ADC_PIN
if (init->enableExternal1) {
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN);
}
#endif
#ifdef CURRENT_METER_ADC_PIN
if (init->enableCurrentMeter) {
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_CURRENT].enabled = true;
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5;
adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN);
}
#endif
ADCDevice device = adcDeviceByInstance(ADC_INSTANCE);
if (device == ADCINVALID)
return;
adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)
continue;
IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AIN, 0));
adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
adcConfig[i].dmaIndex = configuredAdcChannels++;
adcConfig[i].sampleTime = ADC_SampleTime_239Cycles5;
adcConfig[i].enabled = true;
}
RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI)
RCC_ClockCmd(adc.rccADC, ENABLE);
RCC_ClockCmd(adc.rccDMA, ENABLE);
// FIXME ADC driver assumes all the GPIO was already placed in 'AIN' mode
DMA_DeInit(adc.DMAy_Channelx);
DMA_InitTypeDef DMA_InitStructure;
DMA_StructInit(&DMA_InitStructure);

52
src/main/drivers/adc_stm32f30x.c
View File

@ -107,53 +107,25 @@ void adcInit(drv_adc_config_t *init)
#ifdef VBAT_ADC_PIN
if (init->enableVBat) {
IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN));
adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_BATTERY].enabled = true;
adcChannelCount++;
adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN);
}
#endif
#ifdef RSSI_ADC_PIN
if (init->enableRSSI) {
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN));
adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_RSSI].enabled = true;
adcChannelCount++;
adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN);
}
#endif
#ifdef CURRENT_METER_ADC_GPIO
#ifdef CURRENT_METER_ADC_PIN
if (init->enableCurrentMeter) {
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_CURRENT].enabled = true;
adcChannelCount++;
adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN);
}
#endif
#ifdef EXTERNAL1_ADC_GPIO
#ifdef EXTERNAL1_ADC_PIN
if (init->enableExternal1) {
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcChannelCount++;
adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN);
}
#endif
@ -163,6 +135,18 @@ void adcInit(drv_adc_config_t *init)
adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)
continue;
IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
adcConfig[i].dmaIndex = adcChannelCount++;
adcConfig[i].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[i].enabled = true;
}
RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
RCC_ClockCmd(adc.rccADC, ENABLE);
RCC_ClockCmd(adc.rccDMA, ENABLE);

84
src/main/drivers/adc_stm32f4xx.c
View File

@ -39,8 +39,8 @@
#endif
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
};
/* note these could be packed up for saving space */
@ -75,13 +75,13 @@ const adcTagMap_t adcTagMap[] = {
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
{
if (instance == ADC1)
return ADCDEV_1;
if (instance == ADC1)
return ADCDEV_1;
/*
if (instance == ADC2) // TODO add ADC2 and 3
return ADCDEV_2;
if (instance == ADC2) // TODO add ADC2 and 3
return ADCDEV_2;
*/
return ADCINVALID;
return ADCINVALID;
}
void adcInit(drv_adc_config_t *init)
@ -100,45 +100,25 @@ void adcInit(drv_adc_config_t *init)
#ifdef VBAT_ADC_PIN
if (init->enableVBat) {
IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN)); //VBAT_ADC_CHANNEL;
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_BATTERY].enabled = true;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles;
}
#endif
#ifdef EXTERNAL1_ADC_PIN
if (init->enableExternal1) {
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL;
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles;
adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); //VBAT_ADC_CHANNEL;
}
#endif
#ifdef RSSI_ADC_PIN
if (init->enableRSSI) {
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL;
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_RSSI].enabled = true;
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles;
adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); //RSSI_ADC_CHANNEL;
}
#endif
#ifdef EXTERNAL1_ADC_PIN
if (init->enableExternal1) {
adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); //EXTERNAL1_ADC_CHANNEL;
}
#endif
#ifdef CURRENT_METER_ADC_PIN
if (init->enableCurrentMeter) {
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL;
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_CURRENT].enabled = true;
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles;
adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); //CURRENT_METER_ADC_CHANNEL;
}
#endif
@ -150,6 +130,18 @@ void adcInit(drv_adc_config_t *init)
adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)
continue;
IOInit(IOGetByTag(adcConfig[i].tag), OWNER_SYSTEM, RESOURCE_ADC);
IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag);
adcConfig[i].dmaIndex = configuredAdcChannels++;
adcConfig[i].sampleTime = ADC_SampleTime_480Cycles;
adcConfig[i].enabled = true;
}
RCC_ClockCmd(adc.rccDMA, ENABLE);
RCC_ClockCmd(adc.rccADC, ENABLE);
@ -174,21 +166,21 @@ void adcInit(drv_adc_config_t *init)
ADC_CommonInitTypeDef ADC_CommonInitStructure;
ADC_CommonStructInit(&ADC_CommonInitStructure);
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8;
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8;
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
ADC_CommonInit(&ADC_CommonInitStructure);
ADC_StructInit(&ADC_InitStructure);
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels;
ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels;
ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
ADC_Init(adc.ADCx, &ADC_InitStructure);

18
src/main/drivers/barometer_bmp085.c
View File

@ -45,8 +45,8 @@ static bool isEOCConnected = true;
// EXTI14 for BMP085 End of Conversion Interrupt
void bmp085_extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
isConversionComplete = true;
UNUSED(cb);
isConversionComplete = true;
}
bool bmp085TestEOCConnected(const bmp085Config_t *config);
@ -184,13 +184,13 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
delay(20); // datasheet says 10ms, we'll be careful and do 20.
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
if (ack) {
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3;
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
@ -277,7 +277,7 @@ static void bmp085_start_ut(void)
#if defined(BARO_EOC_GPIO)
isConversionComplete = false;
#endif
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
}
static void bmp085_get_ut(void)
@ -291,7 +291,7 @@ static void bmp085_get_ut(void)
}
#endif
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
bmp085_ut = (data[0] << 8) | data[1];
}
@ -305,7 +305,7 @@ static void bmp085_start_up(void)
isConversionComplete = false;
#endif
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
}
/** read out up for pressure conversion
@ -323,7 +323,7 @@ static void bmp085_get_up(void)
}
#endif
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
>> (8 - bmp085.oversampling_setting);
}
@ -343,7 +343,7 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature
static void bmp085_get_cal_param(void)
{
uint8_t data[22];
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
/*parameters AC1-AC6*/
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];

4
src/main/drivers/barometer_bmp085.h
View File

@ -18,8 +18,8 @@
#pragma once
typedef struct bmp085Config_s {
ioTag_t xclrIO;
ioTag_t eocIO;
ioTag_t xclrIO;
ioTag_t eocIO;
} bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);

10
src/main/drivers/barometer_bmp280.c
View File

@ -83,14 +83,14 @@ bool bmp280Detect(baro_t *baro)
// set oversampling + power mode (forced), and start sampling
bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE);
#else
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
return false;
// read calibration
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
// set oversampling + power mode (forced), and start sampling
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
#endif
bmp280InitDone = true;
@ -129,7 +129,7 @@ static void bmp280_start_up(void)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
}
static void bmp280_get_up(void)
@ -137,7 +137,7 @@ static void bmp280_get_up(void)
uint8_t data[BMP280_DATA_FRAME_SIZE];
// read data from sensor
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
}

12
src/main/drivers/barometer_ms5611.c
View File

@ -67,7 +67,7 @@ bool ms5611Detect(baro_t *baro)
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig);
ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig);
if (!ack)
return false;
@ -93,14 +93,14 @@ bool ms5611Detect(baro_t *baro)
static void ms5611_reset(void)
{
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1);
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1);
delayMicroseconds(2800);
}
static uint16_t ms5611_prom(int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
return rxbuf[0] << 8 | rxbuf[1];
}
@ -137,13 +137,13 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
static uint32_t ms5611_read_adc(void)
{
uint8_t rxbuf[3];
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
static void ms5611_start_ut(void)
{
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
}
static void ms5611_get_ut(void)
@ -153,7 +153,7 @@ static void ms5611_get_ut(void)
static void ms5611_start_up(void)
{
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
}
static void ms5611_get_up(void)

310
src/main/drivers/bus_i2c_soft.c
View File

@ -48,218 +48,218 @@ static volatile uint16_t i2cErrorCount = 0;
static void I2C_delay(void)
{
volatile int i = 7;
while (i) {
i--;
}
volatile int i = 7;
while (i) {
i--;
}
}
static bool I2C_Start(void)
{
SDA_H;
SCL_H;
I2C_delay();
if (!SDA_read) {
return false;
}
SDA_L;
I2C_delay();
if (SDA_read) {
return false;
}
SDA_L;
I2C_delay();
return true;
SDA_H;
SCL_H;
I2C_delay();
if (!SDA_read) {
return false;
}
SDA_L;
I2C_delay();
if (SDA_read) {
return false;
}
SDA_L;
I2C_delay();
return true;
}
static void I2C_Stop(void)
{
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SDA_H;
I2C_delay();
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SDA_H;
I2C_delay();
}
static void I2C_Ack(void)
{
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
}
static void I2C_NoAck(void)
{
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
}
static bool I2C_WaitAck(void)
{
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
if (SDA_read) {
SCL_L;
return false;
}
SCL_L;
return true;
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
if (SDA_read) {
SCL_L;
return false;
}
SCL_L;
return true;
}
static void I2C_SendByte(uint8_t byte)
{
uint8_t i = 8;
while (i--) {
SCL_L;
I2C_delay();
if (byte & 0x80) {
SDA_H;
}
else {
SDA_L;
}
byte <<= 1;
I2C_delay();
SCL_H;
I2C_delay();
}
SCL_L;
uint8_t i = 8;
while (i--) {
SCL_L;
I2C_delay();
if (byte & 0x80) {
SDA_H;
}
else {
SDA_L;
}
byte <<= 1;
I2C_delay();
SCL_H;
I2C_delay();
}
SCL_L;
}
static uint8_t I2C_ReceiveByte(void)
{
uint8_t i = 8;
uint8_t byte = 0;
uint8_t i = 8;
uint8_t byte = 0;
SDA_H;
while (i--) {
byte <<= 1;
SCL_L;
I2C_delay();
SCL_H;
I2C_delay();
if (SDA_read) {
byte |= 0x01;
}
}
SCL_L;
return byte;
SDA_H;
while (i--) {
byte <<= 1;
SCL_L;
I2C_delay();
SCL_H;
I2C_delay();
if (SDA_read) {
byte |= 0x01;
}
}
SCL_L;
return byte;
}
void i2cInit(I2CDevice device)
{
UNUSED(device);
UNUSED(device);
scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL));
sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA));
scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL));
sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA));
IOConfigGPIO(scl, IOCFG_OUT_OD);
IOConfigGPIO(sda, IOCFG_OUT_OD);
IOConfigGPIO(scl, IOCFG_OUT_OD);
IOConfigGPIO(sda, IOCFG_OUT_OD);
}
bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
UNUSED(device);
int i;
if (!I2C_Start()) {
i2cErrorCount++;
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
for (i = 0; i < len; i++) {
I2C_SendByte(data[i]);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
}
I2C_Stop();
return true;
int i;
if (!I2C_Start()) {
i2cErrorCount++;
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
for (i = 0; i < len; i++) {
I2C_SendByte(data[i]);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
}
I2C_Stop();
return true;
}
bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data)
{
UNUSED(device);
if (!I2C_Start()) {
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
I2C_SendByte(data);
I2C_WaitAck();
I2C_Stop();
return true;
if (!I2C_Start()) {
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
I2C_SendByte(data);
I2C_WaitAck();
I2C_Stop();
return true;
}
bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
UNUSED(device);
if (!I2C_Start()) {
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
I2C_Start();
I2C_SendByte(addr << 1 | I2C_Direction_Receiver);
I2C_WaitAck();
while (len) {
*buf = I2C_ReceiveByte();
if (len == 1) {
I2C_NoAck();
}
else {
I2C_Ack();
}
buf++;
len--;
}
I2C_Stop();
return true;
if (!I2C_Start()) {
return false;
}
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
if (!I2C_WaitAck()) {
I2C_Stop();
i2cErrorCount++;
return false;
}
I2C_SendByte(reg);
I2C_WaitAck();
I2C_Start();
I2C_SendByte(addr << 1 | I2C_Direction_Receiver);
I2C_WaitAck();
while (len) {
*buf = I2C_ReceiveByte();
if (len == 1) {
I2C_NoAck();
}
else {
I2C_Ack();
}
buf++;
len--;
}
I2C_Stop();
return true;
}
uint16_t i2cGetErrorCounter(void)
{
return i2cErrorCount;
return i2cErrorCount;
}
#endif

3
src/main/drivers/bus_i2c_stm32f10x.c
View File

@ -422,6 +422,9 @@ void i2cInit(I2CDevice device)
I2C_Cmd(i2c->dev, ENABLE);
I2C_Init(i2c->dev, &i2cInit);
I2C_StretchClockCmd(i2c->dev, ENABLE);
// I2C ER Interrupt
nvic.NVIC_IRQChannel = i2c->er_irq;

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

@ -31,6 +31,15 @@
#ifndef SOFT_I2C
#if defined(USE_I2C_PULLUP)
#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP)
#else
#define IOCFG_I2C IOCFG_AF_OD
#endif
#define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4.
#define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000)
#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
#define I2C_GPIO_AF GPIO_AF_4
@ -83,8 +92,8 @@ void i2cInit(I2CDevice device)
RCC_ClockCmd(i2c->rcc, ENABLE);
RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK);
IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4);
IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4);
I2C_InitTypeDef i2cInit = {
.I2C_Mode = I2C_Mode_I2C,
@ -93,14 +102,13 @@ void i2cInit(I2CDevice device)
.I2C_OwnAddress1 = 0x00,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_Timing = i2c->overClock ?
0x00500E30 : // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4.
0x00E0257A, // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
//.I2C_Timing = 0x8000050B;
.I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING)
};
I2C_Init(I2Cx, &i2cInit);
I2C_StretchClockCmd(I2Cx, ENABLE);
I2C_Cmd(I2Cx, ENABLE);
}

28
src/main/drivers/bus_spi.h
View File

@ -52,23 +52,23 @@ typedef enum {
} SPIClockDivider_e;
typedef enum SPIDevice {
SPIINVALID = -1,
SPIDEV_1 = 0,
SPIDEV_2,
SPIDEV_3,
SPIDEV_MAX = SPIDEV_3,
SPIINVALID = -1,
SPIDEV_1 = 0,
SPIDEV_2,
SPIDEV_3,
SPIDEV_MAX = SPIDEV_3,
} SPIDevice;
typedef struct SPIDevice_s {
SPI_TypeDef *dev;
ioTag_t nss;
ioTag_t sck;
ioTag_t mosi;
ioTag_t miso;
rccPeriphTag_t rcc;
uint8_t af;
volatile uint16_t errorCount;
bool sdcard;
SPI_TypeDef *dev;
ioTag_t nss;
ioTag_t sck;
ioTag_t mosi;
ioTag_t miso;
rccPeriphTag_t rcc;
uint8_t af;
volatile uint16_t errorCount;
bool sdcard;
} spiDevice_t;
bool spiInit(SPIDevice device);

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

@ -30,8 +30,6 @@
#include "common/maths.h"
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "bus_spi.h"

24
src/main/drivers/compass_ak8975.c
View File

@ -64,7 +64,7 @@ bool ak8975detect(mag_t *mag)
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
return false;
@ -86,24 +86,24 @@ void ak8975Init()
UNUSED(ack);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
delay(20);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
delay(10);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
delay(10);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
delay(10);
// Clear status registers
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
// Trigger first measurement
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
}
#define BIT_STATUS1_REG_DATA_READY (1 << 0)
@ -118,13 +118,13 @@ bool ak8975Read(int16_t *magData)
uint8_t status;
uint8_t buf[6];
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
return false;
}
#if 1 // USE_I2C_SINGLE_BYTE_READS
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
#else
for (uint8_t i = 0; i < 6; i++) {
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
@ -134,7 +134,7 @@ bool ak8975Read(int16_t *magData)
}
#endif
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
if (!ack) {
return false;
}
@ -152,6 +152,6 @@ bool ak8975Read(int16_t *magData)
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
return true;
}

101
src/main/drivers/compass_hmc5883l.c
View File

@ -29,7 +29,8 @@
#include "system.h"
#include "nvic.h"
#include "gpio.h"
#include "io.h"
#include "exti.h"
#include "bus_i2c.h"
#include "light_led.h"
@ -120,14 +121,14 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static const hmc5883Config_t *hmc5883Config = NULL;
void MAG_DATA_READY_EXTI_Handler(void)
#ifdef USE_MAG_DATA_READY_SIGNAL
static IO_t intIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;
void hmc5883_extiHandler(extiCallbackRec_t* cb)
{
if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) {
return;
}
EXTI_ClearITPendingBit(hmc5883Config->exti_line);
UNUSED(cb);
#ifdef DEBUG_MAG_DATA_READY_INTERRUPT
// Measure the delta between calls to the interrupt handler
// currently should be around 65/66 milli seconds / 15hz output rate
@ -143,57 +144,26 @@ void MAG_DATA_READY_EXTI_Handler(void)
lastCalledAt = now;
#endif
}
#endif
static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) {
if (!(hmc5883Config->intTag)) {
return;
}
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
#endif
intIO = IOGetByTag(hmc5883Config->intTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin);
uint8_t status = IORead(intIO);
if (!status) {
return;
}
#endif
registerExtiCallbackHandler(hmc5883Config->exti_irqn, MAG_DATA_READY_EXTI_Handler);
EXTI_ClearITPendingBit(hmc5883Config->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = hmc5883Config->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(intIO, true);
#endif
}
@ -204,7 +174,7 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
hmc5883Config = hmc5883ConfigToUse;
ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H')
return false;
@ -221,35 +191,16 @@ void hmc5883lInit(void)
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator
gpio_config_t gpio;
if (hmc5883Config) {
#ifdef STM32F303
if (hmc5883Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (hmc5883Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = hmc5883Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(hmc5883Config->gpioPort, &gpio);
}
delay(50);
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
@ -267,9 +218,9 @@ void hmc5883lInit(void)
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
@ -291,9 +242,9 @@ void hmc5883lInit(void)
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
if (!bret) { // Something went wrong so get a best guess
@ -309,7 +260,7 @@ bool hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
return false;
}

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

@ -17,20 +17,10 @@
#pragma once
typedef struct hmc5883Config_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
#include "io.h"
uint8_t exti_port_source;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
typedef struct hmc5883Config_s {
ioTag_t intTag;
} hmc5883Config_t;
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);

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

@ -32,7 +32,7 @@ static dmaHandlers_t dmaHandlers;
void dmaNoOpHandler(DMA_Stream_TypeDef *stream)
{
UNUSED(stream);
UNUSED(stream);
}
void DMA1_Stream2_IRQHandler(void)

44
src/main/drivers/gyro_sync.c
View File

@ -4,48 +4,40 @@
* Created on: 3 aug. 2015
* Author: borisb
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "build_config.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "config/runtime_config.h"
#include "config/config.h"
extern gyro_t gyro;
uint32_t targetLooptime;
static uint8_t mpuDividerDrops;
bool getMpuDataStatus(gyro_t *gyro)
bool gyroSyncCheckUpdate(const gyro_t *gyro)
{
bool mpuDataStatus;
if (!gyro->intStatus)
return false;
gyro->intStatus(&mpuDataStatus);
return mpuDataStatus;
return false;
return gyro->intStatus();
}
bool gyroSyncCheckUpdate(void) {
return getMpuDataStatus(&gyro);
}
#define GYRO_LPF_256HZ 0
#define GYRO_LPF_188HZ 1
#define GYRO_LPF_98HZ 2
#define GYRO_LPF_42HZ 3
#define GYRO_LPF_20HZ 4
#define GYRO_LPF_10HZ 5
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
{
int gyroSamplePeriod;
if (!lpf || lpf == 7) {
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
gyroSamplePeriod = 125;
} else {
gyroSamplePeriod = 1000;
@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
// calculate gyro divider and targetLooptime (expected cycleTime)
mpuDividerDrops = gyroSyncDenominator - 1;
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
return targetLooptime;
}
uint8_t gyroMPU6xxxGetDividerDrops(void) {
uint8_t gyroMPU6xxxGetDividerDrops(void)
{
return mpuDividerDrops;
}

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

@ -5,8 +5,7 @@
* Author: borisb
*/
extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
struct gyro_s;
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void);
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

6
src/main/drivers/inverter.c
View File

@ -31,15 +31,15 @@ static const IO_t pin = DEFIO_IO(INVERTER);
void initInverter(void)
{
IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT);
IOConfigGPIO(pin, IOCFG_OUT_PP);
IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT);
IOConfigGPIO(pin, IOCFG_OUT_PP);
inverterSet(false);
}
void inverterSet(bool on)
{
IOWrite(pin, on);
IOWrite(pin, on);
}
#endif

2
src/main/drivers/io.h
View File

@ -52,6 +52,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration
#define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
#define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO
#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
#define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
#define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL)
#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN)
@ -60,6 +61,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration
#define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN)
#define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP)
#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL)
#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IN, GPIO_Speed_25MHz, 0, GPIO_PuPd_UP)
#elif defined(UNIT_TEST)

16
src/main/drivers/light_ws2811strip.c
View File

@ -80,18 +80,9 @@ void setStripColors(const hsvColor_t *colors)
}
}
void ws2811DMAHandler(DMA_Channel_TypeDef *channel) {
if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) {
ws2811LedDataTransferInProgress = 0;
DMA_Cmd(channel, DISABLE);
DMA_ClearFlag(WS2811_DMA_TC_FLAG);
}
}
void ws2811LedStripInit(void)
{
memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE);
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler);
ws2811LedStripHardwareInit();
ws2811UpdateStrip();
}
@ -141,12 +132,11 @@ STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue)
*/
void ws2811UpdateStrip(void)
{
static uint32_t waitCounter = 0;
static rgbColor24bpp_t *rgb24;
// wait until previous transfer completes
while(ws2811LedDataTransferInProgress) {
waitCounter++;
// don't wait - risk of infinite block, just get an update next time round
if (ws2811LedDataTransferInProgress) {
return;
}
dmaBufferOffset = 0; // reset buffer memory index

52
src/main/drivers/light_ws2811strip_stm32f10x.c
View File

@ -23,35 +23,41 @@
#include "common/color.h"
#include "drivers/light_ws2811strip.h"
#include "nvic.h"
#include "io.h"
#include "dma.h"
#include "timer.h"
#ifdef LED_STRIP
static IO_t ws2811IO = IO_NONE;
bool ws2811Initialised = false;
void ws2811DMAHandler(DMA_Channel_TypeDef *channel)
{
if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) {
ws2811LedDataTransferInProgress = 0;
DMA_Cmd(channel, DISABLE);
DMA_ClearFlag(WS2811_DMA_TC_FLAG);
}
}
void ws2811LedStripHardwareInit(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
uint16_t prescalerValue;
#ifdef CC3D
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
#else
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
/* GPIOA Configuration: TIM3 Channel 1 as alternate function push-pull */
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
#endif
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler);
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */
IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT);
IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP));
RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE);
/* Compute the prescaler value */
prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
/* Time base configuration */
@ -108,16 +114,20 @@ void ws2811LedStripHardwareInit(void)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();
}
void ws2811LedStripDMAEnable(void)
{
if (!ws2811Initialised)
return;
DMA_SetCurrDataCounter(DMA1_Channel6, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
DMA_Cmd(DMA1_Channel6, ENABLE);
}
#endif

61
src/main/drivers/light_ws2811strip_stm32f30x.c
View File

@ -20,51 +20,54 @@
#include <platform.h>
#include "gpio.h"
#include "io.h"
#include "nvic.h"
#include "common/color.h"
#include "drivers/light_ws2811strip.h"
#include "dma.h"
#include "rcc.h"
#include "timer.h"
#ifndef WS2811_GPIO
#define WS2811_GPIO GPIOB
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB
#define WS2811_GPIO_AF GPIO_AF_1
#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1
#define WS2811_PIN_SOURCE GPIO_PinSource8
#ifdef LED_STRIP
#ifndef WS2811_PIN
#define WS2811_PIN PB8 // TIM16_CH1
#define WS2811_TIMER TIM16
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16
#define WS2811_DMA_CHANNEL DMA1_Channel3
#define WS2811_IRQ DMA1_Channel3_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER
#endif
static IO_t ws2811IO = IO_NONE;
bool ws2811Initialised = false;
void ws2811DMAHandler(DMA_Channel_TypeDef *channel)
{
if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) {
ws2811LedDataTransferInProgress = 0;
DMA_Cmd(channel, DISABLE);
DMA_ClearFlag(WS2811_DMA_TC_FLAG);
}
}
void ws2811LedStripHardwareInit(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
uint16_t prescalerValue;
RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE);
GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, WS2811_GPIO_AF);
/* Configuration alternate function push-pull */
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = WS2811_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(WS2811_GPIO, &GPIO_InitStructure);
RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE);
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler);
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */
IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT);
IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER));
RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE);
/* Compute the prescaler value */
prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
@ -122,16 +125,20 @@ void ws2811LedStripHardwareInit(void)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();
}
void ws2811LedStripDMAEnable(void)
{
if (!ws2811Initialised)
return;
DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred
TIM_SetCounter(WS2811_TIMER, 0);
TIM_Cmd(WS2811_TIMER, ENABLE);
DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE);
}
#endif

149
src/main/drivers/light_ws2811strip_stm32f4xx.c
View File

@ -22,64 +22,121 @@
#include "common/color.h"
#include "light_ws2811strip.h"
#include "dma.h"
#include "nvic.h"
#include "io.h"
#include "system.h"
#include "rcc.h"
#include "timer.h"
#ifdef LED_STRIP
#if !defined(WS2811_PIN)
#define WS2811_PIN PA0
#define WS2811_TIMER TIM5
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
#define WS2811_DMA_FLAG DMA_FLAG_TCIF2
#define WS2811_DMA_IT DMA_IT_TCIF2
#define WS2811_DMA_CHANNEL DMA_Channel_6
#define WS2811_DMA_IRQ DMA1_Stream2_IRQn
#define WS2811_TIMER_CHANNEL TIM_Channel_1
#endif
static IO_t ws2811IO = IO_NONE;
static uint16_t timDMASource = 0;
bool ws2811Initialised = false;
void ws2811DMAHandler(DMA_Stream_TypeDef *stream)
{
if (DMA_GetFlagStatus(stream, WS2811_DMA_FLAG)) {
ws2811LedDataTransferInProgress = 0;
DMA_ClearITPendingBit(stream, WS2811_DMA_IT);
DMA_Cmd(stream, DISABLE);
TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE);
}
}
void ws2811LedStripHardwareInit(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
uint16_t prescalerValue;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE);
ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM5);
IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT);
IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER));
// Stop timer
TIM_Cmd(TIM5, DISABLE);
TIM_Cmd(WS2811_TIMER, DISABLE);
/* Compute the prescaler value */
prescalerValue = (uint16_t) (SystemCoreClock / 2 / 84000000) - 1;
prescalerValue = (uint16_t)(SystemCoreClock / 2 / 84000000) - 1;
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = 104; // 800kHz
TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure);
/* PWM1 Mode configuration: Channel1 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(TIM5, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);
uint32_t channelAddress = 0;
switch (WS2811_TIMER_CHANNEL) {
case TIM_Channel_1:
TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure);
timDMASource = TIM_DMA_CC1;
channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure);
timDMASource = TIM_DMA_CC2;
channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure);
timDMASource = TIM_DMA_CC3;
channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure);
timDMASource = TIM_DMA_CC4;
channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable);
break;
}
TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE);
TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE);
TIM_Cmd(TIM5, ENABLE);
TIM_CCxCmd(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_CCx_Enable);
TIM_Cmd(WS2811_TIMER, ENABLE);
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler);
/* configure DMA */
/* DMA1 Channel Config */
DMA_Cmd(DMA1_Stream2, DISABLE); // disable DMA channel 6
DMA_DeInit(DMA1_Stream2);
DMA_Cmd(WS2811_DMA_STREAM, DISABLE);
DMA_DeInit(WS2811_DMA_STREAM);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_Channel = DMA_Channel_6;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(TIM5->CCR1);
DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL;
DMA_InitStructure.DMA_PeripheralBaseAddr = channelAddress;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE;
@ -93,38 +150,36 @@ void ws2811LedStripHardwareInit(void)
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA1_Stream2, &DMA_InitStructure);
DMA_ITConfig(DMA1_Stream2, DMA_IT_TC, ENABLE);
DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2); // clear DMA1 Channel 6 transfer complete flag
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure);
DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE);
DMA_ClearITPendingBit(WS2811_DMA_STREAM, WS2811_DMA_IT);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream2_IRQn;
NVIC_InitStructure.NVIC_IRQChannel = WS2811_DMA_IRQ;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();
}
void DMA1_Stream2_IRQHandler(void)
{
if (DMA_GetFlagStatus(DMA1_Stream2, DMA_FLAG_TCIF2)) {
ws2811LedDataTransferInProgress = 0;
DMA_Cmd(DMA1_Stream2, DISABLE);
TIM_DMACmd(TIM5, TIM_DMA_CC1, DISABLE);
DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2);
}
}
void ws2811LedStripDMAEnable(void)
{
DMA_SetCurrDataCounter(DMA1_Stream2, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred
TIM_SetCounter(TIM5, 0);
DMA_Cmd(DMA1_Stream2, ENABLE);
TIM_DMACmd(TIM5, TIM_DMA_CC1, ENABLE);
if (!ws2811Initialised)
return;
DMA_SetCurrDataCounter(WS2811_DMA_STREAM, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred
TIM_SetCounter(WS2811_TIMER, 0);
DMA_Cmd(WS2811_DMA_STREAM, ENABLE);
TIM_DMACmd(WS2811_TIMER, timDMASource, ENABLE);
}
#endif
#endif

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

@ -157,33 +157,33 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
continue;
#endif
#ifdef LED_STRIP_TIMER
#ifdef WS2811_TIMER
// skip LED Strip output
if (init->useLEDStrip) {
if (timerHardwarePtr->tim == LED_STRIP_TIMER)
if (timerHardwarePtr->tim == WS2811_TIMER)
continue;
#if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE)
if (CheckGPIOPinSource(timerHardwarePtr->tag, WS2811_GPIO, WS2811_PIN_SOURCE))
#if defined(STM32F303xC) && defined(WS2811_PIN)
if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN))
continue;
#endif
}
#endif
#ifdef VBAT_ADC_GPIO
if (init->useVbat && CheckGPIOPin(timerHardwarePtr->tag, VBAT_ADC_GPIO, VBAT_ADC_GPIO_PIN)) {
#ifdef VBAT_ADC_PIN
if (init->useVbat && timerHardwarePtr->tag == IO_TAG(VBAT_ADC_PIN)) {
continue;
}
#endif
#ifdef RSSI_ADC_GPIO
if (init->useRSSIADC && CheckGPIOPin(timerHardwarePtr->tag, RSSI_ADC_GPIO, RSSI_ADC_GPIO_PIN)) {
if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) {
continue;
}
#endif
#ifdef CURRENT_METER_ADC_GPIO
if (init->useCurrentMeterADC && CheckGPIOPin(timerHardwarePtr->tag, CURRENT_METER_ADC_GPIO, CURRENT_METER_ADC_GPIO_PIN)) {
if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) {
continue;
}
#endif
@ -274,7 +274,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
}
if (init->useChannelForwarding && !init->airplane) {
#if defined(NAZE) && defined(LED_STRIP_TIMER)
#if defined(NAZE) && defined(WS2811_TIMER)
// if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14
if (init->useLEDStrip) {
if (timerIndex >= PWM13 && timerIndex <= PWM14) {
@ -306,7 +306,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (type == MAP_TO_PPM_INPUT) {
#if defined(SPARKY) || defined(ALIENFLIGHTF3)
if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
}
#endif

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

@ -78,23 +78,21 @@ typedef struct drv_pwm_config_s {
} drv_pwm_config_t;
enum {
MAP_TO_PPM_INPUT = 1,
MAP_TO_PPM_INPUT = 1,
MAP_TO_PWM_INPUT,
MAP_TO_MOTOR_OUTPUT,
MAP_TO_SERVO_OUTPUT,
};
typedef enum {
PWM_PF_NONE = 0,
PWM_PF_MOTOR = (1 << 0),
PWM_PF_SERVO = (1 << 1),
PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4)
PWM_PF_NONE = 0,
PWM_PF_MOTOR = (1 << 0),
PWM_PF_SERVO = (1 << 1),
PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4)
} pwmPortFlags_e;
enum {PWM_INVERTED = 1};
typedef struct pwmPortConfiguration_s {
uint8_t index;
pwmPortFlags_e flags;

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

@ -97,9 +97,9 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
configTimeBase(timerHardware->tim, period, mhz);
pwmGPIOConfig(timerHardware->tag, IOCFG_AF_PP);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->outputInverted);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED);
if (timerHardware->outputEnable) {
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
}
TIM_Cmd(timerHardware->tim, ENABLE);

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

@ -339,8 +339,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode)
{
IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT);
IOConfigGPIO(IOGetByTag(pin), mode);
IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT);
IOConfigGPIO(IOGetByTag(pin), mode);
}
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)

8
src/main/drivers/rcc.c
View File

@ -7,7 +7,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
int tag = periphTag >> 5;
uint32_t mask = 1 << (periphTag & 0x1f);
switch (tag) {
#if defined(STM32F303xC)
#if defined(STM32F3) || defined(STM32F1)
case RCC_AHB:
RCC_AHBPeriphClockCmd(mask, NewState);
break;
@ -18,7 +18,7 @@ void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
case RCC_APB1:
RCC_APB1PeriphClockCmd(mask, NewState);
break;
#if defined(STM32F40_41xxx) || defined(STM32F411xE)
#if defined(STM32F4)
case RCC_AHB1:
RCC_AHB1PeriphClockCmd(mask, NewState);
break;
@ -31,7 +31,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
int tag = periphTag >> 5;
uint32_t mask = 1 << (periphTag & 0x1f);
switch (tag) {
#if defined(STM32F303xC)
#if defined(STM32F3) || defined(STM32F10X_CL)
case RCC_AHB:
RCC_AHBPeriphResetCmd(mask, NewState);
break;
@ -42,7 +42,7 @@ void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
case RCC_APB1:
RCC_APB1PeriphResetCmd(mask, NewState);
break;
#if defined(STM32F40_41xxx) || defined(STM32F411xE)
#if defined(STM32F4)
case RCC_AHB1:
RCC_AHB1PeriphResetCmd(mask, NewState);
break;

2
src/main/drivers/sensor.h
View File

@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init proto
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready
typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready

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

@ -56,7 +56,7 @@ void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count)
}
}
uint8_t serialRxBytesWaiting(serialPort_t *instance)
uint32_t serialRxBytesWaiting(serialPort_t *instance)
{
return instance->vTable->serialTotalRxWaiting(instance);
}

2
src/main/drivers/serial.h
View File

@ -81,7 +81,7 @@ struct serialPortVTable {
};
void serialWrite(serialPort_t *instance, uint8_t ch);
uint8_t serialRxBytesWaiting(serialPort_t *instance);
uint32_t serialRxBytesWaiting(serialPort_t *instance);
uint8_t serialTxBytesFree(serialPort_t *instance);
void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count);
uint8_t serialRead(serialPort_t *instance);

4
src/main/drivers/serial_softserial.c
View File

@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state)
if (state) {
IOHi(softSerial->txIO);
} else {
IOLo(softSerial->txIO);
IOLo(softSerial->txIO);
}
}
@ -408,7 +408,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
}
}
uint8_t softSerialRxBytesWaiting(serialPort_t *instance)
uint32_t softSerialRxBytesWaiting(serialPort_t *instance)
{
if ((instance->mode & MODE_RX) == 0) {
return 0;

2
src/main/drivers/serial_softserial.h
View File

@ -28,7 +28,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb
// serialPort API
void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
uint8_t softSerialRxBytesWaiting(serialPort_t *instance);
uint32_t softSerialRxBytesWaiting(serialPort_t *instance);
uint8_t softSerialTxBytesFree(serialPort_t *instance);
uint8_t softSerialReadByte(serialPort_t *instance);
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);

12
src/main/drivers/serial_uart.h
View File

@ -40,13 +40,13 @@ typedef struct {
serialPort_t port;
#ifdef STM32F4
DMA_Stream_TypeDef *rxDMAStream;
DMA_Stream_TypeDef *txDMAStream;
uint32_t rxDMAChannel;
uint32_t txDMAChannel;
DMA_Stream_TypeDef *rxDMAStream;
DMA_Stream_TypeDef *txDMAStream;
uint32_t rxDMAChannel;
uint32_t txDMAChannel;
#else
DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel;
DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel;
#endif
uint32_t rxDMAIrq;

188
src/main/drivers/serial_uart_stm32f4xx.c
View File

@ -365,22 +365,22 @@ void DMA2_Stream7_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
}
handleUsartTxDma(s);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
}
}
// USART1 Rx/Tx IRQ Handler
@ -402,30 +402,30 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio
void DMA1_Stream6_IRQHandler(void)
{
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
}
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
}
}
void USART2_IRQHandler(void)
{
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
usartIrqHandler(s);
usartIrqHandler(s);
}
#endif
@ -442,22 +442,22 @@ void DMA1_Stream3_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF3))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3);
}
handleUsartTxDma(s);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3);
}
}
void USART3_IRQHandler(void)
@ -480,22 +480,22 @@ void DMA1_Stream4_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF4))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4);
}
handleUsartTxDma(s);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4);
}
}
void UART4_IRQHandler(void)
@ -518,22 +518,22 @@ void DMA1_Stream7_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
}
handleUsartTxDma(s);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
}
}
void UART5_IRQHandler(void)
@ -556,22 +556,22 @@ void DMA2_Stream6_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port);
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
}
handleUsartTxDma(s);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
}
handleUsartTxDma(s);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
}
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
{
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
}
}
void USART6_IRQHandler(void)

10
src/main/drivers/serial_usb_vcp.c
View File

@ -183,12 +183,12 @@ serialPort_t *usbVcpOpen(void)
#ifdef STM32F4
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_IO);
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_IO);
USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
#else
Set_System();
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
Set_System();
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
#endif
s = &vcpPort;

6
src/main/drivers/sonar_hcsr04.c
View File

@ -92,9 +92,9 @@ void hcsr04_init(sonarRange_t *sonarRange)
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
#ifdef USE_EXTI
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
EXTIEnable(echoIO, true);
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
EXTIEnable(echoIO, true);
#endif
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()

22
src/main/drivers/sound_beeper.c
View File

@ -39,31 +39,31 @@ static bool beeperInverted = false;
void systemBeep(bool onoff)
{
#ifndef BEEPER
UNUSED(onoff);
UNUSED(onoff);
#else
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
#endif
}
void systemBeepToggle(void)
{
#ifdef BEEPER
IOToggle(beeperIO);
IOToggle(beeperIO);
#endif
}
void beeperInit(const beeperConfig_t *config)
{
#ifndef BEEPER
UNUSED(config);
UNUSED(config);
#else
beeperIO = IOGetByTag(config->ioTag);
beeperInverted = config->isInverted;
beeperIO = IOGetByTag(config->ioTag);
beeperInverted = config->isInverted;
if (beeperIO) {
IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT);
IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP);
}
systemBeep(false);
if (beeperIO) {
IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT);
IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP);
}
systemBeep(false);
#endif
}

6
src/main/drivers/sound_beeper.h
View File

@ -30,9 +30,9 @@
#endif
typedef struct beeperConfig_s {
ioTag_t ioTag;
unsigned isInverted : 1;
unsigned isOD : 1;
ioTag_t ioTag;
unsigned isInverted : 1;
unsigned isOD : 1;
} beeperConfig_t;
void systemBeep(bool on);

15
src/main/drivers/system.h
View File

@ -25,13 +25,13 @@ uint32_t micros(void);
uint32_t millis(void);
typedef enum {
FAILURE_DEVELOPER = 0,
FAILURE_MISSING_ACC,
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
FAILURE_DEVELOPER = 0,
FAILURE_MISSING_ACC,
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;
// failure
@ -42,6 +42,7 @@ void systemReset(void);
void systemResetToBootloader(void);
bool isMPUSoftReset(void);
void cycleCounterInit(void);
void checkForBootLoaderRequest(void);
void enableGPIOPowerUsageAndNoiseReductions(void);
// current crystal frequency - 8 or 12MHz

8
src/main/drivers/system_stm32f10x.c
View File

@ -37,7 +37,8 @@ void systemReset(void)
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void systemResetToBootloader(void) {
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC
@ -68,6 +69,8 @@ bool isMPUSoftReset(void)
void systemInit(void)
{
checkForBootLoaderRequest();
SetSysClock(false);
#ifdef CC3D
@ -110,3 +113,6 @@ void systemInit(void)
SysTick_Config(SystemCoreClock / 1000);
}
void checkForBootLoaderRequest(void)
{
}

9
src/main/drivers/system_stm32f30x.c
View File

@ -35,7 +35,8 @@ void systemReset(void)
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void systemResetToBootloader(void) {
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC
@ -82,6 +83,8 @@ bool isMPUSoftReset(void)
void systemInit(void)
{
checkForBootLoaderRequest();
// Enable FPU
SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2));
SetSysClock();
@ -102,3 +105,7 @@ void systemInit(void)
// SysTick
SysTick_Config(SystemCoreClock / 1000);
}
void checkForBootLoaderRequest(void)
{
}

51
src/main/drivers/system_stm32f4xx.c
View File

@ -45,8 +45,8 @@ void systemReset(void)
if (mpuConfiguration.reset)
mpuConfiguration.reset();
__disable_irq();
NVIC_SystemReset();
__disable_irq();
NVIC_SystemReset();
}
void systemResetToBootloader(void)
@ -54,10 +54,10 @@ void systemResetToBootloader(void)
if (mpuConfiguration.reset)
mpuConfiguration.reset();
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
__disable_irq();
NVIC_SystemReset();
__disable_irq();
NVIC_SystemReset();
}
void enableGPIOPowerUsageAndNoiseReductions(void)
@ -82,7 +82,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void)
RCC_AHB1Periph_BKPSRAM |
RCC_AHB1Periph_DMA1 |
RCC_AHB1Periph_DMA2 |
0, ENABLE
0, ENABLE
);
RCC_AHB2PeriphClockCmd(0, ENABLE);
@ -169,28 +169,45 @@ bool isMPUSoftReset(void)
void systemInit(void)
{
checkForBootLoaderRequest();
SetSysClock();
// Configure NVIC preempt/priority groups
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
// cache RCC->CSR value to use it in isMPUSoftreset() and others
cachedRccCsrValue = RCC->CSR;
cachedRccCsrValue = RCC->CSR;
/* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
extern void *isr_vector_table_base;
NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE);
extern void *isr_vector_table_base;
NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE);
RCC_ClearFlag();
RCC_ClearFlag();
enableGPIOPowerUsageAndNoiseReductions();
enableGPIOPowerUsageAndNoiseReductions();
// Init cycle counter
cycleCounterInit();
cycleCounterInit();
memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs));
// SysTick
SysTick_Config(SystemCoreClock / 1000);
memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs));
// SysTick
SysTick_Config(SystemCoreClock / 1000);
}
void(*bootJump)(void);
void checkForBootLoaderRequest(void)
{
if (*((uint32_t *)0x2001FFFC) == 0xDEADBEEF) {
*((uint32_t *)0x2001FFFC) = 0x0;
__enable_irq();
__set_MSP(0x20001000);
bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004));
bootJump();
while (1);
}
}

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

@ -17,7 +17,6 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
@ -26,7 +25,6 @@
#include "nvic.h"
#include "gpio.h"
#include "gpio.h"
#include "rcc.h"
#include "system.h"
@ -148,11 +146,23 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim)
for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
if (timerDefinitions[i].TIMx == tim) {
return timerDefinitions[i].rcc;
}
}
}
return 0;
}
#if defined(STM32F3) || defined(STM32F4)
uint8_t timerGPIOAF(TIM_TypeDef *tim)
{
for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
if (timerDefinitions[i].TIMx == tim) {
return timerDefinitions[i].alternateFunction;
}
}
return 0;
}
#endif
void timerNVICConfigure(uint8_t irq)
{
NVIC_InitTypeDef NVIC_InitStructure;
@ -190,7 +200,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
#else
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1;
#endif
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
@ -458,7 +468,7 @@ void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHig
if(outEnable) {
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
if (timHw->outputInverted) {
if (timHw->output & TIMER_OUTPUT_INVERTED) {
stateHigh = !stateHigh;
}
TIM_OCInitStructure.TIM_OCPolarity = stateHigh ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
@ -660,7 +670,7 @@ void timerInit(void)
IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction);
}
#endif
// initialize timer channel structures
for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
timerChannelInfo[i].type = TYPE_FREE;

17
src/main/drivers/timer.h
View File

@ -69,6 +69,9 @@ typedef struct timerOvrHandlerRec_s {
typedef struct timerDef_s {
TIM_TypeDef *TIMx;
rccPeriphTag_t rcc;
#if defined(STM32F3) || defined(STM32F4)
uint8_t alternateFunction;
#endif
} timerDef_t;
typedef struct timerHardware_s {
@ -76,14 +79,18 @@ typedef struct timerHardware_s {
ioTag_t tag;
uint8_t channel;
uint8_t irq;
uint8_t outputEnable;
uint8_t output;
ioConfig_t ioMode;
#if defined(STM32F3) || defined(STM32F4)
uint8_t alternateFunction;
#endif
uint8_t outputInverted;
} timerHardware_t;
enum {
TIMER_OUTPUT_ENABLED = 0x01,
TIMER_OUTPUT_INVERTED = 0x02
};
#ifdef STM32F1
#if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL)
#define HARDWARE_TIMER_DEFINITION_COUNT 14
@ -147,4 +154,8 @@ void timerForceOverflow(TIM_TypeDef *tim);
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
#if defined(STM32F3) || defined(STM32F4)
uint8_t timerGPIOAF(TIM_TypeDef *tim);
#endif

43
src/main/drivers/timer_stm32f10x.c
View File

@ -57,34 +57,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
{
uint32_t tmp = 0;
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3))
{
tmp += (TIM_Channel>>1);
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
}
else
{
tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint16_t)(TIM_Channel - (uint16_t)4) >> (uint16_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}

60
src/main/drivers/timer_stm32f30x.c
View File

@ -10,16 +10,16 @@
#include "timer.h"
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1) },
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2) },
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3) },
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4) },
{ .TIMx = TIM6, .rcc = RCC_APB1(TIM6) },
{ .TIMx = TIM7, .rcc = RCC_APB1(TIM7) },
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8) },
{ .TIMx = TIM15, .rcc = RCC_APB2(TIM15) },
{ .TIMx = TIM16, .rcc = RCC_APB2(TIM16) },
{ .TIMx = TIM17, .rcc = RCC_APB2(TIM17) },
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_6 },
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_1 },
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_2 },
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_10 },
{ .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 },
{ .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 },
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_5 },
{ .TIMx = TIM15, .rcc = RCC_APB2(TIM15), GPIO_AF_9 },
{ .TIMx = TIM16, .rcc = RCC_APB2(TIM16), GPIO_AF_1 },
{ .TIMx = TIM17, .rcc = RCC_APB2(TIM17), GPIO_AF_1 },
};
@ -58,31 +58,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode)
{
uint32_t tmp = 0;
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST1_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
/* Check the parameters */
assert_param(IS_TIM_LIST1_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_OFFSET;
tmp = (uint32_t) TIMx;
tmp += CCMR_OFFSET;
if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC13M_MASK;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC13M_MASK;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint32_t)(TIM_Channel - (uint32_t)4) >> (uint32_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC24M_MASK;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC24M_MASK;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8);
}
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8);
}
}

71
src/main/drivers/timer_stm32f4xx.c
View File

@ -35,53 +35,50 @@
#define CCMR_Offset ((uint16_t)0x0018)
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1) },
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2) },
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3) },
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4) },
{ .TIMx = TIM5, .rcc = RCC_APB1(TIM5) },
{ .TIMx = TIM6, .rcc = RCC_APB1(TIM6) },
{ .TIMx = TIM7, .rcc = RCC_APB1(TIM7) },
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8) },
{ .TIMx = TIM9, .rcc = RCC_APB2(TIM9) },
{ .TIMx = TIM10, .rcc = RCC_APB2(TIM10) },
{ .TIMx = TIM11, .rcc = RCC_APB2(TIM11) },
{ .TIMx = TIM12, .rcc = RCC_APB1(TIM12) },
{ .TIMx = TIM13, .rcc = RCC_APB1(TIM13) },
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14) },
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 },
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 },
{ .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 },
{ .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 },
{ .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 },
{ .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 },
{ .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 },
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 },
{ .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 },
{ .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 },
{ .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 },
{ .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 },
{ .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF_TIM13 },
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF_TIM14 },
};
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
{
uint32_t tmp = 0;
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3))
{
tmp += (TIM_Channel>>1);
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
}
else
{
tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}

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

@ -28,7 +28,6 @@
#include "debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/system.h"
#include "drivers/sensor.h"

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

@ -36,7 +36,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "rx/rx.h"
@ -947,7 +946,7 @@ void filterServos(void)
#ifdef USE_SERVOS
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
@ -956,11 +955,11 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime);
biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}

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

@ -29,7 +29,6 @@
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/gyro_sync.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we
void setTargetPidLooptime(uint8_t pidProcessDenom)
{
targetPidLooptime = targetLooptime * pidProcessDenom;
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
@ -113,8 +112,8 @@ float getdT (void)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
#ifndef SKIP_PID_LUXFLOAT
static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
@ -206,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
@ -231,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
delta *= (1.0f / getdT());
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
@ -345,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
@ -370,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

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

@ -440,7 +440,7 @@ void updateLedCount(void)
}
}
void reevalulateLedConfig(void)
void reevaluateLedConfig(void)
{
updateLedCount();
determineLedStripDimensions();
@ -534,7 +534,7 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config)
memset(ledConfig, 0, sizeof(ledConfig_t));
}
reevalulateLedConfig();
reevaluateLedConfig();
return ok;
}
@ -1095,7 +1095,7 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t));
memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig));
reevalulateLedConfig();
reevaluateLedConfig();
}
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse)
@ -1107,7 +1107,7 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse)
void ledStripEnable(void)
{
reevalulateLedConfig();
reevaluateLedConfig();
ledStripInitialised = true;
ws2811LedStripInit();

2
src/main/io/ledstrip.h
View File

@ -94,4 +94,4 @@ bool parseColor(uint8_t index, const char *colorConfig);
void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount);
void ledStripEnable(void);
void reevalulateLedConfig(void);
void reevaluateLedConfig(void);

1
src/main/io/osd.c
View File

@ -52,7 +52,6 @@
#include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/gyro_sync.h"
#include "drivers/usb_io.h"
#include "drivers/transponder_ir.h"
#include "drivers/sdcard.h"

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

@ -34,7 +34,6 @@
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
@ -210,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroSetCalibrationCycles(calculateCalibratingCycles());
gyroSetCalibrationCycles();
#ifdef GPS
if (feature(FEATURE_GPS)) {

25
src/main/io/serial_4way.c
View File

@ -44,15 +44,15 @@
#define USE_TXRX_LED
#if defined(USE_TXRX_LED) && defined(LED0)
# define RX_LED_OFF LED0_OFF
# define RX_LED_ON LED0_ON
# ifdef LED1
# define TX_LED_OFF LED1_OFF
# define TX_LED_ON LED1_ON
# else
# define TX_LED_OFF LED0_OFF
# define TX_LED_ON LED0_ON
# endif
#define RX_LED_OFF LED0_OFF
#define RX_LED_ON LED0_ON
#ifdef LED1
#define TX_LED_OFF LED1_OFF
#define TX_LED_ON LED1_ON
#else
#define TX_LED_OFF LED0_OFF
#define TX_LED_ON LED0_ON
#endif
#else
# define RX_LED_OFF do {} while(0)
# define RX_LED_ON do {} while(0)
@ -330,6 +330,7 @@ void esc4wayProcess(serialPort_t *serial)
while(!esc4wayExitRequested) {
// restart looking for new sequence from host
crcIn = 0;
uint8_t esc = readByteCrc();
if(esc != cmd_Local_Escape)
continue; // wait for sync character
@ -348,7 +349,6 @@ void esc4wayProcess(serialPort_t *serial)
paramBuf[i] = readByteCrc();
readByteCrc(); readByteCrc(); // update input CRC
RX_LED_OFF;
outLen = 0; // output handling code will send single zero byte if necessary
replyAck = esc4wayAck_OK;
@ -356,8 +356,10 @@ void esc4wayProcess(serialPort_t *serial)
if(crcIn != 0) // CRC of correct message == 0
replyAck = esc4wayAck_I_INVALID_CRC;
TX_LED_ON;
if (replyAck == esc4wayAck_OK)
replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen);
RX_LED_OFF;
// send single '\0' byte is output when length is zero (len ==0 -> 256 bytes)
if(!outLen) {
@ -366,14 +368,13 @@ void esc4wayProcess(serialPort_t *serial)
}
crcOut = 0;
TX_LED_ON;
serialBeginWrite(port);
writeByteCrc(cmd_Remote_Escape);
writeByteCrc(command);
writeByteCrc(addr >> 8);
writeByteCrc(addr & 0xff);
writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B
for(int i = 0; i < outLen; i++)
for(int i = 0; i < outLen % 0x100; i++)
writeByteCrc(paramBuf[i]);
writeByteCrc(replyAck);
writeByte(crcOut >> 8);

2
src/main/io/serial_4way_avrootloader.c
View File

@ -238,7 +238,7 @@ void BL_SendCMDRunRestartBootloader(void)
static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr
{
// skip if adr == 0xFFFF
if((pMem->addr == 0xffff))
if(pMem->addr == 0xffff)
return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff };
BL_SendBuf(sCMD, sizeof(sCMD), true);

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

@ -48,7 +48,6 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/sdcard.h"
#include "drivers/gyro_sync.h"
#include "drivers/buf_writer.h"
@ -113,6 +112,7 @@ static void cliRxFail(char *cmdline);
static void cliAdjustmentRange(char *cmdline);
static void cliMotorMix(char *cmdline);
static void cliDefaults(char *cmdline);
void cliDfu(char *cmdLine);
static void cliDump(char *cmdLine);
void cliDumpProfile(uint8_t profileIndex);
void cliDumpRateProfile(uint8_t rateProfileIndex) ;
@ -123,6 +123,7 @@ static void cliPlaySound(char *cmdline);
static void cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
static void cliReboot(void);
static void cliRebootEx(bool bootLoader);
static void cliSave(char *cmdline);
static void cliSerial(char *cmdline);
#ifndef SKIP_SERIAL_PASSTHROUGH
@ -264,8 +265,8 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
#endif
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
CLI_COMMAND_DEF("dump", "dump configuration",
"[master|profile]", cliDump),
CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump),
CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
CLI_COMMAND_DEF("feature", "configure features",
"list\r\n"
@ -2565,10 +2566,19 @@ static void cliRateProfile(char *cmdline) {
static void cliReboot(void)
{
cliPrint("\r\nRebooting");
cliRebootEx(false);
}
static void cliRebootEx(bool bootLoader)
{
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
waitForSerialPortToFinishTransmitting(cliPort);
stopMotors();
if (bootLoader) {
systemResetToBootloader();
return;
}
systemReset();
}
@ -3108,6 +3118,13 @@ static void cliResource(char *cmdline)
}
}
void cliDfu(char *cmdLine)
{
UNUSED(cmdLine);
cliPrint("\r\nRestarting in DFU mode");
cliRebootEx(true);
}
void cliInit(serialConfig_t *serialConfig)
{
UNUSED(serialConfig);

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

@ -40,7 +40,6 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/gyro_sync.h"
#include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
#include "drivers/max7456.h"
@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
uint16_t gyroSampleRate = 1000;
uint8_t maxDivider = 1;
if (looptime != targetLooptime || looptime == 0) {
if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes
if (looptime != gyro.targetLooptime || looptime == 0) {
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
#ifdef STM32F303xC
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_LOOP_TIME:
headSerialReply(2);
serialize16((uint16_t)targetLooptime);
serialize16((uint16_t)gyro.targetLooptime);
break;
case MSP_RC_TUNING:
headSerialReply(11);
@ -1808,7 +1807,7 @@ static bool processInCommand(void)
ledConfig->color = read8();
reevalulateLedConfig();
reevaluateLedConfig();
}
break;
#endif

30
src/main/main.c
View File

@ -17,11 +17,11 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
@ -49,7 +49,6 @@
#include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/gyro_sync.h"
#include "drivers/sdcard.h"
#include "drivers/usb_io.h"
#include "drivers/transponder_ir.h"
@ -153,9 +152,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING;
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
@ -260,6 +256,7 @@ void init(void)
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
@ -333,6 +330,7 @@ void init(void)
#endif
pwmRxInit(masterConfig.inputFilteringMode);
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
@ -488,12 +486,12 @@ void init(void)
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -504,7 +502,7 @@ void init(void)
LED0_OFF;
LED2_OFF;
for (i = 0; i < 10; i++) {
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
@ -626,7 +624,7 @@ void init(void)
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(calculateCalibratingCycles());
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
@ -688,18 +686,18 @@ void processLoopback(void) {
#define processLoopback()
#endif
void main_init(void)
void main_init(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, targetLooptime);
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if(sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future
switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
case(500):
case(375):
case(250):

12
src/main/mw.c
View File

@ -185,7 +185,7 @@ float calculateRate(int axis, int16_t rc) {
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
}
void processRcCommand(void)
@ -377,7 +377,7 @@ void mwArm(void)
static bool firstArmingCalibrationWasCompleted;
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles(calculateCalibratingCycles());
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}
@ -698,6 +698,8 @@ void subTaskMainSubprocesses(void) {
#endif
#if defined(BARO) || defined(SONAR)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold(&masterConfig.airplaneConfig);
@ -776,7 +778,7 @@ void subTaskMotorUpdate(void)
uint8_t setPidUpdateCountDown(void) {
if (masterConfig.gyro_soft_lpf_hz) {
return masterConfig.pid_process_denom - 1;
return masterConfig.pid_process_denom - 1;
} else {
return 1;
}
@ -800,7 +802,7 @@ void taskMainPidLoopCheck(void)
const uint32_t startTime = micros();
while (true) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
static uint8_t pidUpdateCountdown;
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
@ -878,8 +880,10 @@ void taskUpdateRxMain(void)
processRx();
isRXDataNew = true;
#if !defined(BARO) && !defined(SONAR)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
#endif
updateLEDs();
#ifdef BARO

1
src/main/rx/rx.c
View File

@ -41,7 +41,6 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "rx/pwm.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"

4
src/main/scheduler/scheduler_tasks.c
View File

@ -168,8 +168,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_LEDSTRIP] = {
.taskName = "LEDSTRIP",
.taskFunc = taskLedStrip,
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_IDLE,
.desiredPeriod = 1000000 / 10, // 10 Hz
.staticPriority = TASK_PRIORITY_LOW,
},
#endif

22
src/main/sensors/acceleration.c
View File

@ -28,7 +28,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
@ -45,10 +44,9 @@ acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationArmed;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
@ -56,7 +54,7 @@ extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static float accLpfCutHz = 0;
static biquad_t accFilterState[XYZ_AXIS_COUNT];
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -88,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
int axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
@ -180,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
int axis;
int16_t accADCRaw[XYZ_AXIS_COUNT];
if (!acc.read(accADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis];
}
@ -195,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
}

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

@ -29,10 +29,9 @@ typedef enum {
ACC_MPU6000 = 7,
ACC_MPU6500 = 8,
ACC_FAKE = 9,
ACC_MAX = ACC_FAKE
} accelerationSensor_e;
#define ACC_MAX ACC_FAKE
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint32_t accTargetLooptime;

7
src/main/sensors/barometer.c
View File

@ -20,6 +20,10 @@
#include <math.h>
#include "platform.h"
int32_t BaroAlt = 0;
#ifdef BARO
#include "common/maths.h"
#include "drivers/barometer.h"
@ -32,9 +36,6 @@ baro_t baro; // barometer access functions
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t BaroAlt = 0;
#ifdef BARO
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;

6
src/main/sensors/barometer.h
View File

@ -22,11 +22,11 @@ typedef enum {
BARO_NONE = 1,
BARO_BMP085 = 2,
BARO_MS5611 = 3,
BARO_BMP280 = 4
BARO_BMP280 = 4,
BARO_MAX = BARO_BMP280
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
#define BARO_MAX BARO_MS5611
typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array
@ -38,7 +38,6 @@ typedef struct barometerConfig_s {
extern int32_t BaroAlt;
extern int32_t baroTemperature; // Use temperature for telemetry
#ifdef BARO
void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void);
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
@ -46,4 +45,3 @@ uint32_t baroUpdate(void);
bool isBaroReady(void);
int32_t baroCalculateAltitude(void);
void performBaroCalibrationCycle(void);
#endif

15
src/main/sensors/battery.c
View File

@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src)
static void updateBatteryVoltage(void)
{
uint16_t vbatSample;
static biquad_t vbatFilterState;
static bool vbatFilterStateIsSet;
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
// store the battery voltage with some other recent battery voltage readings
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterStateIsSet) {
BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update
vbatFilterStateIsSet = true;
if (!vbatFilterIsInitialised) {
biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState);
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;

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

@ -20,7 +20,9 @@
#include "rx/rx.h"
#include "common/maths.h"
#ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 110
#endif
#define VBAT_RESDIVVAL_DEFAULT 10
#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
#define VBAT_SCALE_MIN 0

11
src/main/sensors/compass.c
View File

@ -35,18 +35,19 @@
#include "sensors/sensors.h"
#include "sensors/compass.h"
#ifdef NAZE
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
mag_t mag; // mag access functions
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
int16_t magADCRaw[XYZ_AXIS_COUNT];
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG
static int16_t magADCRaw[XYZ_AXIS_COUNT];
static uint8_t magInit = 0;
void compassInit(void)

10
src/main/sensors/compass.h
View File

@ -23,15 +23,13 @@ typedef enum {
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
MAG_AK8963 = 4
MAG_AK8963 = 4,
MAG_MAX = MAG_AK8963
} magSensor_e;
#define MAG_MAX MAG_AK8963
#ifdef MAG
void compassInit(void);
void updateCompass(flightDynamicsTrims_t *magZero);
#endif
union flightDynamicsTrims_u;
void updateCompass(union flightDynamicsTrims_u *magZero);
extern int32_t magADC[XYZ_AXIS_COUNT];

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

@ -28,7 +28,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
@ -36,36 +35,31 @@
#include "sensors/gyro.h"
uint16_t calibratingG = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[3];
static bool gyroFilterStateIsSet;
static float gyroLpfCutFreq;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroLpfCutFreq = gyro_lpf_hz;
gyroSoftLpfHz = gyro_soft_lpf_hz;
}
void initGyroFilterCoefficients(void) {
int axis;
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
gyroFilterStateIsSet = true;
}
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
void gyroInit(void)
{
calibratingG = calibrationCyclesRequired;
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
bool isGyroCalibrationComplete(void)
@ -73,27 +67,32 @@ bool isGyroCalibrationComplete(void)
return calibratingG == 0;
}
bool isOnFinalGyroCalibrationCycle(void)
static bool isOnFinalGyroCalibrationCycle(void)
{
return calibratingG == 1;
}
uint16_t calculateCalibratingCycles(void) {
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
static uint16_t gyroCalculateCalibratingCycles(void)
{
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
}
bool isOnFirstGyroCalibrationCycle(void)
static bool isOnFirstGyroCalibrationCycle(void)
{
return calibratingG == calculateCalibratingCycles();
return calibratingG == gyroCalculateCalibratingCycles();
}
void gyroSetCalibrationCycles(void)
{
calibratingG = gyroCalculateCalibratingCycles();
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
int8_t axis;
static int32_t g[3];
static stdev_t var[3];
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(calculateCalibratingCycles());
gyroSetCalibrationCycles();
return;
}
gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles();
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
}
}
@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
static void applyGyroZero(void)
{
int8_t axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
}
}
@ -138,14 +136,13 @@ static void applyGyroZero(void)
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
gyroADC[axis] = gyroADCRaw[axis];
}
@ -158,16 +155,14 @@ void gyroUpdate(void)
applyGyroZero();
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (gyroFilterStateIsSet) {
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
} else {
gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled
}
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = gyroADC[axis];
}
}
}

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

@ -27,23 +27,22 @@ typedef enum {
GYRO_MPU6000,
GYRO_MPU6500,
GYRO_MPU9250,
GYRO_FAKE
GYRO_FAKE,
GYRO_MAX = GYRO_FAKE
} gyroSensor_e;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
extern float gyroADCf[XYZ_AXIS_COUNT];
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);
uint16_t calculateCalibratingCycles(void);

Some files were not shown because too many files have changed in this diff Show More