From 4d238b27d5d33ed3145b9c5b0424e89ed7380409 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 08:41:24 +0100 Subject: [PATCH 1/7] Moved targetLooptime into gyro_t, tidied gyro_sync and gyro --- src/main/blackbox/blackbox.c | 5 +--- src/main/blackbox/blackbox_io.c | 1 - src/main/config/config.c | 1 - src/main/drivers/accgyro.h | 1 + src/main/drivers/accgyro_mpu.c | 10 ++++--- src/main/drivers/accgyro_mpu.h | 2 +- src/main/drivers/accgyro_mpu3050.c | 1 - src/main/drivers/accgyro_mpu6050.c | 2 +- src/main/drivers/gyro_sync.c | 44 +++++++++++++----------------- src/main/drivers/gyro_sync.h | 7 ++--- src/main/drivers/sensor.h | 2 +- src/main/flight/mixer.c | 1 - src/main/flight/pid.c | 3 +- src/main/io/osd.c | 1 - src/main/io/rc_controls.c | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 7 ++--- src/main/main.c | 5 ++-- src/main/mw.c | 2 +- src/main/rx/rx.c | 1 - src/main/sensors/acceleration.c | 1 - src/main/sensors/gyro.c | 20 +++++++------- src/main/sensors/gyro.h | 2 -- src/main/sensors/initialisation.c | 3 +- 24 files changed, 52 insertions(+), 72 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ed88dd40b1..46bae19584 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 82c105eb4a..7f9b1ec5bd 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -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" diff --git a/src/main/config/config.c b/src/main/config/config.c index c371ad2946..b7d404d3d6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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" diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 385ea90b97..3d90de1bc0 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -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 { diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d34..1dbe976090 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -32,7 +32,6 @@ #include "gpio.h" #include "exti.h" #include "bus_i2c.h" -#include "gyro_sync.h" #include "sensor.h" #include "accgyro.h" @@ -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; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6c..20389147ed 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -185,4 +185,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); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index c974728156..cefaf94b27 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -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 diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index d2717b1350..d4fa0fb846 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -1,4 +1,4 @@ -/* +/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 934708f379..61305e1819 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -4,48 +4,40 @@ * Created on: 3 aug. 2015 * Author: borisb */ + #include #include -#include #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; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 5c229c689c..f682e218ce 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -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); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 6554f8267d..7ae17f5104 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e677237899..adc0145ba9 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e1ba582272..91ccbc2d84 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index db4c8ad4e0..69f3f56911 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 23a87e7641..7eec0de896 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -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" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 237a5310f1..87df646344 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3bd8d14415..9f2609c0a0 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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); diff --git a/src/main/main.c b/src/main/main.c index dba36d3f43..686e395048 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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" @@ -694,12 +693,12 @@ void main_init(void) /* 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): diff --git a/src/main/mw.c b/src/main/mw.c index 5b282c3e8b..31d6e22c78 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -800,7 +800,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 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5d36956d4a..255fb547ea 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -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" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddde..e4471fe308 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -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" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bce60a1fca..5cd7415266 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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,29 +35,30 @@ #include "sensors/gyro.h" -uint16_t calibratingG = 0; +gyro_t gyro; // gyro access functions +sensor_align_e gyroAlign = 0; + int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +static uint16_t calibratingG = 0; +static 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) { gyroConfig = gyroConfigToUse; gyroLpfCutFreq = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) { +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); + if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); gyroFilterStateIsSet = true; } } @@ -79,7 +79,7 @@ bool isOnFinalGyroCalibrationCycle(void) } uint16_t calculateCalibratingCycles(void) { - return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES; + return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } bool isOnFirstGyroCalibrationCycle(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 04a5188a05..075cccbe69 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,11 +31,9 @@ typedef enum { } 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. diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 00093d18e6..df394c7a69 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -79,6 +79,7 @@ extern float magneticDeclination; extern gyro_t gyro; extern baro_t baro; extern acc_t acc; +extern sensor_align_e gyroAlign; uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; @@ -632,7 +633,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a acc.init(&acc); } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation + gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); detectMag(magHardwareToUse); From 75237dd2099351512368617ae4b9df9be78080a1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 16:14:17 +0100 Subject: [PATCH 2/7] Fixed up gyro init. --- src/main/config/config.c | 2 +- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 53 +++++++++++++------------------ src/main/sensors/gyro.h | 5 +-- src/main/sensors/initialisation.c | 1 + 7 files changed, 30 insertions(+), 37 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b7d404d3d6..2c0c9c17d9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -750,7 +750,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 7eec0de896..0848670dbc 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,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(gyroCalculateCalibratingCycles()); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index 686e395048..c6cf2ffb29 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 31d6e22c78..65abe6b484 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(calculateCalibratingCycles()); + gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5cd7415266..fc28705bf9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -45,21 +45,20 @@ static uint16_t calibratingG = 0; static 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; +static uint8_t gyroLpfHz; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) +void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfCutFreq = gyro_lpf_hz; + gyroLpfHz = gyro_lpf_hz; } -void initGyroFilterCoefficients(void) +void gyroInit(void) { - int axis; - if (gyroLpfCutFreq && gyro.targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], gyro.targetLooptime); - gyroFilterStateIsSet = true; + if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + for (int axis = 0; axis < 3; axis++) { + BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + } } } @@ -73,27 +72,27 @@ bool isGyroCalibrationComplete(void) return calibratingG == 0; } -bool isOnFinalGyroCalibrationCycle(void) +static bool isOnFinalGyroCalibrationCycle(void) { return calibratingG == 1; } -uint16_t calculateCalibratingCycles(void) { +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(); } 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(gyroCalculateCalibratingCycles()); 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,10 @@ 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 (gyroLpfHz) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADC[axis] = lrintf(gyroADCf[axis]); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 075cccbe69..7a8214092c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,9 +39,10 @@ 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 gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void gyroInit(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); -uint16_t calculateCalibratingCycles(void); +uint16_t gyroCalculateCalibratingCycles(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index df394c7a69..80d6169518 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -635,6 +635,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); + gyroInit(); detectMag(magHardwareToUse); From d069945f894eb7ac2848eb19d8e6abf7bb529c3b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Jun 2016 21:43:04 +0100 Subject: [PATCH 3/7] Improved gyroSetCalibrationCycles parameters. --- src/main/io/rc_controls.c | 2 +- src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 32 ++++++++++++++++---------------- src/main/sensors/gyro.h | 5 ++--- 5 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 0848670dbc..fdea810034 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/main.c b/src/main/main.c index c6cf2ffb29..743f771785 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -625,7 +625,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 65abe6b484..bf97c56a2d 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -377,7 +377,7 @@ void mwArm(void) static bool firstArmingCalibrationWasCompleted; if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { - gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); armingCalibrationWasInitialised = true; firstArmingCalibrationWasCompleted = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fc28705bf9..48113e934a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,32 +41,27 @@ sensor_align_e gyroAlign = 0; int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; -static uint16_t calibratingG = 0; static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; -static gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[3]; -static uint8_t gyroLpfHz; +static const gyroConfig_t *gyroConfig; +static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static uint8_t gyroSoftLpfHz; +static uint16_t calibratingG = 0; -void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz) { gyroConfig = gyroConfigToUse; - gyroLpfHz = gyro_lpf_hz; + gyroSoftLpfHz = gyro_soft_lpf_hz; } void gyroInit(void) { - if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known + if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); } } } -void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) -{ - calibratingG = calibrationCyclesRequired; -} - bool isGyroCalibrationComplete(void) { return calibratingG == 0; @@ -77,7 +72,7 @@ static bool isOnFinalGyroCalibrationCycle(void) return calibratingG == 1; } -uint16_t gyroCalculateCalibratingCycles(void) +static uint16_t gyroCalculateCalibratingCycles(void) { return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } @@ -87,6 +82,11 @@ static bool isOnFirstGyroCalibrationCycle(void) return calibratingG == gyroCalculateCalibratingCycles(); } +void gyroSetCalibrationCycles(void) +{ + calibratingG = gyroCalculateCalibratingCycles(); +} + static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; @@ -112,7 +112,7 @@ 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(gyroCalculateCalibratingCycles()); + gyroSetCalibrationCycles(); return; } gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); @@ -155,7 +155,7 @@ void gyroUpdate(void) applyGyroZero(); - if (gyroLpfHz) { + if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7a8214092c..54069b8a64 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,10 +39,9 @@ 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 gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t 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 gyroCalculateCalibratingCycles(void); From 97fe5afd6c1e7f0c066c71ba3c3da0fb4f4ab2ec Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:26:02 +0100 Subject: [PATCH 4/7] Converted tabs to spaces. --- src/main/drivers/accgyro_bma280.c | 8 +- src/main/drivers/accgyro_l3g4200d.c | 8 +- src/main/drivers/accgyro_lsm303dlhc.c | 6 +- src/main/drivers/accgyro_mma845x.c | 8 +- src/main/drivers/accgyro_mpu.c | 36 +- src/main/drivers/accgyro_mpu.h | 12 +- src/main/drivers/accgyro_spi_mpu6000.c | 16 +- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/accgyro_spi_mpu9250.c | 64 ++-- src/main/drivers/accgyro_spi_mpu9250.h | 2 +- src/main/drivers/adc.c | 10 +- src/main/drivers/adc_stm32f10x.c | 6 +- src/main/drivers/adc_stm32f30x.c | 16 +- src/main/drivers/adc_stm32f4xx.c | 26 +- src/main/drivers/barometer_bmp085.c | 18 +- src/main/drivers/barometer_bmp085.h | 4 +- src/main/drivers/barometer_bmp280.c | 10 +- src/main/drivers/barometer_ms5611.c | 12 +- src/main/drivers/bus_i2c_soft.c | 310 +++++++++--------- src/main/drivers/bus_spi.h | 28 +- src/main/drivers/compass_ak8975.c | 24 +- src/main/drivers/compass_hmc5883l.c | 20 +- src/main/drivers/dma_stm32f4xx.c | 2 +- src/main/drivers/inverter.c | 6 +- src/main/drivers/pwm_mapping.c | 2 +- src/main/drivers/pwm_mapping.h | 14 +- src/main/drivers/pwm_rx.c | 4 +- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_uart.h | 12 +- src/main/drivers/serial_uart_stm32f4xx.c | 188 +++++------ src/main/drivers/serial_usb_vcp.c | 10 +- src/main/drivers/sonar_hcsr04.c | 6 +- src/main/drivers/sound_beeper.c | 22 +- src/main/drivers/sound_beeper.h | 6 +- src/main/drivers/system.h | 14 +- src/main/drivers/system_stm32f4xx.c | 34 +- src/main/drivers/timer.c | 6 +- src/main/main.c | 14 +- src/main/mw.c | 4 +- .../target/ALIENFLIGHTF3/hardware_revision.c | 4 +- src/main/target/CC3D/target.c | 52 +-- src/main/target/CJMCU/hardware_revision.c | 4 +- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/FURYF3/target.h | 6 +- src/main/target/KISSFC/target.h | 2 +- src/main/target/LUX_RACE/target.c | 12 +- src/main/target/REVO/target.h | 2 +- src/main/target/REVONANO/target.h | 2 +- src/main/target/SPRACINGF3/target.c | 14 +- src/main/target/X_RACERSPI/target.c | 14 +- src/main/target/X_RACERSPI/target.mk | 2 +- src/main/target/ZCOREF3/target.h | 6 +- src/main/telemetry/hott.c | 12 +- src/main/vcpf4/usb_bsp.c | 10 +- src/main/vcpf4/usb_conf.h | 6 +- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- src/main/vcpf4/usbd_desc.c | 6 +- 57 files changed, 575 insertions(+), 575 deletions(-) diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index 866d944e3d..9d0ab92060 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -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; } diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index 27beb9d63d..33bd2f1f80 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -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; } diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 505764b29a..7a7222d4d6 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -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); diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 2687c34b1a..4adbe59ab8 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -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) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 0882581d34..63cb88ecfc 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -228,43 +228,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; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 6dbfcebf6c..15b6ee9429 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -120,12 +120,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; diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 3d58913d0a..cb78ffe102 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -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 diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4df65c19cb..2731797258 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -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; } diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 2bad53bf35..95c0b3f50f 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -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 { diff --git a/src/main/drivers/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro_spi_mpu9250.h index 521cefdf28..1af293288b 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro_spi_mpu9250.h @@ -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 diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 46f987dfc7..d214b49d35 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -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) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index b082388fe2..a810ac731b 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -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; @@ -114,7 +114,7 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; @@ -125,7 +125,7 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index dc9c98ee51..8c2ccba5ec 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -107,8 +107,8 @@ 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)); + 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; @@ -120,8 +120,8 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; @@ -133,8 +133,8 @@ void adcInit(drv_adc_config_t *init) #ifdef CURRENT_METER_ADC_GPIO 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)); + 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; @@ -146,8 +146,8 @@ void adcInit(drv_adc_config_t *init) #ifdef EXTERNAL1_ADC_GPIO 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)); + 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; diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 90ca22d12a..e4c086a609 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -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) @@ -112,7 +112,7 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; @@ -123,7 +123,7 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; @@ -134,7 +134,7 @@ void adcInit(drv_adc_config_t *init) #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)); + 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; @@ -184,11 +184,11 @@ void adcInit(drv_adc_config_t *init) 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_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_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index a746500b56..34a01cd38e 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -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]; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index dafb03a819..328bffb134 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -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); diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index cf7e60a817..6d42458486 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -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)); } diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 577146e2dc..468f86046f 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -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) diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index edf9e6d726..cac5c62211 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -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 diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index d408eac2f7..019080d963 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -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); diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 386a06e7d8..c35db1d1f6 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -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; } diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 4e59c375b8..a018ab0f52 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -204,7 +204,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; @@ -241,15 +241,15 @@ void hmc5883lInit(void) } 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 +267,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 +291,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 +309,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; } diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index 6b49fe6299..fefbae2801 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -32,7 +32,7 @@ static dmaHandlers_t dmaHandlers; void dmaNoOpHandler(DMA_Stream_TypeDef *stream) { - UNUSED(stream); + UNUSED(stream); } void DMA1_Stream2_IRQHandler(void) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 82ce8b0994..e0231ddb78 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -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 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 50061fb790..8d332ba56e 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -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 diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2212153cde..5d577af8e0 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -78,19 +78,19 @@ 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}; diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 9452a89eae..856613d2b3 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -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) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index a2ad403b26..da2c032681 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) if (state) { IOHi(softSerial->txIO); } else { - IOLo(softSerial->txIO); + IOLo(softSerial->txIO); } } diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index a332edebf7..33dde32167 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -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; diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 71b112e368..f5bba84699 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -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) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index e9ccef82fb..bb74b36b3d 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -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; diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 6374f81d3f..3b8ec91f7e 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -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() diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 1e71845d20..cba9a2a5c4 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -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 } diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index bdd17b7454..ab7a7c3dfc 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -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); diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 6f8bf23d6e..92e17f8a82 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -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 diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index bcaf71c9d0..f0042026f9 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -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); @@ -172,25 +172,25 @@ void systemInit(void) 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); } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 673265f545..732fa5f4ae 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -148,7 +148,7 @@ 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; } @@ -190,7 +190,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); @@ -660,7 +660,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; diff --git a/src/main/main.c b/src/main/main.c index dba36d3f43..b1a157c4d5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -488,12 +488,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); } @@ -688,7 +688,7 @@ void processLoopback(void) { #define processLoopback() #endif -void main_init(void) +void main_init(void) { init(); diff --git a/src/main/mw.c b/src/main/mw.c index e2819b722c..f77c426733 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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) @@ -778,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; } diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index 2108369958..b51d38658f 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -39,7 +39,7 @@ static IO_t HWDetectPin = IO_NONE; void detectHardwareRevision(void) { - HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); + HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT); IOConfigGPIO(HWDetectPin, IOCFG_IPU); @@ -74,4 +74,4 @@ const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) else { return &alienFlightF3V2MPUIntExtiConfig; } -} \ No newline at end of file +} diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index b146e6a72b..abfe089aa9 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -28,12 +28,12 @@ const uint16_t multiPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 0xFFFF }; @@ -59,26 +59,26 @@ const uint16_t airPWM[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; const uint16_t multiPPM_BP6[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -89,9 +89,9 @@ const uint16_t multiPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -118,11 +118,11 @@ const uint16_t airPWM_BP6[] = { PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 0xFFFF }; diff --git a/src/main/target/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c index 29071314b8..558e19a760 100755 --- a/src/main/target/CJMCU/hardware_revision.c +++ b/src/main/target/CJMCU/hardware_revision.c @@ -55,5 +55,5 @@ void updateHardwareRevision(void) const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) { - return NULL; -} \ No newline at end of file + return NULL; +} diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 68bf284483..976b287a05 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -34,7 +34,7 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 #define USE_SPI diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 6201b8b3f0..f849070b7d 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -25,9 +25,9 @@ #define USE_EXTI #define CONFIG_PREFER_ACC_ON -#define LED0 PC14 +#define LED0 PC14 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect @@ -188,7 +188,7 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 03c3cf31f6..6d79046a74 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -86,6 +86,6 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTF (BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 30804c5754..b665072acf 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -26,12 +26,12 @@ const uint16_t multiPWM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index cda67742e6..d2124e5b28 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -23,7 +23,7 @@ #define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8020000" +#define USBD_SERIALNUMBER_STRING "0x8020000" #endif #define LED0 PB5 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 3c0287f2f7..6f4a2c4315 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -22,7 +22,7 @@ #define USBD_PRODUCT_STRING "Revo Nano" #ifdef OPBL - #define USBD_SERIALNUMBER_STRING "0x8010000" +#define USBD_SERIALNUMBER_STRING "0x8010000" #endif #define LED0 PC14 diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ff9c8044f8..ddebee117f 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ff9c8044f8..ddebee117f 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12 - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP }; diff --git a/src/main/target/X_RACERSPI/target.mk b/src/main/target/X_RACERSPI/target.mk index c0c219bd30..3d831edc43 100644 --- a/src/main/target/X_RACERSPI/target.mk +++ b/src/main/target/X_RACERSPI/target.mk @@ -12,5 +12,5 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c + drivers/sonar_hcsr04.c diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c02b4d3ecb..d8ac143e88 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 #define BEEPER PC15 #define BEEPER_INVERTED @@ -45,8 +45,8 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 1eec8b9c06..6fb0bc1a92 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -424,8 +424,8 @@ static void hottCheckSerialData(uint32_t currentMicros) } static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) { - closeSerialPort(hottPort); - hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); + closeSerialPort(hottPort); + hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED); } static void hottSendTelemetryData(void) { @@ -433,9 +433,9 @@ static void hottSendTelemetryData(void) { hottIsSending = true; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); else - serialSetMode(hottPort, MODE_TX); + serialSetMode(hottPort, MODE_TX); hottMsgCrc = 0; return; } @@ -445,9 +445,9 @@ static void hottSendTelemetryData(void) { hottIsSending = false; // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3)) - workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); + workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); else - serialSetMode(hottPort, MODE_RX); + serialSetMode(hottPort, MODE_RX); flushHottRxBuffer(); return; } diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c index ba3fee7677..1160c47d8a 100644 --- a/src/main/vcpf4/usb_bsp.c +++ b/src/main/vcpf4/usb_bsp.c @@ -28,12 +28,12 @@ #include "../drivers/io.h" void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; } void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { - (void)pdev; - (void)state; + (void)pdev; + (void)state; } @@ -46,7 +46,7 @@ void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) { void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; GPIO_InitTypeDef GPIO_InitStructure; #ifndef USE_ULPI_PHY @@ -100,7 +100,7 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) */ void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) { - (void)pdev; + (void)pdev; NVIC_InitTypeDef NVIC_InitStructure; NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); diff --git a/src/main/vcpf4/usb_conf.h b/src/main/vcpf4/usb_conf.h index 6caf298889..60c8036202 100644 --- a/src/main/vcpf4/usb_conf.h +++ b/src/main/vcpf4/usb_conf.h @@ -233,9 +233,9 @@ #elif defined (__ICCARM__) /* IAR Compiler */ #define __packed __packed #elif defined ( __GNUC__ ) /* GNU Compiler */ - #ifndef __packed - #define __packed __attribute__ ((__packed__)) - #endif + #ifndef __packed + #define __packed __attribute__ ((__packed__)) + #endif #elif defined (__TASKING__) /* TASKING Compiler */ #define __packed __unaligned #endif /* __CC_ARM */ diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 3c89b5b9e8..3ca22aa4ef 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 +#define USB_RX_BUFSIZE 1024 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index 41282dab25..f9478327e6 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -197,7 +197,7 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc; } @@ -211,7 +211,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; } @@ -245,7 +245,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) { - (void)speed; + (void)speed; USBD_GetString ((uint8_t*)USBD_MANUFACTURER_STRING, USBD_StrDesc, length); return USBD_StrDesc; } From 7d5c8de5520b5d177f683a415be42318e4600878 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 19:32:41 +0100 Subject: [PATCH 5/7] Changed tabs to spaces --- src/main/drivers/accgyro_l3gd20.c | 2 +- src/main/drivers/accgyro_spi_mpu6000.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index dd28aceb11..aade7445b2 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -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) diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index c717a7bb16..0081bddebb 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -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 From a2d1af04aafa5c23b105aad894deb3e6cc787da4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 27 Jun 2016 20:04:21 +0100 Subject: [PATCH 6/7] Minor cosmetic tidying --- src/main/drivers/timer.c | 2 -- src/main/main.c | 9 ++++----- src/main/sensors/acceleration.c | 2 +- src/main/sensors/barometer.c | 7 ++++--- src/main/sensors/compass.c | 9 +++++---- src/main/target/X_RACERSPI/target.c | 1 - src/main/target/X_RACERSPI/target.h | 2 +- 7 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 732fa5f4ae..6999a9798e 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -17,7 +17,6 @@ #include #include -#include #include #include "platform.h" @@ -26,7 +25,6 @@ #include "nvic.h" -#include "gpio.h" #include "gpio.h" #include "rcc.h" #include "system.h" diff --git a/src/main/main.c b/src/main/main.c index b1a157c4d5..e31c6933df 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -17,11 +17,11 @@ #include #include -#include #include #include #include "platform.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -153,9 +153,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING; void init(void) { - uint8_t i; - drv_pwm_config_t pwm_params; - printfSupportInit(); initEEPROM(); @@ -260,6 +257,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 +331,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); @@ -504,7 +503,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); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 320d00ddde..1a4f6ef219 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,7 +45,7 @@ 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; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 2a011d8e2b..1793e37948 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -20,6 +20,10 @@ #include #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; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7ee14da4c7..de075bfe23 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,13 +40,14 @@ #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) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index ddebee117f..9c0bd04034 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -84,7 +84,6 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index a655c93e25..39c2e6c209 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "X_RACERSPI" +#define TARGET_BOARD_IDENTIFIER "XRC3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE From abd6b1e0f67637c64c053e415920640009a8b6b0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 28 Jun 2016 07:25:02 +1000 Subject: [PATCH 7/7] Enable pull ups for i2c for F3 targets by default. --- src/main/drivers/bus_i2c_stm32f30x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index c324d03e7a..adb424390b 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -83,8 +83,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, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); + IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C,