diff --git a/Makefile b/Makefile index e41d2d19e4..281b169c63 100644 --- a/Makefile +++ b/Makefile @@ -546,10 +546,17 @@ VPATH := $(VPATH):$(STDPERIPH_DIR)/src # Things that might need changing to use different tools # +# Find out if ccache is installed on the system +CCACHE := ccache +RESULT = $(shell (which $(CCACHE) > /dev/null 2>&1; echo $$?) ) +ifneq ($(RESULT),0) +CCACHE := +endif + # Tool names -CC = arm-none-eabi-gcc -OBJCOPY = arm-none-eabi-objcopy -SIZE = arm-none-eabi-size +CC := $(CCACHE) arm-none-eabi-gcc +OBJCOPY := arm-none-eabi-objcopy +SIZE := arm-none-eabi-size # # Tool options. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c529f4132b..96281b05fe 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -632,7 +632,7 @@ static void writeInterframe(void) arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); - /* + /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ @@ -1140,6 +1140,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); + BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); @@ -1210,7 +1211,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); @@ -1218,12 +1219,13 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); @@ -1314,7 +1316,7 @@ static void blackboxCheckAndLogFlightMode() } } -/* +/* * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control * the portion of logged loop iterations. */ diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 7f9b1ec5bd..cd4f05d980 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -507,7 +507,7 @@ void blackboxWriteFloat(float value) /** * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. - * + * * Intended to be called regularly for the blackbox device to perform housekeeping. */ void blackboxDeviceFlush(void) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index d059fe882f..90c3decdae 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -56,24 +56,39 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) } /* sets up a biquad Filter */ -void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate) +void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) +{ + biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType) { - const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - // setup variables - const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; + const float sampleRate = 1 / ((float)refreshRate * 0.000001f); + const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate; const float sn = sinf(omega); const float cs = cosf(omega); - //this is wrong, should be hyperbolic sine - //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn); - const float alpha = sn / (2 * BIQUAD_Q); + const float alpha = sn / (2 * Q); - const float b0 = (1 - cs) / 2; - const float b1 = 1 - cs; - const float b2 = (1 - cs) / 2; - const float a0 = 1 + alpha; - const float a1 = -2 * cs; - const float a2 = 1 - alpha; + float b0, b1, b2, a0, a1, a2; + + switch (filterType) { + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + } // precompute the coefficients filter->b0 = b0 / a0; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 66fcb53bb0..d97becc015 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,8 +29,13 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; +typedef enum { + FILTER_LPF, + FILTER_NOTCH +} filterType_e; -void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate); +void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 888847e075..52990c6ac8 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -231,7 +231,7 @@ int32_t quickMedianFilter5(int32_t * v) QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]); - QMF_SORT(p[1], p[2]); + QMF_SORT(p[1], p[2]); return p[2]; } @@ -279,7 +279,7 @@ float quickMedianFilter5f(float * v) QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[0], p[3]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[2], p[3]); - QMF_SORTF(p[1], p[2]); + QMF_SORTF(p[1], p[2]); return p[2]; } diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 7847fb3035..e11d49d151 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -56,7 +56,7 @@ typedef void (*putcf) (void *, char); static putcf stdout_putf; static void *stdout_putp; -// print bf, padded from left to at least n characters. +// print bf, padded from left to at least n characters. // padding is zero ('0') if z!=0, space (' ') otherwise static int putchw(void *putp, putcf putf, int n, char z, char *bf) { diff --git a/src/main/common/utils.h b/src/main/common/utils.h index a5c0591918..27a51f3984 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -71,4 +71,15 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } +// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions +// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) +#ifdef UNIT_TEST +// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) +#include +static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); }; +#else +void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); +#endif + + #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 06ba7c9c09..74399f1bd8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,7 +159,7 @@ size_t custom_flash_memory_address = 0; #define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) #else // use the last flash pages for storage -#ifndef CONFIG_START_FLASH_ADDRESS +#ifndef CONFIG_START_FLASH_ADDRESS #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) #endif #endif @@ -182,12 +182,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) void resetPidProfile(pidProfile_t *pidProfile) { - -#if (defined(STM32F10X)) - pidProfile->pidController = PID_CONTROLLER_INTEGER; -#else - pidProfile->pidController = PID_CONTROLLER_FLOAT; -#endif + pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; @@ -220,11 +215,19 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 180; - pidProfile->yawItermIgnoreRate = 35; + pidProfile->rollPitchItermIgnoreRate = 200; + pidProfile->yawItermIgnoreRate = 50; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; - pidProfile->dynamic_pid = 1; + pidProfile->vbatPidCompensation = 0; + pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; + + // Betaflight PID controller parameters + pidProfile->toleranceBand = 15; + pidProfile->toleranceBandReduction = 35; + pidProfile->zeroCrossAllowanceCount = 3; + pidProfile->accelerationLimitPercent = 20; + pidProfile->itermThrottleGain = 10; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -278,7 +281,6 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; - escAndServoConfig->escDesyncProtection = 0; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) @@ -311,7 +313,6 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->vbatmincellvoltage = 33; batteryConfig->vbatwarningcellvoltage = 35; batteryConfig->vbathysteresis = 1; - batteryConfig->vbatPidCompensation = 0; batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; @@ -349,7 +350,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) serialConfig->reboot_character = 'R'; } -static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) +static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; controlRateConfig->rcYawRate8 = 100; @@ -366,7 +367,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) } -void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) +void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->deadband = 0; rcControlsConfig->yaw_deadband = 0; @@ -374,7 +375,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) rcControlsConfig->alt_hold_fast_change = 1; } -void resetMixerConfig(mixerConfig_t *mixerConfig) +void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; #ifdef USE_SERVOS @@ -441,6 +442,7 @@ static void resetConf(void) featureSet(FEATURE_VBAT); #endif + masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; @@ -451,12 +453,14 @@ static void resetConf(void) masterConfig.gyro_lpf = 0; // 256HZ default #ifdef STM32F10X masterConfig.gyro_sync_denom = 8; + masterConfig.pid_process_denom = 1; #else masterConfig.gyro_sync_denom = 4; + masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_lpf_hz = 100; - - masterConfig.pid_process_denom = 2; + masterConfig.gyro_soft_notch_hz = 0; + masterConfig.gyro_soft_notch_q = 5; masterConfig.debug_mode = 0; @@ -533,6 +537,7 @@ static void resetConf(void) #ifdef BRUSHED_MOTORS masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED; + masterConfig.use_unsyncedPwm = true; #else masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125; @@ -612,8 +617,10 @@ static void resetConf(void) } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); + applyDefaultColors(masterConfig.colors); applyDefaultLedStripConfig(masterConfig.ledConfigs); + applyDefaultModeColors(masterConfig.modeColors); + applyDefaultSpecialColors(&(masterConfig.specialColors)); masterConfig.ledstrip_visual_beeper = 0; #endif @@ -657,6 +664,7 @@ static void resetConf(void) targetConfiguration(); #endif + // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); @@ -715,7 +723,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config.h b/src/main/config/config.h index 5a46a78c33..b97ac6b45b 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -24,6 +24,8 @@ #endif #define MAX_RATEPROFILES 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 +#define MAX_NAME_LENGTH 16 + typedef enum { FEATURE_RX_PPM = 1 << 0, @@ -52,6 +54,8 @@ typedef enum { FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_OSD = 1 << 24, FEATURE_VTX = 1 << 25, + FEATURE_RX_NRF24 = 1 << 26, + FEATURE_SOFTSPI = 1 << 27, } features_e; void latchActiveFeatures(void); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c6f01b4029..29a236e565 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -56,9 +56,11 @@ typedef struct master_t { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. - uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider - uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz + uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz + uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz + uint8_t gyro_soft_notch_q; // Biquad gyro notch quality uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) @@ -117,8 +119,10 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; #ifdef LED_STRIP - ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; - hsvColor_t colors[CONFIGURABLE_COLOR_COUNT]; + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on #endif @@ -137,7 +141,7 @@ typedef struct master_t { profile_t profile[MAX_PROFILE_COUNT]; uint8_t current_profile_index; - + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; @@ -161,6 +165,9 @@ typedef struct master_t { uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum + + char name[MAX_NAME_LENGTH+1]; + } master_t; extern master_t masterConfig; diff --git a/src/main/debug.h b/src/main/debug.h index 4f6250b758..19124266d0 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -50,5 +50,6 @@ typedef enum { DEBUG_MIXER, DEBUG_AIRMODE, DEBUG_PIDLOOP, + DEBUG_NOTCH, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 9a3cd57253..1c705ef137 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -252,7 +252,7 @@ void mpuIntExtiInit(void) EXTIEnable(mpuIntIO, true); #endif - mpuExtiInitDone = true; + mpuExtiInitDone = true; } static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 917c06f1a8..132c9e13a0 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -18,7 +18,7 @@ #pragma once #include "exti.h" - + // MPU6050 #define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I_LEGACY 0x00 @@ -119,7 +119,7 @@ typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); -typedef void(*mpuResetFuncPtr)(void); +typedef void(*mpuResetFuncPtr)(void); typedef struct mpuConfiguration_s { uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 3cc3ac8f69..78e8b8e801 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -104,7 +104,7 @@ static void mpu6050GyroInit(uint8_t lpf) delay(100); ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) - delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure + delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index b483380912..dc21901e7a 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -158,7 +158,7 @@ bool mpu6000SpiDetect(void) uint8_t in; uint8_t attemptsRemaining = 5; -#ifdef MPU6000_CS_PIN +#ifdef MPU6000_CS_PIN mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN)); #endif IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); @@ -253,7 +253,7 @@ static void mpu6000AccAndGyroInit(void) { delayMicroseconds(15); #endif - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); delayMicroseconds(1); mpuSpi6000InitDone = true; diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index b5beca2946..0406e6023c 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -54,7 +54,7 @@ static IO_t mpuSpi9250CsPin = IO_NONE; #define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin) #define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin) -void mpu9250ResetGyro(void) +void mpu9250ResetGyro(void) { // Device Reset mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); @@ -123,7 +123,7 @@ void mpu9250SpiAccInit(acc_t *acc) acc->acc_1G = 512 * 8; } -bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) +bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) { uint8_t in; uint8_t attemptsRemaining = 20; @@ -176,7 +176,7 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { 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); + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); mpuSpi9250InitDone = true; //init done } @@ -208,7 +208,7 @@ bool mpu9250SpiDetect(void) } } while (attemptsRemaining--); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); return true; } diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 258c8acc8d..146ada62be 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -24,14 +24,14 @@ #define ADC_TAG_MAP_COUNT 16 #elif defined(STM32F3) #define ADC_TAG_MAP_COUNT 39 -#else +#else #define ADC_TAG_MAP_COUNT 10 -#endif +#endif typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, -#if defined(STM32F3) +#if defined(STM32F3) ADCDEV_2, ADCDEV_MAX = ADCDEV_2, #elif defined(STM32F4) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 451c9296c6..3104bcd5e9 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -24,9 +24,6 @@ #include "build_config.h" #include "system.h" - -#include "sensors/sensors.h" // FIXME dependency into the main code - #include "sensor.h" #include "accgyro.h" #include "adc.h" @@ -38,13 +35,13 @@ #define ADC_INSTANCE ADC1 #endif -const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; /* TODO -- ADC2 available on large 10x devices. @@ -54,7 +51,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) return ADCINVALID; } -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA0, ADC_Channel_0 }, // ADC12 { DEFIO_TAG_E__PA1, ADC_Channel_1 }, // ADC12 { DEFIO_TAG_E__PA2, ADC_Channel_2 }, // ADC12 @@ -64,7 +61,7 @@ const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12 { DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12 { DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12 - { DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12 + { DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12 }; // Driver for STM32F103CB onboard ADC @@ -84,7 +81,6 @@ void adcInit(drv_adc_config_t *init) UNUSED(init); #endif - uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); @@ -117,9 +113,9 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + const adcDevice_t adc = adcHardware[device]; - for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) continue; @@ -163,7 +159,7 @@ void adcInit(drv_adc_config_t *init) ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 6297c2bd16..f58a8086d6 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -21,7 +21,6 @@ #include "platform.h" #include "system.h" -#include "common/utils.h" #include "gpio.h" #include "sensor.h" @@ -32,16 +31,18 @@ #include "io.h" #include "rcc.h" +#include "common/utils.h" + #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 #endif -const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } }; -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA0, ADC_Channel_1 }, // ADC1 { DEFIO_TAG_E__PA1, ADC_Channel_2 }, // ADC1 { DEFIO_TAG_E__PA2, ADC_Channel_3 }, // ADC1 @@ -85,7 +86,7 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; if (instance == ADC2) @@ -100,7 +101,6 @@ void adcInit(drv_adc_config_t *init) ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; - uint8_t i; uint8_t adcChannelCount = 0; memset(&adcConfig, 0, sizeof(adcConfig)); @@ -133,9 +133,9 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + adcDevice_t adc = adcHardware[device]; - for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) continue; @@ -203,7 +203,7 @@ void adcInit(drv_adc_config_t *init) ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index e1bb1ae617..968f05a67e 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -26,8 +26,6 @@ #include "io_impl.h" #include "rcc.h" -#include "sensors/sensors.h" // FIXME dependency into the main code - #include "sensor.h" #include "accgyro.h" @@ -38,13 +36,13 @@ #define ADC_INSTANCE ADC1 #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 } +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 } }; /* note these could be packed up for saving space */ -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { /* { DEFIO_TAG_E__PF3, ADC_Channel_9 }, { DEFIO_TAG_E__PF4, ADC_Channel_14 }, @@ -75,7 +73,7 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; /* if (instance == ADC2) // TODO add ADC2 and 3 @@ -128,7 +126,7 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + adcDevice_t adc = adcHardware[device]; for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 4c3e779855..9b35646b6b 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -35,7 +35,7 @@ #ifdef BARO -#if defined(BARO_EOC_GPIO) +#if defined(BARO_EOC_GPIO) static IO_t eocIO; @@ -49,7 +49,7 @@ void bmp085_extiHandler(extiCallbackRec_t* cb) isConversionComplete = true; } -bool bmp085TestEOCConnected(const bmp085Config_t *config); +bool bmp085TestEOCConnected(const bmp085Config_t *config); # endif typedef struct { @@ -139,7 +139,7 @@ static IO_t xclrIO; #endif -void bmp085InitXclrIO(const bmp085Config_t *config) +void bmp085InitXclrIO(const bmp085Config_t *config) { if (!xclrIO && config && config->xclrIO) { xclrIO = IOGetByTag(config->xclrIO); @@ -184,7 +184,7 @@ 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; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index 328bffb134..ffe863d1ba 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -19,7 +19,7 @@ typedef struct bmp085Config_s { ioTag_t xclrIO; - ioTag_t eocIO; + ioTag_t eocIO; } bmp085Config_t; bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index bdfc75dfa9..86f7a66181 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -26,7 +26,7 @@ #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID -#endif +#endif typedef enum I2CDevice { I2CINVALID = -1, diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 74a9f36559..7daea6fa95 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -47,35 +47,35 @@ static void i2cUnstick(IO_t scl, IO_t sda); #define IOCFG_I2C IOCFG_AF_OD #endif -#ifndef I2C1_SCL -#define I2C1_SCL PB8 +#ifndef I2C1_SCL +#define I2C1_SCL PB8 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB9 +#ifndef I2C1_SDA +#define I2C1_SDA PB9 #endif #else -#ifndef I2C1_SCL -#define I2C1_SCL PB6 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 +#ifndef I2C1_SDA +#define I2C1_SDA PB7 #endif #endif -#ifndef I2C2_SCL -#define I2C2_SCL PB10 +#ifndef I2C2_SCL +#define I2C2_SCL PB10 #endif -#ifndef I2C2_SDA +#ifndef I2C2_SDA #define I2C2_SDA PB11 #endif #ifdef STM32F4 -#ifndef I2C3_SCL +#ifndef I2C3_SCL #define I2C3_SCL PA8 #endif -#ifndef I2C3_SDA -#define I2C3_SDA PB4 +#ifndef I2C3_SDA +#define I2C3_SDA PB4 #endif #endif @@ -93,7 +93,7 @@ static i2cState_t i2cState[] = { { false, false, 0, 0, 0, 0, 0, 0, 0 }, { false, false, 0, 0, 0, 0, 0, 0, 0 }, { false, false, 0, 0, 0, 0, 0, 0, 0 } -}; +}; void I2C1_ER_IRQHandler(void) { i2c_er_handler(I2CDEV_1); @@ -121,7 +121,7 @@ void I2C3_EV_IRQHandler(void) { } #endif -static bool i2cHandleHardwareFailure(I2CDevice device) +static bool i2cHandleHardwareFailure(I2CDevice device) { i2cErrorCount++; // reinit peripheral + clock out garbage @@ -129,7 +129,7 @@ static bool i2cHandleHardwareFailure(I2CDevice device) return false; } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) { if (device == I2CINVALID) @@ -138,7 +138,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint32_t timeout = I2C_DEFAULT_TIMEOUT; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; i2cState_t *state; state = &(i2cState[device]); @@ -171,12 +171,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, return !(state->error); } -bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) { return i2cWriteBuffer(device, addr_, reg_, 1, &data); } -bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { if (device == I2CINVALID) return false; @@ -369,7 +369,7 @@ void i2c_ev_handler(I2CDevice device) { } } -void i2cInit(I2CDevice device) +void i2cInit(I2CDevice device) { if (device == I2CINVALID) return; @@ -392,7 +392,7 @@ void i2cInit(I2CDevice device) I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE); i2cUnstick(scl, sda); - + // Init pins #ifdef STM32F4 IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index d645a70504..524df1bc86 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -39,18 +39,18 @@ #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) -#define I2C_GPIO_AF GPIO_AF_4 +#define I2C_GPIO_AF GPIO_AF_4 -#ifndef I2C1_SCL -#define I2C1_SCL PB6 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 +#ifndef I2C1_SDA +#define I2C1_SDA PB7 #endif -#ifndef I2C2_SCL -#define I2C2_SCL PF4 +#ifndef I2C2_SCL +#define I2C2_SCL PF4 #endif -#ifndef I2C2_SDA +#ifndef I2C2_SDA #define I2C2_SDA PA10 #endif @@ -82,7 +82,7 @@ void i2cInit(I2CDevice device) I2C_TypeDef *I2Cx; I2Cx = i2c->dev; - + IO_t scl = IOGetByTag(i2c->scl); IO_t sda = IOGetByTag(i2c->sda); @@ -108,7 +108,7 @@ void i2cInit(I2CDevice device) I2C_Init(I2Cx, &i2cInit); I2C_StretchClockCmd(I2Cx, ENABLE); - + I2C_Cmd(I2Cx, ENABLE); } @@ -122,7 +122,7 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) addr_ <<= 1; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; @@ -188,7 +188,7 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* addr_ <<= 1; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 005f7936b6..690f25b52e 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -36,7 +36,7 @@ #ifndef GPIO_AF_SPI3 #define GPIO_AF_SPI3 GPIO_AF_6 #endif -#endif +#endif #ifndef SPI1_SCK_PIN #define SPI1_NSS_PIN PA4 @@ -84,7 +84,7 @@ static spiDevice_t spiHardwareMap[] = { SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { - if (instance == SPI1) + if (instance == SPI1) return SPIDEV_1; if (instance == SPI2) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index dbba94881c..5a6ea7dd37 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -31,8 +31,6 @@ #define SPI_IO_AF_MOSI_CFG IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_50MHz) #define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_50MHz) #define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz) -#else -#error "Unknown processor" #endif /* diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 4f0dabdfff..726a5a1807 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -22,13 +22,15 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel #ifdef STM32F4 typedef enum { - DMA1_ST1_HANDLER = 0, + DMA1_ST0_HANDLER = 0, + DMA1_ST1_HANDLER, DMA1_ST2_HANDLER, DMA1_ST3_HANDLER, DMA1_ST4_HANDLER, DMA1_ST5_HANDLER, DMA1_ST6_HANDLER, DMA1_ST7_HANDLER, + DMA2_ST0_HANDLER, DMA2_ST1_HANDLER, DMA2_ST2_HANDLER, DMA2_ST3_HANDLER, @@ -63,7 +65,7 @@ typedef struct dmaChannelDescriptor_s { #define DMA_IT_TEIF ((uint32_t)0x00000008) #define DMA_IT_DMEIF ((uint32_t)0x00000004) #define DMA_IT_FEIF ((uint32_t)0x00000001) - + #else typedef enum { diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ce960ab4c2..ec6e7908d3 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -45,12 +45,12 @@ static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1Periph_DMA2), DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1Periph_DMA2), DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1Periph_DMA2), - }; /* * DMA IRQ Handlers */ +DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER) @@ -58,12 +58,14 @@ DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) - +DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) void dmaInit(void) { diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index cd97ba9b74..ce665bb077 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -24,22 +24,22 @@ static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS]; #if defined(STM32F1) || defined(STM32F4) static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { - EXTI0_IRQn, - EXTI1_IRQn, - EXTI2_IRQn, - EXTI3_IRQn, + EXTI0_IRQn, + EXTI1_IRQn, + EXTI2_IRQn, + EXTI3_IRQn, EXTI4_IRQn, - EXTI9_5_IRQn, + EXTI9_5_IRQn, EXTI15_10_IRQn }; #elif defined(STM32F3) static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { - EXTI0_IRQn, - EXTI1_IRQn, - EXTI2_TS_IRQn, - EXTI3_IRQn, + EXTI0_IRQn, + EXTI1_IRQn, + EXTI2_TS_IRQn, + EXTI3_IRQn, EXTI4_IRQn, - EXTI9_5_IRQn, + EXTI9_5_IRQn, EXTI15_10_IRQn }; #else diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 806c777dab..29620fbe11 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -199,7 +199,7 @@ static bool m25p16_readIdentification() bool m25p16_init() { -#ifdef M25P16_CS_PIN +#ifdef M25P16_CS_PIN m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); #endif IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index fbbff6ffd4..967c87680a 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -20,7 +20,7 @@ #include "platform.h" -#ifdef INVERTER +#ifdef INVERTER #include "io.h" #include "io_impl.h" diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 07c53a2032..af9deca810 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -52,7 +52,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO -#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN) diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 1b3cca2bcf..0c880fd78d 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -12,7 +12,7 @@ typedef struct ioRec_s { GPIO_TypeDef *gpio; uint16_t pin; resourceOwner_t owner; - resourceType_t resource; + resourceType_t resource; uint8_t index; } ioRec_t; @@ -26,7 +26,7 @@ int IO_GPIO_PortSource(IO_t io); #if defined(STM32F3) || defined(STM32F4) int IO_EXTI_PortSourceGPIO(IO_t io); -int IO_EXTI_PinSource(IO_t io); +int IO_EXTI_PinSource(IO_t io); #endif GPIO_TypeDef* IO_GPIO(IO_t io); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 5fe73a1664..a6b2a6892e 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -27,12 +27,12 @@ static const IO_t leds[] = { DEFIO_IO(LED0), #else DEFIO_IO(NONE), -#endif +#endif #ifdef LED1 DEFIO_IO(LED1), #else DEFIO_IO(NONE), -#endif +#endif #ifdef LED2 DEFIO_IO(LED2), #else @@ -78,15 +78,14 @@ uint8_t ledPolarity = 0 #endif ; -uint8_t ledOffset = 0; +static uint8_t ledOffset = 0; void ledInit(bool alternative_led) { - uint32_t i; - #if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) - if (alternative_led) + if (alternative_led) { ledOffset = LED_NUMBER; + } #else UNUSED(alternative_led); #endif @@ -95,7 +94,7 @@ void ledInit(bool alternative_led) LED1_OFF; LED2_OFF; - for (i = 0; i < LED_NUMBER; i++) { + for (int i = 0; i < LED_NUMBER; i++) { if (leds[i + ledOffset]) { IOInit(leds[i + ledOffset], OWNER_LED, RESOURCE_OUTPUT, RESOURCE_INDEX(i)); IOConfigGPIO(leds[i + ledOffset], IOCFG_OUT_PP); @@ -114,6 +113,6 @@ void ledToggle(int led) void ledSet(int led, bool on) { - bool inverted = (1 << (led + ledOffset)) & ledPolarity; + const bool inverted = (1 << (led + ledOffset)) & ledPolarity; IOWrite(leds[led + ledOffset], on ? inverted : !inverted); } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 974c19c655..853ac7acbc 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -53,6 +53,3 @@ bool isWS2811LedStripReady(void); extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; extern volatile uint8_t ws2811LedDataTransferInProgress; - -extern const hsvColor_t hsv_white; -extern const hsvColor_t hsv_black; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 96bb85fd26..b4a9b54268 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -105,6 +105,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index ff52a64836..722dd9a2e4 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -111,6 +111,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 3a888bd21b..e8676e2cdf 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -31,8 +31,8 @@ #ifdef LED_STRIP -#if !defined(WS2811_PIN) -#define WS2811_PIN PA0 +#if !defined(WS2811_PIN) +#define WS2811_PIN PA0 #define WS2811_TIMER TIM5 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER #define WS2811_DMA_STREAM DMA1_Stream2 @@ -154,6 +154,7 @@ void ws2811LedStripHardwareInit(void) dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index b9f521273c..82f3f29af9 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -75,6 +75,7 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s #endif // Common to both channels + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(MAX7456_SPI_INSTANCE->DR)); DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; @@ -82,22 +83,31 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s DMA_InitStructure.DMA_BufferSize = buffer_size; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; #ifdef MAX7456_DMA_CHANNEL_RX // Rx Channel +#ifdef STM32F4 + DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; +#else DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); - DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; +#endif + DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure); DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE); #endif // Tx channel +#ifdef STM32F4 + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; +#else DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; +#endif + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure); DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE); @@ -147,7 +157,7 @@ void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) { } #endif -void max7456_init(uint8_t video_system) +void max7456_init(uint8_t video_system) { uint8_t max_screen_rows; uint8_t srdata = 0; @@ -247,7 +257,7 @@ void max7456_draw_screen(void) { max7456_send(MAX7456ADD_DMM, 1); for (xx = 0; xx < max_screen_size; ++xx) { max7456_send(MAX7456ADD_DMDI, SCREEN_BUFFER[xx]); - SCREEN_BUFFER[xx] = MAX7456_CHAR(0); + SCREEN_BUFFER[xx] = MAX7456_CHAR(' '); } max7456_send(MAX7456ADD_DMDI, 0xFF); max7456_send(MAX7456ADD_DMM, 0); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 13f01eac42..d6f41808cb 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -30,11 +30,6 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); - /* Configuration maps @@ -100,7 +95,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) int channelIndex = 0; memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); - + // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] if (init->airplane) i = 2; // switch to air hardware config @@ -265,7 +260,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (init->useChannelForwarding && !init->airplane) { #if defined(NAZE) && defined(WS2811_TIMER) // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14 - if (init->useLEDStrip) { + if (init->useLEDStrip) { if (timerIndex >= PWM13 && timerIndex <= PWM14) { type = MAP_TO_SERVO_OUTPUT; } @@ -313,14 +308,14 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif if (init->useFastPwm) { - pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED; } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM; } pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount; pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ffb5540995..5a3f402e97 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -30,24 +30,8 @@ #error Invalid motor/servo/port configuration #endif -#define PULSE_1MS (1000) // 1ms pulse width -#define MAX_INPUTS 8 #define PWM_TIMER_MHZ 1 -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT42_TIMER_MHZ 21 -#define ONESHOT125_TIMER_MHZ 12 -#define PWM_BRUSHED_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#else -#define PWM_BRUSHED_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define ONESHOT42_TIMER_MHZ 24 -#define ONESHOT125_TIMER_MHZ 8 -#endif - -#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) -#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) typedef struct sonarIOConfig_s { ioTag_t triggerTag; @@ -133,7 +117,7 @@ enum { PWM13, PWM14, PWM15, - PWM16, + PWM16, PWM17, PWM18, PWM19, diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 056ee17faa..bcee6c70b8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,21 +17,31 @@ #include #include - -#include #include #include "platform.h" #include "io.h" #include "timer.h" - -#include "flight/failsafe.h" // FIXME dependency into the main code from a driver - #include "pwm_mapping.h" - #include "pwm_output.h" +#if defined(STM32F40_41xxx) // must be multiples of timer clock +#define ONESHOT125_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 21 +#define MULTISHOT_TIMER_MHZ 84 +#define PWM_BRUSHED_TIMER_MHZ 21 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) + + typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { @@ -52,35 +62,43 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; -static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t ouputPolarity) + + +static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + } + else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + } TIM_OCInitStructure.TIM_Pulse = value; - TIM_OCInitStructure.TIM_OCPolarity = ouputPolarity ? TIM_OCPolarity_High : TIM_OCPolarity_Low; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; switch (channel) { - case TIM_Channel_1: - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(tim, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(tim, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); - break; + case TIM_Channel_1: + TIM_OC1Init(tim, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(tim, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(tim, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(tim, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); + break; } } @@ -90,11 +108,11 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 configTimeBase(timerHardware->tim, period, mhz); - IO_t io = IOGetByTag(timerHardware->tag); + const IO_t io = IOGetByTag(timerHardware->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); IOConfigGPIO(io, IOCFG_AF_PP); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); @@ -102,24 +120,24 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 TIM_Cmd(timerHardware->tim, ENABLE); switch (timerHardware->channel) { - case TIM_Channel_1: - p->ccr = &timerHardware->tim->CCR1; - break; - case TIM_Channel_2: - p->ccr = &timerHardware->tim->CCR2; - break; - case TIM_Channel_3: - p->ccr = &timerHardware->tim->CCR3; - break; - case TIM_Channel_4: - p->ccr = &timerHardware->tim->CCR4; - break; + case TIM_Channel_1: + p->ccr = &timerHardware->tim->CCR1; + break; + case TIM_Channel_2: + p->ccr = &timerHardware->tim->CCR2; + break; + case TIM_Channel_3: + p->ccr = &timerHardware->tim->CCR3; + break; + case TIM_Channel_4: + p->ccr = &timerHardware->tim->CCR4; + break; } p->period = period; p->tim = timerHardware->tim; *p->ccr = 0; - + return p; } @@ -133,16 +151,16 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) *motors[index]->ccr = value; } -static void pwmWriteOneShot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); -} - static void pwmWriteOneShot125(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); } +static void pwmWriteOneShot42(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); +} + static void pwmWriteMultiShot(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); @@ -150,15 +168,14 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) + if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { motors[index]->pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { - uint8_t index; - - for(index = 0; index < motorCount; index++){ + for(int index = 0; index < motorCount; index++){ // Set the compare register to 0, which stops the output pulsing if the timer overflows *motors[index]->ccr = 0; } @@ -176,17 +193,14 @@ void pwmEnableMotors(void) void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { - uint8_t index; TIM_TypeDef *lastTimerPtr = NULL; - for (index = 0; index < motorCount; index++) { - + for (int index = 0; index < motorCount; index++) { // Force the timer to overflow if it's the first motor to output, or if we change timers if (motors[index]->tim != lastTimerPtr) { lastTimerPtr = motors[index]->tim; timerForceOverflow(motors[index]->tim); } - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. *motors[index]->ccr = 0; @@ -195,44 +209,46 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) { - uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; } void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) { - uint32_t hz = PWM_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType) { uint32_t timerMhzCounter; + pwmWriteFuncPtr pwmWritePtr; switch (fastPwmProtocolType) { - default: - case (PWM_TYPE_ONESHOT125): - timerMhzCounter = ONESHOT125_TIMER_MHZ; - break; - case (PWM_TYPE_ONESHOT42): - timerMhzCounter = ONESHOT42_TIMER_MHZ; - break; - case (PWM_TYPE_MULTISHOT): - timerMhzCounter = MULTISHOT_TIMER_MHZ; + default: + case (PWM_TYPE_ONESHOT125): + timerMhzCounter = ONESHOT125_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot125; + break; + case (PWM_TYPE_ONESHOT42): + timerMhzCounter = ONESHOT42_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot42; + break; + case (PWM_TYPE_MULTISHOT): + timerMhzCounter = MULTISHOT_TIMER_MHZ; + pwmWritePtr = pwmWriteMultiShot; + break; } if (motorPwmRate > 0) { - uint32_t hz = timerMhzCounter * 1000000; + const uint32_t hz = timerMhzCounter * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); } else { motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0); } - - motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : - ((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 : - pwmWriteOneShot42); + motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS @@ -243,7 +259,8 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui void pwmWriteServo(uint8_t index, uint16_t value) { - if (servos[index] && index < MAX_SERVOS) + if (servos[index] && index < MAX_SERVOS) { *servos[index]->ccr = value; + } } #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index c89e7fa802..b9bc62c68d 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -25,6 +25,12 @@ typedef enum { PWM_TYPE_BRUSHED } motorPwmProtocolTypes_e; +struct timerHardware_s; +void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); +void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmFastPwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType); +void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index a177dab1da..f9430d2844 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -1,7 +1,7 @@ #pragma once -#define RESOURCE_INDEX(x) x + 1 +#define RESOURCE_INDEX(x) (x + 1) typedef enum { OWNER_FREE = 0, diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 489f2260ef..878d5eacf6 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -126,7 +126,7 @@ void sdcardInsertionDetectDeinit(void) #ifdef SDCARD_DETECT_PIN sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); IOInit(sdCardDetectPin, OWNER_FREE, RESOURCE_NONE, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); + IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); #endif } @@ -135,7 +135,7 @@ void sdcardInsertionDetectInit(void) #ifdef SDCARD_DETECT_PIN sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); IOInit(sdCardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); + IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); #endif } @@ -192,6 +192,11 @@ static void sdcard_deselect(void) */ static void sdcard_reset(void) { + if (!sdcard_isInserted()) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + if (sdcard.state >= SDCARD_STATE_READY) { spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); } diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 09b6663e86..852a60831e 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -114,8 +114,8 @@ static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { if (options & SERIAL_BIDIR) { - ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, - (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD, + ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, + (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD, (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 97f087a510..f79a99ae6e 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -64,17 +64,17 @@ typedef struct uartDevice_s { //static uartPort_t uartPort[MAX_UARTS]; #ifdef USE_UART1 -static uartDevice_t uart1 = -{ +static uartDevice_t uart1 = +{ .DMAChannel = DMA_Channel_4, .txDMAStream = DMA2_Stream7, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA2_Stream5, #endif - .dev = USART1, - .rx = IO_TAG(UART1_RX_PIN), - .tx = IO_TAG(UART1_TX_PIN), - .af = GPIO_AF_USART1, + .dev = USART1, + .rx = IO_TAG(UART1_RX_PIN), + .tx = IO_TAG(UART1_TX_PIN), + .af = GPIO_AF_USART1, #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif @@ -87,17 +87,17 @@ static uartDevice_t uart1 = #endif #ifdef USE_UART2 -static uartDevice_t uart2 = -{ +static uartDevice_t uart2 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART2_RX_DMA .rxDMAStream = DMA1_Stream5, #endif .txDMAStream = DMA1_Stream6, - .dev = USART2, - .rx = IO_TAG(UART2_RX_PIN), - .tx = IO_TAG(UART2_TX_PIN), - .af = GPIO_AF_USART2, + .dev = USART2, + .rx = IO_TAG(UART2_RX_PIN), + .tx = IO_TAG(UART2_TX_PIN), + .af = GPIO_AF_USART2, #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif @@ -110,17 +110,17 @@ static uartDevice_t uart2 = #endif #ifdef USE_UART3 -static uartDevice_t uart3 = -{ +static uartDevice_t uart3 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART3_RX_DMA .rxDMAStream = DMA1_Stream1, #endif .txDMAStream = DMA1_Stream3, - .dev = USART3, - .rx = IO_TAG(UART3_RX_PIN), - .tx = IO_TAG(UART3_TX_PIN), - .af = GPIO_AF_USART3, + .dev = USART3, + .rx = IO_TAG(UART3_RX_PIN), + .tx = IO_TAG(UART3_TX_PIN), + .af = GPIO_AF_USART3, #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif @@ -133,17 +133,17 @@ static uartDevice_t uart3 = #endif #ifdef USE_UART4 -static uartDevice_t uart4 = -{ +static uartDevice_t uart4 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA1_Stream2, #endif .txDMAStream = DMA1_Stream4, - .dev = UART4, - .rx = IO_TAG(UART4_RX_PIN), - .tx = IO_TAG(UART4_TX_PIN), - .af = GPIO_AF_UART4, + .dev = UART4, + .rx = IO_TAG(UART4_RX_PIN), + .tx = IO_TAG(UART4_TX_PIN), + .af = GPIO_AF_UART4, #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif @@ -156,17 +156,17 @@ static uartDevice_t uart4 = #endif #ifdef USE_UART5 -static uartDevice_t uart5 = -{ +static uartDevice_t uart5 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA1_Stream0, #endif .txDMAStream = DMA2_Stream7, - .dev = UART5, - .rx = IO_TAG(UART5_RX_PIN), - .tx = IO_TAG(UART5_TX_PIN), - .af = GPIO_AF_UART5, + .dev = UART5, + .rx = IO_TAG(UART5_RX_PIN), + .tx = IO_TAG(UART5_TX_PIN), + .af = GPIO_AF_UART5, #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif @@ -179,17 +179,17 @@ static uartDevice_t uart5 = #endif #ifdef USE_UART6 -static uartDevice_t uart6 = -{ +static uartDevice_t uart6 = +{ .DMAChannel = DMA_Channel_5, #ifdef USE_UART6_RX_DMA .rxDMAStream = DMA2_Stream1, #endif .txDMAStream = DMA2_Stream6, - .dev = USART6, - .rx = IO_TAG(UART6_RX_PIN), - .tx = IO_TAG(UART6_TX_PIN), - .af = GPIO_AF_USART6, + .dev = USART6, + .rx = IO_TAG(UART6_RX_PIN), + .tx = IO_TAG(UART6_TX_PIN), + .af = GPIO_AF_USART6, #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif @@ -344,7 +344,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); } - if (mode & MODE_RX) { + if (mode & MODE_RX) { IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device)); IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af); } @@ -376,6 +376,7 @@ void USART1_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port); uartIrqHandler(s); } + #endif #ifdef USE_UART2 diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index f225dd62bb..8360b0068f 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -77,27 +77,25 @@ static uint8_t usbVcpRead(serialPort_t *instance) uint8_t buf[1]; - uint32_t rxed = 0; - - while (rxed < 1) { - rxed += CDC_Receive_DATA((uint8_t*)buf + rxed, 1 - rxed); + while (true) { + if (CDC_Receive_DATA(buf, 1)) + return buf[0]; } - - return buf[0]; } static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) { UNUSED(instance); - if (!(usbIsConnected() && usbIsConfigured())) { return; } uint32_t start = millis(); - for (uint8_t *p = data; count > 0; ) { - uint32_t txed = CDC_Send_DATA(p, count); + uint8_t *p = data; + uint32_t txed = 0; + while (count > 0) { + txed = CDC_Send_DATA(p, count); count -= txed; p += txed; @@ -120,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port) return false; } - uint32_t txed; uint32_t start = millis(); + uint8_t *p = port->txBuf; + uint32_t txed = 0; + while (count > 0) { + txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; - do { - txed = CDC_Send_DATA(port->txBuf, count); - } while (txed != count && (millis() - start < USB_TIMEOUT)); - - return txed == count; + if (millis() - start > USB_TIMEOUT) { + break; + } + } + return count == 0; } static void usbVcpWrite(serialPort_t *instance, uint8_t c) diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index f5eab63fac..3e33c545b8 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -36,7 +36,7 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index e0aaad1bb0..fb30936968 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -34,7 +34,7 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 8101974377..5c09ebdf6a 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -41,7 +41,7 @@ void systemReset(void) NVIC_SystemReset(); } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { if (mpuConfiguration.reset) mpuConfiguration.reset(); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index a3b3624371..15e36cce3f 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -253,7 +253,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) } #else TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; -#endif +#endif TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 26078d4517..7050bbabda 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -87,8 +87,9 @@ typedef struct timerHardware_s { } timerHardware_t; enum { - TIMER_OUTPUT_ENABLED = 0x01, - TIMER_OUTPUT_INVERTED = 0x02 + TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_INVERTED = 0x02, + TIMER_OUTPUT_N_CHANNEL= 0x04 }; #ifdef STM32F1 diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 09ef0efc8c..1a5085e63a 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -23,10 +23,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { { .TIMx = TIM8, .rcc = RCC_APB1(TIM8) }, { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, #endif }; diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 033aa316c4..04069b20c3 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -54,7 +54,7 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { */ #define CCMR_OFFSET ((uint16_t)0x0018) #define CCMR_OC13M_MASK ((uint32_t)0xFFFEFF8F) -#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF) +#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF) void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode) { diff --git a/src/main/drivers/usb_io.c b/src/main/drivers/usb_io.c index d8a7206109..8520ef8e54 100644 --- a/src/main/drivers/usb_io.c +++ b/src/main/drivers/usb_io.c @@ -66,7 +66,7 @@ bool usbCableIsInserted(void) void usbGenerateDisconnectPulse(void) { /* Pull down PA12 to create USB disconnect pulse */ - IO_t usbPin = IOGetByTag(IO_TAG(PA12)); + IO_t usbPin = IOGetByTag(IO_TAG(PA12)); IOConfigGPIO(usbPin, IOCFG_OUT_OD); IOHi(usbPin); diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index ee540da60e..93ffa83ecb 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -179,7 +179,7 @@ void rtc6705SetFreq(uint16_t freq) uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers) uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers) - + val_hex |= RTC6705_SET_WRITE; val_hex |= (val_a << 5); val_hex |= (val_n << 12); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b851fd355e..f8ac94b22b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -65,13 +65,11 @@ static flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; -static bool syncPwm = false; +static bool syncPwmWithPidLoop = false; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -float errorLimiter = 1.0f; - #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; @@ -427,7 +425,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura motorCount = 0; servoCount = pwmOutputConfiguration->servoCount; - syncPwm = use_unsyncedPwm; + syncPwmWithPidLoop = !use_unsyncedPwm; if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer @@ -528,9 +526,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) customMixers = initialCustomMixers; } -void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) +void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) { UNUSED(pwmOutputConfiguration); + + syncPwmWithPidLoop = !use_unsyncedPwm; + motorCount = 4; #ifdef USE_SERVOS servoCount = 0; @@ -644,7 +645,7 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - if (syncPwm) { + if (syncPwmWithPidLoop) { pwmCompleteOneshotMotorUpdate(motorCount); } } @@ -752,13 +753,14 @@ STATIC_UNIT_TESTED void servoMixer(void) #endif -void mixTable(void) +void mixTable(void *pidProfilePtr) { uint32_t i = 0; fix12_t vbatCompensationFactor = 0; static fix12_t mixReduction; bool use_vbat_compensation = false; - if (batteryConfig && batteryConfig->vbatPidCompensation) { + pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; + if (batteryConfig && pidProfile->vbatPidCompensation) { use_vbat_compensation = true; vbatCompensationFactor = calculateVbatPidCompensation(); } @@ -827,21 +829,40 @@ void mixTable(void) rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center - if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2); + throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } - // adjust feedback to scale PID error inputs to our limitations. - errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f); - if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100; + // Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math + float motorDtms; + if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { + static uint32_t previousMotorTime; + uint32_t currentMotorTime = micros(); + motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f; + previousMotorTime = currentMotorTime; + } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); + // Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic + if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { + static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS]; + + // acceleration limit + float delta = motor[i] - lastFilteredMotor[i]; + const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f); + float maxDelta = maxDeltaPerMs * motorDtms; + if (delta > maxDelta) { // accelerating too hard + motor[i] = lastFilteredMotor[i] + maxDelta; + } + lastFilteredMotor[i] = motor[i]; + } + if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { @@ -860,19 +881,6 @@ void mixTable(void) motor[i] = escAndServoConfig->mincommand; } } - - // Experimental Code. Anti Desync feature for ESC's - if (escAndServoConfig->escDesyncProtection) { - const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000); - - // Only makes sense when it's within the range - if (maxThrottleStep < throttleRange) { - static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; - - motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); - motorPrevious[i] = motor[i]; - } - } } // Disarmed mode @@ -956,7 +964,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); + biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4f4869b205..b76aafa1bd 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -211,7 +211,7 @@ void loadCustomServoMixer(void); int servoDirection(int servoIndex, int fromChannel); #endif void mixerResetDisarmedMotors(void); -void mixTable(void); +void mixTable(void *pidProfilePtr); void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b0dee87496..41c8ef838b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,9 +49,10 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float errorLimiter; extern float angleRate[3], angleRateSmooth[3]; +static bool pidStabilisationEnabled; + int16_t axisPID[3]; #ifdef BLACKBOX @@ -62,46 +63,31 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; uint8_t PIDweight[3]; static int32_t errorGyroI[3]; -#ifndef SKIP_PID_LUXFLOAT static float errorGyroIf[3]; -#endif -static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); -pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using +pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using -void setTargetPidLooptime(uint8_t pidProcessDenom) +void setTargetPidLooptime(uint8_t pidProcessDenom) { targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } -uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) -{ - uint16_t dynamicKi; - uint16_t resetRate; - - resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - - uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8); - - dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - return dynamicKi; -} - void pidResetErrorGyroState(void) { - int axis; - - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; -#ifndef SKIP_PID_LUXFLOAT errorGyroIf[axis] = 0.0f; -#endif } } +void pidStabilisationState(pidStabilisationState_e pidControllerState) +{ + pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; +} + float getdT (void) { static float dT; @@ -115,8 +101,8 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; -#ifndef SKIP_PID_LUXFLOAT -static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage +static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { float RateError = 0, gyroRate = 0, RateErrorSmooth = 0; @@ -129,9 +115,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float // Scaling factors for Pids for better tunable range in configurator - static const float luxPTermScale = 1.0f / 128; - static const float luxITermScale = 1000000.0f / 0x1000000; - static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508; + static const float PTermScale = 0.032029f; + static const float ITermScale = 0.244381f; + static const float DTermScale = 0.000529f; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -147,6 +133,25 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } } + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { @@ -154,23 +159,23 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; + angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f); + angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } - gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion + gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes @@ -178,6 +183,34 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = angleRate[axis] - gyroRate; + float dynReduction = tpaFactor; + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(RateError) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (RateError < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (RateError > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { // Smoothed Error for Derivative when delta from error selected if (flightModeFlags && axis != YAW) @@ -187,7 +220,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate P component - PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; + PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -195,9 +228,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate I component. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis]; + // Prevent strong Iterm accumulation during stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -232,12 +267,14 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // Filter delta if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); + DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); } + if (!pidStabilisationEnabled) axisPID[axis] = 0; + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -251,9 +288,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat #endif } } -#endif -static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future +static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { int axis; @@ -330,11 +367,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis]; + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - int32_t rateErrorLimited = errorLimiter * RateError; - - errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -377,6 +415,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin axisPID[axis] = PTerm + ITerm + DTerm; } + if (!pidStabilisationEnabled) axisPID[axis] = 0; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { @@ -396,13 +435,11 @@ void pidSetController(pidControllerType_e type) { switch (type) { default: - case PID_CONTROLLER_INTEGER: - pid_controller = pidInteger; + case PID_CONTROLLER_LEGACY: + pid_controller = pidLegacy; break; -#ifndef SKIP_PID_LUXFLOAT - case PID_CONTROLLER_FLOAT: - pid_controller = pidFloat; -#endif + case PID_CONTROLLER_BETAFLIGHT: + pid_controller = pidBetaflight; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a407aeb80a..64182f8256 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -41,8 +41,8 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets - PID_CONTROLLER_FLOAT, + PID_CONTROLLER_LEGACY = 0, // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future + PID_CONTROLLER_BETAFLIGHT, // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future PID_COUNT } pidControllerType_e; @@ -57,10 +57,18 @@ typedef enum { SUPEREXPO_YAW_ALWAYS } pidSuperExpoYaw_e; -#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) +typedef enum { + NEGATIVE_ERROR = 0, + POSITIVE_ERROR +} pidErrorPolarity_e; + +typedef enum { + PID_STABILISATION_OFF = 0, + PID_STABILISATION_ON +} pidStabilisationState_e; typedef struct pidProfile_s { - uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid + uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; @@ -73,7 +81,15 @@ typedef struct pidProfile_s { uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I) + uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage + uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. + + // Betaflight PID controller parameters + uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances + uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage + uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in + uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms + uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune @@ -97,5 +113,6 @@ extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); +void pidStabilisationState(pidStabilisationState_e pidControllerState); void setTargetPidLooptime(uint8_t pidProcessDenom); diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index b4ed924130..ed02aee30a 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -41,7 +41,7 @@ typedef enum { BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save prefered beeper configuration + BEEPER_PREFERENCE, // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum } beeperMode_e; diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index b73f9920f3..66cd7db59e 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,5 +24,4 @@ typedef struct escAndServoConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. - uint16_t escDesyncProtection; // Value that a motor is allowed to increase or decrease in a period of 1ms } escAndServoConfig_t; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index cf8e141aec..af963adcdc 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1069,7 +1069,7 @@ static void gpsHandlePassthrough(uint8_t data) #endif } - + void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) { diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index e74ca001a0..573d562374 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -42,6 +42,7 @@ #include #include "common/axis.h" +#include "common/utils.h" #include "io/rc_controls.h" @@ -61,6 +62,8 @@ #include "io/osd.h" #include "io/vtx.h" +#include "rx/rx.h" + #include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -74,6 +77,13 @@ #include "config/config_profile.h" #include "config/config_master.h" +/* +PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); +*/ + static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -82,73 +92,13 @@ static void ledStripDisable(void); //#define USE_LED_ANIMATION //#define USE_LED_RING_DEFAULT_CONFIG -// timers -#ifdef USE_LED_ANIMATION -static uint32_t nextAnimationUpdateAt = 0; +#define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) +#define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) + +#if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +# error "Led strip length must match driver" #endif -static uint32_t nextIndicatorFlashAt = 0; -static uint32_t nextWarningFlashAt = 0; -static uint32_t nextRotationUpdateAt = 0; - -#define LED_STRIP_20HZ ((1000 * 1000) / 20) -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - -#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH -#error "Led strip length must match driver" -#endif - -// H S V -#define LED_BLACK { 0, 0, 0} -#define LED_WHITE { 0, 255, 255} -#define LED_RED { 0, 0, 255} -#define LED_ORANGE { 30, 0, 255} -#define LED_YELLOW { 60, 0, 255} -#define LED_LIME_GREEN { 90, 0, 255} -#define LED_GREEN {120, 0, 255} -#define LED_MINT_GREEN {150, 0, 255} -#define LED_CYAN {180, 0, 255} -#define LED_LIGHT_BLUE {210, 0, 255} -#define LED_BLUE {240, 0, 255} -#define LED_DARK_VIOLET {270, 0, 255} -#define LED_MAGENTA {300, 0, 255} -#define LED_DEEP_PINK {330, 0, 255} - -const hsvColor_t hsv_black = LED_BLACK; -const hsvColor_t hsv_white = LED_WHITE; -const hsvColor_t hsv_red = LED_RED; -const hsvColor_t hsv_orange = LED_ORANGE; -const hsvColor_t hsv_yellow = LED_YELLOW; -const hsvColor_t hsv_limeGreen = LED_LIME_GREEN; -const hsvColor_t hsv_green = LED_GREEN; -const hsvColor_t hsv_mintGreen = LED_MINT_GREEN; -const hsvColor_t hsv_cyan = LED_CYAN; -const hsvColor_t hsv_lightBlue = LED_LIGHT_BLUE; -const hsvColor_t hsv_blue = LED_BLUE; -const hsvColor_t hsv_darkViolet = LED_DARK_VIOLET; -const hsvColor_t hsv_magenta = LED_MAGENTA; -const hsvColor_t hsv_deepPink = LED_DEEP_PINK; - -#define LED_DIRECTION_COUNT 6 - -const hsvColor_t * const defaultColors[] = { - &hsv_black, - &hsv_white, - &hsv_red, - &hsv_orange, - &hsv_yellow, - &hsv_limeGreen, - &hsv_green, - &hsv_mintGreen, - &hsv_cyan, - &hsv_lightBlue, - &hsv_blue, - &hsv_darkViolet, - &hsv_magenta, - &hsv_deepPink -}; - typedef enum { COLOR_BLACK = 0, COLOR_WHITE, @@ -164,98 +114,134 @@ typedef enum { COLOR_DARK_VIOLET, COLOR_MAGENTA, COLOR_DEEP_PINK, -} colorIds; +} colorId_e; -typedef enum { - DIRECTION_NORTH = 0, - DIRECTION_EAST, - DIRECTION_SOUTH, - DIRECTION_WEST, - DIRECTION_UP, - DIRECTION_DOWN -} directionId_e; - -typedef struct modeColorIndexes_s { - uint8_t north; - uint8_t east; - uint8_t south; - uint8_t west; - uint8_t up; - uint8_t down; -} modeColorIndexes_t; - - -// Note, the color index used for the mode colors below refer to the default colors. -// if the colors are reconfigured the index is still valid but the displayed color might -// be different. -// See colors[] and defaultColors[] and applyDefaultColors[] - -static const modeColorIndexes_t orientationModeColors = { - COLOR_WHITE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +const hsvColor_t hsv[] = { + // H S V + [COLOR_BLACK] = { 0, 0, 0}, + [COLOR_WHITE] = { 0, 255, 255}, + [COLOR_RED] = { 0, 0, 255}, + [COLOR_ORANGE] = { 30, 0, 255}, + [COLOR_YELLOW] = { 60, 0, 255}, + [COLOR_LIME_GREEN] = { 90, 0, 255}, + [COLOR_GREEN] = {120, 0, 255}, + [COLOR_MINT_GREEN] = {150, 0, 255}, + [COLOR_CYAN] = {180, 0, 255}, + [COLOR_LIGHT_BLUE] = {210, 0, 255}, + [COLOR_BLUE] = {240, 0, 255}, + [COLOR_DARK_VIOLET] = {270, 0, 255}, + [COLOR_MAGENTA] = {300, 0, 255}, + [COLOR_DEEP_PINK] = {330, 0, 255}, }; +// macro to save typing on default colors +#define HSV(color) (hsv[COLOR_ ## color]) -static const modeColorIndexes_t headfreeModeColors = { - COLOR_LIME_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +STATIC_UNIT_TESTED uint8_t ledGridWidth; +STATIC_UNIT_TESTED uint8_t ledGridHeight; +// grid offsets +STATIC_UNIT_TESTED uint8_t highestYValueForNorth; +STATIC_UNIT_TESTED uint8_t lowestYValueForSouth; +STATIC_UNIT_TESTED uint8_t highestXValueForWest; +STATIC_UNIT_TESTED uint8_t lowestXValueForEast; + +STATIC_UNIT_TESTED ledCounts_t ledCounts; + +// macro for initializer +#define LF(name) LED_FUNCTION_ ## name +#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) +#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) + +#ifdef USE_LED_RING_DEFAULT_CONFIG +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), }; +#else +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), -static const modeColorIndexes_t horizonModeColors = { - COLOR_BLUE, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), -static const modeColorIndexes_t angleModeColors = { - COLOR_CYAN, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), -#ifdef MAG -static const modeColorIndexes_t magModeColors = { - COLOR_MINT_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE }; #endif -static const modeColorIndexes_t baroModeColors = { - COLOR_LIGHT_BLUE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +#undef LD +#undef LF +#undef LO + +static const modeColorIndexes_t defaultModeColors[] = { + // NORTH EAST SOUTH WEST UP DOWN + [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, +}; + +static const specialColorIndexes_t defaultSpecialColors[] = { + {{ [LED_SCOLOR_DISARMED] = COLOR_GREEN, + [LED_SCOLOR_ARMED] = COLOR_BLUE, + [LED_SCOLOR_ANIMATION] = COLOR_WHITE, + [LED_SCOLOR_BACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_GPSNOSATS] = COLOR_RED, + [LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE, + [LED_SCOLOR_GPSLOCKED] = COLOR_GREEN, + }} }; -uint8_t ledGridWidth; -uint8_t ledGridHeight; -uint8_t ledCount; -uint8_t ledsInRingCount; - -ledConfig_t *ledConfigs; -hsvColor_t *colors; +static int scaledThrottle; + + +/* #ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, @@ -273,16 +259,16 @@ const ledConfig_t defaultLedStripConfig[] = { }; #elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, }; #else const ledConfig_t defaultLedStripConfig[] = { @@ -326,118 +312,58 @@ const ledConfig_t defaultLedStripConfig[] = { }; #endif +*/ -/* - * 6 coords @nn,nn - * 4 direction @## - * 6 modes @#### - * = 16 bytes per led - * 16 * 32 leds = 512 bytes storage needed worst case. - * = not efficient to store led configs as strings in flash. - * = becomes a problem to send all the data via cli due to serial/cli buffers - */ -typedef enum { - X_COORDINATE, - Y_COORDINATE, - DIRECTIONS, - FUNCTIONS, - RING_COLORS -} parseState_e; +static void updateLedRingCounts(void); -#define PARSE_STATE_COUNT 5 - -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; - -static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) -static const uint8_t directionMappings[DIRECTION_COUNT] = { - LED_DIRECTION_NORTH, - LED_DIRECTION_EAST, - LED_DIRECTION_SOUTH, - LED_DIRECTION_WEST, - LED_DIRECTION_UP, - LED_DIRECTION_DOWN -}; - -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; -#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) -static const uint16_t functionMappings[FUNCTION_COUNT] = { - LED_FUNCTION_INDICATOR, - LED_FUNCTION_WARNING, - LED_FUNCTION_FLIGHT_MODE, - LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE, - LED_FUNCTION_THRUST_RING, - LED_FUNCTION_COLOR -}; - -// grid offsets -uint8_t highestYValueForNorth; -uint8_t lowestYValueForSouth; -uint8_t highestXValueForWest; -uint8_t lowestXValueForEast; - -void determineLedStripDimensions(void) +STATIC_UNIT_TESTED void determineLedStripDimensions(void) { - ledGridWidth = 0; - ledGridHeight = 0; + int maxX = 0; + int maxY = 0; - uint8_t ledIndex; - const ledConfig_t *ledConfig; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - - if (GET_LED_X(ledConfig) >= ledGridWidth) { - ledGridWidth = GET_LED_X(ledConfig) + 1; - } - if (GET_LED_Y(ledConfig) >= ledGridHeight) { - ledGridHeight = GET_LED_Y(ledConfig) + 1; - } + maxX = MAX(ledGetX(ledConfig), maxX); + maxY = MAX(ledGetY(ledConfig), maxY); } + ledGridWidth = maxX + 1; + ledGridHeight = maxY + 1; } -void determineOrientationLimits(void) +STATIC_UNIT_TESTED void determineOrientationLimits(void) { - bool isOddHeight = (ledGridHeight & 1); - bool isOddWidth = (ledGridWidth & 1); - uint8_t heightModifier = isOddHeight ? 1 : 0; - uint8_t widthModifier = isOddWidth ? 1 : 0; - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = (ledGridHeight / 2) + heightModifier; + lowestYValueForSouth = ((ledGridHeight + 1) / 2); highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = (ledGridWidth / 2) + widthModifier; + lowestXValueForEast = ((ledGridWidth + 1) / 2); } -void updateLedCount(void) +STATIC_UNIT_TESTED void updateLedCount(void) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; - ledCount = 0; - ledsInRingCount = 0; + int count = 0, countRing = 0, countScanner= 0; - if( ledConfigs == 0 ){ - return; - } + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (ledConfig->flags == 0 && ledConfig->xy == 0) { + if (!(*ledConfig)) break; - } - ledCount++; + count++; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - ledsInRingCount++; - } + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) + countRing++; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) + countScanner++; } + + ledCounts.count = count; + ledCounts.ring = countRing; + ledCounts.larson = countScanner; } void reevaluateLedConfig(void) @@ -445,499 +371,714 @@ void reevaluateLedConfig(void) updateLedCount(); determineLedStripDimensions(); determineOrientationLimits(); + updateLedRingCounts(); } -#define CHUNK_BUFFER_SIZE 10 - -#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) - - -bool parseLedStripConfig(uint8_t ledIndex, const char *config) +// get specialColor by index +static hsvColor_t* getSC(ledSpecialColorIds_e index) { - char chunk[CHUNK_BUFFER_SIZE]; - uint8_t chunkIndex; - uint8_t val; + return &masterConfig.colors[masterConfig.specialColors.color[index]]; +} - uint8_t parseState = X_COORDINATE; - bool ok = true; +static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; +static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' }; +static const char overlayCodes[LED_OVERLAY_COUNT] = { 'T', 'O', 'B', 'N', 'I', 'W' }; - if (ledIndex >= MAX_LED_STRIP_LENGTH) { - return !ok; - } +#define CHUNK_BUFFER_SIZE 11 - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; +bool parseLedStripConfig(int ledIndex, const char *config) +{ + if (ledIndex >= LED_MAX_STRIP_LENGTH) + return false; + + enum parseState_e { + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS, + RING_COLORS, + PARSE_STATE_COUNT + }; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); - while (ok) { + int x = 0, y = 0, color = 0; // initialize to prevent warnings + int baseFunction = 0; + int overlay_flags = 0; + int direction_flags = 0; - char chunkSeparator = chunkSeparators[parseState]; - - memset(&chunk, 0, sizeof(chunk)); - chunkIndex = 0; - - while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { - chunk[chunkIndex++] = *config++; + for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) { + char chunk[CHUNK_BUFFER_SIZE]; + { + char chunkSeparator = chunkSeparators[parseState]; + int chunkIndex = 0; + while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) { + chunk[chunkIndex++] = *config++; + } + chunk[chunkIndex++] = 0; // zero-terminate chunk + if (*config != chunkSeparator) { + return false; + } + config++; // skip separator } - - if (*config++ != chunkSeparator) { - ok = false; - break; - } - - switch((parseState_e)parseState) { + switch (parseState) { case X_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_X(val); + x = atoi(chunk); break; case Y_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_Y(val); + y = atoi(chunk); break; case DIRECTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (directionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= directionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (directionCodes[dir] == *ch) { + direction_flags |= LED_FLAG_DIRECTION(dir); break; } } } break; case FUNCTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (functionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= functionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) { + if (baseFunctionCodes[fn] == *ch) { + baseFunction = fn; + break; + } + } + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (overlayCodes[ol] == *ch) { + overlay_flags |= LED_FLAG_OVERLAY(ol); break; } } } break; case RING_COLORS: - if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { - ledConfig->color = atoi(chunk); - } else { - ledConfig->color = 0; - } + color = atoi(chunk); + if (color >= LED_CONFIGURABLE_COLOR_COUNT) + color = 0; break; - default : - break; - } - - parseState++; - if (parseState >= PARSE_STATE_COUNT) { - break; + case PARSE_STATE_COUNT:; // prevent warning } } - if (!ok) { - memset(ledConfig, 0, sizeof(ledConfig_t)); - } + *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0); reevaluateLedConfig(); - return ok; + return true; } -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) { - char functions[FUNCTION_COUNT]; - char directions[DIRECTION_COUNT]; - uint8_t index; - uint8_t mappingIndex; + char directions[LED_DIRECTION_COUNT + 1]; + char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); - memset(&functions, 0, sizeof(functions)); - memset(&directions, 0, sizeof(directions)); - for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (ledConfig->flags & functionMappings[mappingIndex]) { - functions[index++] = functionCodes[mappingIndex]; + char *dptr = directions; + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (ledGetDirectionBit(ledConfig, dir)) { + *dptr++ = directionCodes[dir]; } } + *dptr = 0; - for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (ledConfig->flags & directionMappings[mappingIndex]) { - directions[index++] = directionCodes[mappingIndex]; + char *fptr = baseFunctionOverlays; + *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)]; + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (ledGetOverlayBit(ledConfig, ol)) { + *fptr++ = overlayCodes[ol]; } } + *fptr = 0; - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); + // TODO - check buffer length + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } -void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) -{ - // apply up/down colors regardless of quadrant. - if ((ledConfig->flags & LED_DIRECTION_UP)) { - setLedHsv(ledIndex, &colors[modeColors->up]); - } - - if ((ledConfig->flags & LED_DIRECTION_DOWN)) { - setLedHsv(ledIndex, &colors[modeColors->down]); - } - - // override with n/e/s/w colors to each n/s e/w half - bail at first match. - if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, &colors[modeColors->west]); - } - - if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, &colors[modeColors->east]); - } - - if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { - setLedHsv(ledIndex, &colors[modeColors->north]); - } - - if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { - setLedHsv(ledIndex, &colors[modeColors->south]); - } - -} typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST + // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW + QUADRANT_NORTH = 1 << 0, + QUADRANT_SOUTH = 1 << 1, + QUADRANT_EAST = 1 << 2, + QUADRANT_WEST = 1 << 3, + QUADRANT_NORTH_EAST = 1 << 4, + QUADRANT_SOUTH_EAST = 1 << 5, + QUADRANT_NORTH_WEST = 1 << 6, + QUADRANT_SOUTH_WEST = 1 << 7, + QUADRANT_NONE = 1 << 8, + QUADRANT_NOTDIAG = 1 << 9, // not in NE/SE/NW/SW + // values for test + QUADRANT_ANY = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE, } quadrant_e; -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +static quadrant_e getLedQuadrant(const int ledIndex) { - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + int x = ledGetX(ledConfig); + int y = ledGetY(ledConfig); - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; + int quad = 0; + if (y <= highestYValueForNorth) + quad |= QUADRANT_NORTH; + else if (y >= lowestYValueForSouth) + quad |= QUADRANT_SOUTH; + if (x >= lowestXValueForEast) + quad |= QUADRANT_EAST; + else if (x <= highestXValueForWest) + quad |= QUADRANT_WEST; + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH)) + && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led in one of NE/SE/NW/SW? + quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0)); + } else { + quad |= QUADRANT_NOTDIAG; + } + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0) + quad |= QUADRANT_NONE; + + return quad; +} + +static const struct { + uint8_t dir; // ledDirectionId_e + uint16_t quadrantMask; // quadrant_e +} directionQuadrantMap[] = { + {LED_DIRECTION_SOUTH, QUADRANT_SOUTH}, + {LED_DIRECTION_NORTH, QUADRANT_NORTH}, + {LED_DIRECTION_EAST, QUADRANT_EAST}, + {LED_DIRECTION_WEST, QUADRANT_WEST}, + {LED_DIRECTION_DOWN, QUADRANT_ANY}, + {LED_DIRECTION_UP, QUADRANT_ANY}, +}; + +static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) +{ + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + quadrant_e quad = getLedQuadrant(ledIndex); + for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { + ledDirectionId_e dir = directionQuadrantMap[i].dir; + quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; + + if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) + return &masterConfig.colors[modeColors->color[dir]]; + } + return NULL; +} + + +// map flight mode to led mode, in order of priority +// flightMode == 0 is always active +static const struct { + uint16_t flightMode; + uint8_t ledMode; +} flightModeToLed[] = { + {HEADFREE_MODE, LED_MODE_HEADFREE}, +#ifdef MAG + {MAG_MODE, LED_MODE_MAG}, +#endif +#ifdef BARO + {BARO_MODE, LED_MODE_BARO}, +#endif + {HORIZON_MODE, LED_MODE_HORIZON}, + {ANGLE_MODE, LED_MODE_ANGLE}, + {0, LED_MODE_ORIENTATION}, +}; + +static void applyLedFixedLayers() +{ + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); + + int fn = ledGetFunction(ledConfig); + int hOffset = HSV_HUE_MAX; + + switch (fn) { + case LED_FUNCTION_COLOR: + color = masterConfig.colors[ledGetColor(ledConfig)]; + break; + + case LED_FUNCTION_FLIGHT_MODE: + for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) + if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { + color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + break; // stop on first match + } + break; + + case LED_FUNCTION_ARM_STATE: + color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); + break; + + case LED_FUNCTION_BATTERY: + color = HSV(RED); + hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); + break; + + case LED_FUNCTION_RSSI: + color = HSV(RED); + hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); + break; + + default: + break; + } + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += ((scaledThrottle - 10) * 4) / 3; + } + + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); + + setLedHsv(ledIndex, &color); - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; } } -void applyLedModeLayer(void) +static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { - const ledConfig_t *ledConfig; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - if (ledConfig->flags & LED_FUNCTION_COLOR) { - setLedHsv(ledIndex, &colors[ledConfig->color]); - } else { - setLedHsv(ledIndex, &hsv_black); - } - } - - if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { - if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { - if (!ARMING_FLAG(ARMED)) { - setLedHsv(ledIndex, &hsv_green); - } else { - setLedHsv(ledIndex, &hsv_blue); - } - } - continue; - } - - applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors); - - if (FLIGHT_MODE(HEADFREE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors); -#ifdef MAG - } else if (FLIGHT_MODE(MAG_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); -#endif -#ifdef BARO - } else if (FLIGHT_MODE(BARO_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors); -#endif - } else if (FLIGHT_MODE(HORIZON_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); - } else if (FLIGHT_MODE(ANGLE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors); - } + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if ((*ledConfig & mask) == mask) + setLedHsv(ledIndex, color); } } typedef enum { - WARNING_FLAG_NONE = 0, - WARNING_FLAG_LOW_BATTERY = (1 << 0), - WARNING_FLAG_FAILSAFE = (1 << 1), - WARNING_FLAG_ARMING_DISABLED = (1 << 2) + WARNING_ARMING_DISABLED, + WARNING_LOW_BATTERY, + WARNING_FAILSAFE, } warningFlags_e; -static uint8_t warningFlags = WARNING_FLAG_NONE; -void applyLedWarningLayer(uint8_t updateNow) +static void applyLedWarningLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; static uint8_t warningFlashCounter = 0; + static uint8_t warningFlags = 0; // non-zero during blinks - if (updateNow && warningFlashCounter == 0) { - warningFlags = WARNING_FLAG_NONE; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { - warningFlags |= WARNING_FLAG_LOW_BATTERY; - } - if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { - warningFlags |= WARNING_FLAG_FAILSAFE; - } - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { - warningFlags |= WARNING_FLAG_ARMING_DISABLED; - } - } - - if (warningFlags || warningFlashCounter > 0) { - const hsvColor_t *warningColor = &hsv_black; - - if ((warningFlashCounter & 1) == 0) { - if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { - warningColor = &hsv_green; - } - if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { - warningColor = &hsv_red; - } - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_yellow; - } - } else { - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_blue; - } - } - - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { - continue; - } - setLedHsv(ledIndex, warningColor); - } - } - - if (updateNow && (warningFlags || warningFlashCounter)) { + if (updateNow) { + // keep counter running, so it stays in sync with blink warningFlashCounter++; - if (warningFlashCounter == 20) { - warningFlashCounter = 0; + warningFlashCounter &= 0xF; + + if (warningFlashCounter == 0) { // update when old flags was processed + warningFlags = 0; + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) + warningFlags |= 1 << WARNING_LOW_BATTERY; + if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) + warningFlags |= 1 << WARNING_FAILSAFE; + if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + warningFlags |= 1 << WARNING_ARMING_DISABLED; } + *timer += LED_STRIP_HZ(10); + } + + if (warningFlags) { + const hsvColor_t *warningColor = NULL; + + bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ + warningFlags_e warningId = warningFlashCounter / 4; + if (warningFlags & (1 << warningId)) { + switch (warningId) { + case WARNING_ARMING_DISABLED: + warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK); + break; + case WARNING_LOW_BATTERY: + warningColor = colorOn ? &HSV(RED) : &HSV(BLACK); + break; + case WARNING_FAILSAFE: + warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); + break; + default:; + } + } + if (warningColor) + applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } +static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 1; + + if (updateNow) { + state = getBatteryState(); + + switch (state) { + case BATTERY_OK: + flash = false; + timeOffset = 1; + break; + case BATTERY_WARNING: + timeOffset = 2; + break; + default: + timeOffset = 8; + break; + } + flash = !flash; + } + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); + } +} + +static void applyLedRssiLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 0; + + if (updateNow) { + state = (rssi * 100) / 1023; + + if (state > 50) { + flash = false; + timeOffset = 1; + } else if (state > 20) { + timeOffset = 2; + } else { + timeOffset = 8; + } + flash = !flash; + } + + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); + } +} + +#ifdef GPS +static void applyLedGpsLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t gpsFlashCounter = 0; + static uint8_t gpsPauseCounter = 0; + const uint8_t blinkPauseLength = 4; + + if (updateNow) { + if (gpsPauseCounter > 0) { + gpsPauseCounter--; + } else if (gpsFlashCounter >= GPS_numSat) { + gpsFlashCounter = 0; + gpsPauseCounter = blinkPauseLength; + } else { + gpsFlashCounter++; + gpsPauseCounter = 1; + } + *timer += LED_STRIP_HZ(2.5); + } + + const hsvColor_t *gpsColor; + + if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { + gpsColor = getSC(LED_SCOLOR_GPSNOSATS); + } else { + bool colorOn = gpsPauseCounter == 0; // each interval starts with pause + if (STATE(GPS_FIX)) { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND); + } else { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS); + } + } + + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor); +} + +#endif + + #define INDICATOR_DEADBAND 25 -void applyLedIndicatorLayer(uint8_t indicatorFlashState) +static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static bool flash = 0; - if (!rxIsReceivingSignal()) { + if (updateNow) { + if (rxIsReceivingSignal()) { + // calculate update frequency + int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 + scale += (50 - INDICATOR_DEADBAND); // start increasing frequency right after deadband + *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale); // 5 - 50Hz update, 2.5 - 25Hz blink + + flash = !flash; + } else { + *timer += LED_STRIP_HZ(5); // try again soon + } + } + + if (!flash) return; + + const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color? + + quadrant_e quadrants = 0; + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST; + } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST; + } + if (rcCommand[PITCH] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST; + } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST; } - if (indicatorFlashState == 0) { - flashColor = &hsv_orange; - } else { - flashColor = &hsv_black; - } - - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { - continue; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { + if (getLedQuadrant(ledIndex) & quadrants) + setLedHsv(ledIndex, flashColor); } - - if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - } - - if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - - if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - } - - if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - } -} - -void applyLedThrottleLayer() -{ - const ledConfig_t *ledConfig; - hsvColor_t color; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { - continue; - } - - getLedHsv(ledIndex, &color); - - int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); - scaled += HSV_HUE_MAX; - color.h = (color.h + scaled) % HSV_HUE_MAX; - setLedHsv(ledIndex, &color); } } #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off -#define ROTATION_SEQUENCE_LED_WIDTH 2 +#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on -#define RING_PATTERN_NOT_CALCULATED 255 - -void applyLedThrustRingLayer(void) +static void updateLedRingCounts(void) { - const ledConfig_t *ledConfig; - hsvColor_t ringColor; - uint8_t ledIndex; - - // initialised to special value instead of using more memory for a flag. - static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; - static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - - uint8_t ledRingIndex = 0; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { - continue; + int seqLen; + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) { + seqLen = ROTATION_SEQUENCE_LED_COUNT; + } else { + seqLen = ledCounts.ring; + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + // TODO - improve partitioning (15 leds -> 3x5) + while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) { + seqLen /= 2; } + } + ledCounts.ringSeqLen = seqLen; +} - bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { - applyColor = true; - } - } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; - } +static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t rotationPhase; + int ledRingIndex = 0; - if (applyColor) { - ringColor = colors[ledConfig->color]; - } else { - ringColor = hsv_black; - } + if (updateNow) { + rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } - uint8_t ledRingLedCount = ledRingIndex; - if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { - // update ring pattern according to total number of ring leds found + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { - rotationSeqLedCount = ledRingLedCount; - - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { - rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; - } else { - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { - rotationSeqLedCount >>= 1; + bool applyColor; + if (ARMING_FLAG(ARMED)) { + applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH; + } else { + applyColor = !(ledRingIndex % 2); // alternating pattern } + + if (applyColor) { + const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + setLedHsv(ledIndex, ringColor); + } + + ledRingIndex++; } - - // trigger start over - rotationPhase = 1; - } - - rotationPhase--; - if (rotationPhase == 0) { - rotationPhase = rotationSeqLedCount; } } +typedef struct larsonParameters_s { + uint8_t currentBrightness; + int8_t currentIndex; + int8_t direction; +} larsonParameters_t; + +static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex) +{ + int offset = larsonIndex - larsonParameters->currentIndex; + static const int larsonLowValue = 8; + + if (ABS(offset) > 1) + return (larsonLowValue); + + if (offset == 0) + return (larsonParameters->currentBrightness); + + if (larsonParameters->direction == offset) { + return (larsonParameters->currentBrightness - 127); + } + + return (255 - larsonParameters->currentBrightness); + +} + +static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta) +{ + if (larsonParameters->currentBrightness > (255 - delta)) { + larsonParameters->currentBrightness = 127; + if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) { + larsonParameters->direction = -larsonParameters->direction; + } + larsonParameters->currentIndex += larsonParameters->direction; + } else { + larsonParameters->currentBrightness += delta; + } +} + +static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) +{ + static larsonParameters_t larsonParameters = { 0, 0, 1 }; + + if (updateNow) { + larsonScannerNextStep(&larsonParameters, 15); + *timer += LED_STRIP_HZ(60); + } + + int scannerLedIndex = 0; + for (unsigned i = 0; i < ledCounts.count; i++) { + + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { + hsvColor_t ledColor; + getLedHsv(i, &ledColor); + ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex); + setLedHsv(i, &ledColor); + scannerLedIndex++; + } + } +} + +// blink twice, then wait ; either always or just when landing +static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) +{ + const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; + static uint16_t blinkMask; + + if (updateNow) { + blinkMask = blinkMask >> 1; + if (blinkMask <= 1) + blinkMask = blinkPattern; + + *timer += LED_STRIP_HZ(10); + } + + bool ledOn = (blinkMask & 1); // b_b_____... + if (!ledOn) { + for (int i = 0; i < ledCounts.count; ++i) { + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || + (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) { + setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND)); + } + } + } +} + + #ifdef USE_LED_ANIMATION -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; -void updateLedAnimationState(void) +static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) { static uint8_t frameCounter = 0; - - uint8_t animationFrames = ledGridHeight; - - previousRow = (frameCounter + animationFrames - 1) % animationFrames; - currentRow = frameCounter; - nextRow = (frameCounter + 1) % animationFrames; - - frameCounter = (frameCounter + 1) % animationFrames; -} - -static void applyLedAnimationLayer(void) -{ - const ledConfig_t *ledConfig; - - if (ARMING_FLAG(ARMED)) { - return; + const int animationFrames = ledGridHeight; + if(updateNow) { + frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; + *timer += LED_STRIP_HZ(20); } - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + if (ARMING_FLAG(ARMED)) + return; - ledConfig = &ledConfigs[ledIndex]; + int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1; + int currentRow = frameCounter; + int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; - if (GET_LED_Y(ledConfig) == previousRow) { - setLedHsv(ledIndex, &hsv_white); + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + if (ledGetY(ledConfig) == previousRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); scaleLedValue(ledIndex, 50); - - } else if (GET_LED_Y(ledConfig) == currentRow) { - setLedHsv(ledIndex, &hsv_white); - } else if (GET_LED_Y(ledConfig) == nextRow) { + } else if (ledGetY(ledConfig) == currentRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); + } else if (ledGetY(ledConfig) == nextRow) { scaleLedValue(ledIndex, 50); } } } #endif +typedef enum { + timBlink, + timLarson, + timBattery, + timRssi, +#ifdef GPS + timGps, +#endif + timWarning, + timIndicator, +#ifdef USE_LED_ANIMATION + timAnimation, +#endif + timRing, + timTimerCount +} timId_e; + +static uint32_t timerVal[timTimerCount]; + + +// function to apply layer. +// function must replan self using timer pointer +// when updateNow is true (timer triggered), state must be updated first, +// before calculating led state. Otherwise update started by different trigger +// may modify LED state. +typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); + + +static applyLayerFn_timed* layerTable[] = { + [timBlink] = &applyLedBlinkLayer, + [timLarson] = &applyLarsonScannerLayer, + [timBattery] = &applyLedBatteryLayer, + [timRssi] = &applyLedRssiLayer, +#ifdef GPS + [timGps] = &applyLedGpsLayer, +#endif + [timWarning] = &applyLedWarningLayer, + [timIndicator] = &applyLedIndicatorLayer, +#ifdef USE_LED_ANIMATION + [timAnimation] = &applyLedAnimationLayer, +#endif + [timRing] = &applyLedThrustRingLayer +}; + void updateLedStrip(void) { - if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } @@ -947,161 +1088,173 @@ void updateLedStrip(void) ledStripDisable(); ledStripEnabled = false; } - } else { - ledStripEnabled = true; - } - - if (!ledStripEnabled){ return; } - + ledStripEnabled = true; uint32_t now = micros(); - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; -#ifdef USE_LED_ANIMATION - bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; -#endif - if (!( - indicatorFlashNow || - rotationUpdateNow || - warningFlashNow -#ifdef USE_LED_ANIMATION - || animationUpdateNow -#endif - )) { - return; - } - - static uint8_t indicatorFlashState = 0; - - // LAYER 1 - applyLedModeLayer(); - applyLedThrottleLayer(); - - // LAYER 2 - - if (warningFlashNow) { - nextWarningFlashAt = now + LED_STRIP_10HZ; - } - applyLedWarningLayer(warningFlashNow); - - // LAYER 3 - - if (indicatorFlashNow) { - - uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; - uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; - uint8_t scale = MAX(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); - - if (indicatorFlashState == 0) { - indicatorFlashState = 1; - } else { - indicatorFlashState = 0; + // test all led timers, setting corresponding bits + uint32_t timActive = 0; + for (timId_e timId = 0; timId < timTimerCount; timId++) { + // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. + // max delay is limited to 5s + int32_t delta = cmp32(now, timerVal[timId]); + if (delta < 0 && delta > -LED_STRIP_MS(5000)) + continue; // not ready yet + timActive |= 1 << timId; + if (delta >= LED_STRIP_MS(100) || delta < 0) { + timerVal[timId] = now; } } - applyLedIndicatorLayer(indicatorFlashState); + if (!timActive) + return; // no change this update, keep old state -#ifdef USE_LED_ANIMATION - if (animationUpdateNow) { - nextAnimationUpdateAt = now + LED_STRIP_20HZ; - updateLedAnimationState(); + // apply all layers; triggered timed functions has to update timers + + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + + applyLedFixedLayers(); + + for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { + uint32_t *timer = &timerVal[timId]; + bool updateNow = timActive & (1 << timId); + (*layerTable[timId])(updateNow, timer); } - applyLedAnimationLayer(); -#endif - - if (rotationUpdateNow) { - - applyLedThrustRingLayer(); - - uint8_t animationSpeedScale = 1; - - if (ARMING_FLAG(ARMED)) { - animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); - } - - nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; - } - ws2811UpdateStrip(); } -bool parseColor(uint8_t index, const char *colorConfig) +bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &colors[index]; + hsvColor_t *color = &masterConfig.colors[index]; - bool ok = true; - - uint8_t componentIndex; - for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { - uint16_t val = atoi(remainingCharacters); + bool result = true; + static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { + [HSV_HUE] = HSV_HUE_MAX, + [HSV_SATURATION] = HSV_SATURATION_MAX, + [HSV_VALUE] = HSV_VALUE_MAX, + }; + for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { + int val = atoi(remainingCharacters); + if(val > hsv_limit[componentIndex]) { + result = false; + break; + } switch (componentIndex) { case HSV_HUE: - if (val > HSV_HUE_MAX) { - ok = false; - continue; - - } - colors[index].h = val; + color->h = val; break; case HSV_SATURATION: - if (val > HSV_SATURATION_MAX) { - ok = false; - continue; - } - colors[index].s = (uint8_t)val; + color->s = val; break; case HSV_VALUE: - if (val > HSV_VALUE_MAX) { - ok = false; - continue; - } - colors[index].v = (uint8_t)val; + color->v = val; break; } - remainingCharacters = strstr(remainingCharacters, ","); + remainingCharacters = strchr(remainingCharacters, ','); if (remainingCharacters) { - remainingCharacters++; + remainingCharacters++; // skip separator } else { - if (componentIndex < 2) { - ok = false; + if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) { + result = false; } } } - if (!ok) { - memset(color, 0, sizeof(hsvColor_t)); + if (!result) { + memset(color, 0, sizeof(*color)); } - return ok; + return result; } -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount) +/* + * Redefine a color in a mode. + * */ +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) { - memset(colors, 0, colorCount * sizeof(hsvColor_t)); - for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) { - *colors++ = *defaultColors[colorIndex]; + // check color + if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) + return false; + if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough + if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) + return false; + masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_SPECIAL) { + if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) + return false; + masterConfig.specialColors.color[modeColorIndex] = colorIndex; + } else { + return false; + } + return true; +} + +/* +void pgResetFn_ledConfigs(ledConfig_t *instance) +{ + memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); +} + +void pgResetFn_colors(hsvColor_t *instance) +{ + // copy hsv colors as default + BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); + + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *instance++ = hsv[colorIndex]; } } +void pgResetFn_modeColors(modeColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); +} + +void pgResetFn_specialColors(specialColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} +*/ + void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); + memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); reevaluateLedConfig(); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) +void applyDefaultColors(hsvColor_t *colors) +{ + // copy hsv colors as default + memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *colors++ = hsv[colorIndex]; + } +} + +void applyDefaultModeColors(modeColorIndexes_t *modeColors) +{ + memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); +} + +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) +{ + memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} + + + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; + modeColors = modeColorsToUse; + specialColors = *specialColorsToUse; ledStripInitialised = false; } @@ -1115,8 +1268,8 @@ void ledStripEnable(void) static void ledStripDisable(void) { - setStripColor(&hsv_black); - - ws2811UpdateStrip(); + setStripColor(&HSV(BLACK)); + + ws2811UpdateStrip(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 18915229b7..39b3cad6d2 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,81 +17,163 @@ #pragma once -#define MAX_LED_STRIP_LENGTH 32 -#define CONFIGURABLE_COLOR_COUNT 16 +#define LED_MAX_STRIP_LENGTH 32 +#define LED_CONFIGURABLE_COLOR_COUNT 16 +#define LED_MODE_COUNT 6 +#define LED_DIRECTION_COUNT 6 +#define LED_BASEFUNCTION_COUNT 7 +#define LED_OVERLAY_COUNT 6 +#define LED_SPECIAL_COLOR_COUNT 11 + +#define LED_POS_OFFSET 0 +#define LED_FUNCTION_OFFSET 8 +#define LED_OVERLAY_OFFSET 12 +#define LED_COLOR_OFFSET 18 +#define LED_DIRECTION_OFFSET 22 +#define LED_PARAMS_OFFSET 28 + +#define LED_POS_BITCNT 8 +#define LED_FUNCTION_BITCNT 4 +#define LED_OVERLAY_BITCNT 6 +#define LED_COLOR_BITCNT 4 +#define LED_DIRECTION_BITCNT 6 +#define LED_PARAMS_BITCNT 4 + +#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1) +#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1) + +#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET) +#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET) +#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET) +#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET) +#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET) +#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET) + +#define LED_BIT_MASK(len) ((1 << (len)) - 1) + +#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1)) +#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1)) +#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK) +#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1)) +#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK) +#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1)) + +#define LED_FLAG_OVERLAY(id) (1 << (id)) +#define LED_FLAG_DIRECTION(id) (1 << (id)) #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 - -#define LED_XY_MASK (0x0F) - -#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) -#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) - -#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) -#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) - - -#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) +#define LED_XY_MASK 0x0F +#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) typedef enum { - LED_DISABLED = 0, - LED_DIRECTION_NORTH = (1 << 0), - LED_DIRECTION_EAST = (1 << 1), - LED_DIRECTION_SOUTH = (1 << 2), - LED_DIRECTION_WEST = (1 << 3), - LED_DIRECTION_UP = (1 << 4), - LED_DIRECTION_DOWN = (1 << 5), - LED_FUNCTION_INDICATOR = (1 << 6), - LED_FUNCTION_WARNING = (1 << 7), - LED_FUNCTION_FLIGHT_MODE = (1 << 8), - LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10), - LED_FUNCTION_THRUST_RING = (1 << 11), - LED_FUNCTION_COLOR = (1 << 12), -} ledFlag_e; + LED_MODE_ORIENTATION = 0, + LED_MODE_HEADFREE, + LED_MODE_HORIZON, + LED_MODE_ANGLE, + LED_MODE_MAG, + LED_MODE_BARO, + LED_SPECIAL +} ledModeIndex_e; -#define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK ( \ - LED_DIRECTION_NORTH | \ - LED_DIRECTION_EAST | \ - LED_DIRECTION_SOUTH | \ - LED_DIRECTION_WEST | \ - LED_DIRECTION_UP | \ - LED_DIRECTION_DOWN \ -) -#define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK ( \ - LED_FUNCTION_INDICATOR | \ - LED_FUNCTION_WARNING | \ - LED_FUNCTION_FLIGHT_MODE | \ - LED_FUNCTION_ARM_STATE | \ - LED_FUNCTION_THROTTLE | \ - LED_FUNCTION_THRUST_RING | \ - LED_FUNCTION_COLOR \ -) +typedef enum { + LED_SCOLOR_DISARMED = 0, + LED_SCOLOR_ARMED, + LED_SCOLOR_ANIMATION, + LED_SCOLOR_BACKGROUND, + LED_SCOLOR_BLINKBACKGROUND, + LED_SCOLOR_GPSNOSATS, + LED_SCOLOR_GPSNOLOCK, + LED_SCOLOR_GPSLOCKED +} ledSpecialColorIds_e; + +typedef enum { + LED_DIRECTION_NORTH = 0, + LED_DIRECTION_EAST, + LED_DIRECTION_SOUTH, + LED_DIRECTION_WEST, + LED_DIRECTION_UP, + LED_DIRECTION_DOWN +} ledDirectionId_e; + +typedef enum { + LED_FUNCTION_COLOR, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE, + LED_FUNCTION_BATTERY, + LED_FUNCTION_RSSI, + LED_FUNCTION_GPS, + LED_FUNCTION_THRUST_RING, +} ledBaseFunctionId_e; + +typedef enum { + LED_OVERLAY_THROTTLE, + LED_OVERLAY_LARSON_SCANNER, + LED_OVERLAY_BLINK, + LED_OVERLAY_LANDING_FLASH, + LED_OVERLAY_INDICATOR, + LED_OVERLAY_WARNING, +} ledOverlayId_e; + +typedef struct modeColorIndexes_s { + uint8_t color[LED_DIRECTION_COUNT]; +} modeColorIndexes_t; + +typedef struct specialColorIndexes_s { + uint8_t color[LED_SPECIAL_COLOR_COUNT]; +} specialColorIndexes_t; + +typedef uint32_t ledConfig_t; + +typedef struct ledCounts_s { + uint8_t count; + uint8_t ring; + uint8_t larson; + uint8_t ringSeqLen; +} ledCounts_t; -typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines - uint8_t color; // see colors (config_master) - uint16_t flags; // see ledFlag_e -} ledConfig_t; +ledConfig_t *ledConfigs; +hsvColor_t *colors; +modeColorIndexes_t *modeColors; +specialColorIndexes_t specialColors; -extern uint8_t ledCount; -extern uint8_t ledsInRingCount; +#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params)) +static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); } +static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); } +static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); } +static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); } +static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); } +static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); } +static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); } +static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); } +/* +PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs); +PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors); +PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors); +PG_DECLARE(specialColorIndexes_t, specialColors); +*/ +bool parseColor(int index, const char *colorConfig); -bool parseLedStripConfig(uint8_t ledIndex, const char *config); +bool parseLedStripConfig(int ledIndex, const char *config); +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); +void reevaluateLedConfig(void); + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripEnable(void); void updateLedStrip(void); -void updateLedRing(void); + +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); + +extern uint16_t rssi; // FIXME dependency on mw.c + void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); +void applyDefaultColors(hsvColor_t *colors); +void applyDefaultModeColors(modeColorIndexes_t *modeColors); +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); -bool parseColor(uint8_t index, const char *colorConfig); -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); - -void ledStripEnable(void); -void reevaluateLedConfig(void); diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h new file mode 100644 index 0000000000..66ab4b9c58 --- /dev/null +++ b/src/main/io/msp_protocol.h @@ -0,0 +1,297 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/** + * MSP Guidelines, emphasis is used to clarify. + * + * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed. + * + * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier. + * + * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version + * if the API doesn't match EXACTLY. + * + * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command. + * If no response is obtained then client MAY try the legacy MSP_IDENT command. + * + * API consumers should ALWAYS handle communication failures gracefully and attempt to continue + * without the information if possible. Clients MAY log/display a suitable message. + * + * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION. + * + * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time + * the API client was written and handle command failures gracefully. Clients MAY disable + * functionality that depends on the commands while still leaving other functionality intact. + * that the newer API version may cause problems before using API commands that change FC state. + * + * It is for this reason that each MSP command should be specific as possible, such that changes + * to commands break as little functionality as possible. + * + * API client authors MAY use a compatibility matrix/table when determining if they can support + * a given command from a given flight controller at a given api version level. + * + * Developers MUST NOT create new MSP commands that do more than one thing. + * + * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools + * that use the API and the users of those tools. + */ + +#pragma once + +/* Protocol numbers used both by the wire format, config system, and + field setters. +*/ + +#define MSP_PROTOCOL_VERSION 0 + +#define API_VERSION_MAJOR 1 // increment when major changes are made +#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR + +#define API_VERSION_LENGTH 2 + +#define MULTIWII_IDENTIFIER "MWII"; +#define BASEFLIGHT_IDENTIFIER "BAFL"; +#define BETAFLIGHT_IDENTIFIER "BTFL" +#define CLEANFLIGHT_IDENTIFIER "CLFL" +#define INAV_IDENTIFIER "INAV" +#define RACEFLIGHT_IDENTIFIER "RCFL" + +#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 +#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 +#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF + +#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. +#define BOARD_HARDWARE_REVISION_LENGTH 2 + +// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version. +#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) +#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30) + +// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask. +#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31) +#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30) +#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29) +#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28) + +#define CAP_DYNBALANCE ((uint32_t)1 << 2) +#define CAP_FLAPS ((uint32_t)1 << 3) +#define CAP_NAVCAP ((uint32_t)1 << 4) +#define CAP_EXTAUX ((uint32_t)1 << 5) + +#define MSP_API_VERSION 1 //out message +#define MSP_FC_VARIANT 2 //out message +#define MSP_FC_VERSION 3 //out message +#define MSP_BOARD_INFO 4 //out message +#define MSP_BUILD_INFO 5 //out message + +#define MSP_NAME 10 //out message Returns user set board name - betaflight +#define MSP_SET_NAME 11 //in message Sets board name - betaflight + + +// +// MSP commands for Cleanflight original features +// +#define MSP_MODE_RANGES 34 //out message Returns all mode ranges +#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range + +#define MSP_FEATURE 36 +#define MSP_SET_FEATURE 37 + +#define MSP_BOARD_ALIGNMENT 38 +#define MSP_SET_BOARD_ALIGNMENT 39 + +#define MSP_CURRENT_METER_CONFIG 40 +#define MSP_SET_CURRENT_METER_CONFIG 41 + +#define MSP_MIXER 42 +#define MSP_SET_MIXER 43 + +#define MSP_RX_CONFIG 44 +#define MSP_SET_RX_CONFIG 45 + +#define MSP_LED_COLORS 46 +#define MSP_SET_LED_COLORS 47 + +#define MSP_LED_STRIP_CONFIG 48 +#define MSP_SET_LED_STRIP_CONFIG 49 + +#define MSP_RSSI_CONFIG 50 +#define MSP_SET_RSSI_CONFIG 51 + +#define MSP_ADJUSTMENT_RANGES 52 +#define MSP_SET_ADJUSTMENT_RANGE 53 + +// private - only to be used by the configurator, the commands are likely to change +#define MSP_CF_SERIAL_CONFIG 54 +#define MSP_SET_CF_SERIAL_CONFIG 55 + +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + +#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] + +#define MSP_PID_CONTROLLER 59 +#define MSP_SET_PID_CONTROLLER 60 + +#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters +#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters + +// +// Baseflight MSP commands (if enabled they exist in Cleanflight) +// +#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) +#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP + +// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. +// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. +#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save + +#define MSP_REBOOT 68 //in message reboot settings + +// DEPRECATED - Use MSP_BUILD_INFO instead +#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion + + +#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip +#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip +#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip + +#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter +#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter + +#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings +#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings + +#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings +#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings + +#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card + +#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings +#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings + +#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings + +#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight +#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight + +#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight +#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight + +#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight +#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight + +// Betaflight Additional Commands +#define MSP_PID_ADVANCED_CONFIG 90 +#define MSP_SET_PID_ADVANCED_CONFIG 91 + +#define MSP_FILTER_CONFIG 92 +#define MSP_SET_FILTER_CONFIG 93 + +#define MSP_ADVANCED_TUNING 94 +#define MSP_SET_ADVANCED_TUNING 95 + +#define MSP_SENSOR_CONFIG 96 +#define MSP_SET_SENSOR_CONFIG 97 + +#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility + +// +// OSD specific +// +#define MSP_OSD_VIDEO_CONFIG 180 +#define MSP_SET_OSD_VIDEO_CONFIG 181 + +// +// Multwii original MSP commands +// + +// DEPRECATED - See MSP_API_VERSION and MSP_MIXER +#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable + + +#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_RAW_IMU 102 //out message 9 DOF +#define MSP_SERVO 103 //out message servos +#define MSP_MOTOR 104 //out message motors +#define MSP_RC 105 //out message rc channels and more +#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course +#define MSP_COMP_GPS 107 //out message distance home, direction home +#define MSP_ATTITUDE 108 //out message 2 angles 1 heading +#define MSP_ALTITUDE 109 //out message altitude, variometer +#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX +#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 //out message P I D coeff (9 are used currently) +#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) +#define MSP_MISC 114 //out message powermeter trig +#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI +#define MSP_BOXNAMES 116 //out message the aux switch names +#define MSP_PIDNAMES 117 //out message the PID names +#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold +#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes +#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. +#define MSP_NAV_STATUS 121 //out message Returns navigation status +#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_3D 124 //out message Settings needed for reversible ESCs +#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll +#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag +#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings + +#define MSP_SET_RAW_RC 200 //in message 8 rc chan +#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed +#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) +#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) +#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo +#define MSP_ACC_CALIBRATION 205 //in message no param +#define MSP_MAG_CALIBRATION 206 //in message no param +#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use +#define MSP_RESET_CONF 208 //in message no param +#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) +#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) +#define MSP_SET_HEAD 211 //in message define a new heading hold direction +#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings +#define MSP_SET_MOTOR 214 //in message PropBalance function +#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom +#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs +#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll +#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults +#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag +#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings + +// #define MSP_BIND 240 //in message no param +// #define MSP_ALARMS 242 + +#define MSP_EEPROM_WRITE 250 //in message no param +#define MSP_RESERVE_1 251 //reserved for system usage +#define MSP_RESERVE_2 252 //reserved for system usage +#define MSP_DEBUGMSG 253 //out message debug string buffer +#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 +#define MSP_RESERVE_3 255 //reserved for system usage + +// Additional commands that are not compatible with MultiWii +#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc +#define MSP_UID 160 //out message Unique device ID +#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) +#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data +#define MSP_ACC_TRIM 240 //out message get acc angle trim values +#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values +#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration +#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration +#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 68896a9a89..f354577f87 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -672,8 +672,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; applySelectAdjustment(adjustmentFunction, position); } diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 75cbe4c8ef..be5e7311da 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -50,6 +50,7 @@ typedef enum { BOXFAILSAFE, BOXAIRMODE, BOX3DDISABLESWITCH, + BOXFPVANGLEMIX, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 890e218280..15aae3e469 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -465,7 +465,7 @@ static void nopConsumer(uint8_t data) arbitrary serial passthrough "proxy". Optional callbacks can be given to allow for specialized data processing. */ -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer +void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC) { waitForSerialPortToFinishTransmitting(left); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 7b783422fa..9dd777b462 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -25,9 +25,8 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" +#include "drivers/buf_writer.h" #include "drivers/gpio.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -36,158 +35,236 @@ #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" +#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) #include "io/serial_4way_stk500v2.h" +#endif #define USE_TXRX_LED -#if defined(USE_TXRX_LED) && defined(LED0) -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON #ifdef LED1 -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON #else -#define TX_LED_OFF LED0_OFF -#define TX_LED_ON LED0_ON +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON #endif #else -# define RX_LED_OFF do {} while(0) -# define RX_LED_ON do {} while(0) -# define TX_LED_OFF do {} while(0) -# define TX_LED_ON do {} while(0) +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON #endif #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" -#define SERIAL_4WAY_VER_MAIN 14 -#define SERIAL_4WAY_VER_SUB_1 4 -#define SERIAL_4WAY_VER_SUB_2 4 +// *** change to adapt Revision +#define SERIAL_4WAY_VER_MAIN 14 +#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 + #define SERIAL_4WAY_PROTOCOL_VER 106 +// *** end -#if SERIAL_4WAY_VER_MAIN > 24 -# error "SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1 must fit in uint8_t" -#endif -#if SERIAL_4WAY_VER_SUB_1 >= 10 -# warning "SERIAL_4WAY_VER_SUB_1 should be 0-9" +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" #endif -#if SERIAL_4WAY_VER_SUB_2 >= 100 -# warning "SERIAL_4WAY_VER_SUB_2 should be <= 99 (9.9)" -#endif +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) -#define SERIAL_4WAY_VERSION_HI (uint8_t)(SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1) -#define SERIAL_4WAY_VERSION_LO (uint8_t)(SERIAL_4WAY_VER_SUB_2) +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) static uint8_t escCount; -uint8_t escSelected; + escHardware_t escHardware[MAX_PWM_MOTORS]; -bool esc4wayExitRequested = false; +uint8_t selected_esc; -static escDeviceInfo_t deviceInfo; +uint8_32_u DeviceInfo; -static bool isMcuConnected(void) +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) { - return deviceInfo.signature != 0; + return (DeviceInfo.bytes[0] > 0); } -static void setDisconnected(void) -{ - deviceInfo.signature = 0; -} - -bool isEscHi(uint8_t selEsc) +inline bool isEscHi(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) != Bit_RESET); } - -bool isEscLo(uint8_t selEsc) +inline bool isEscLo(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) == Bit_RESET); } -void setEscHi(uint8_t selEsc) +inline void setEscHi(uint8_t selEsc) { IOHi(escHardware[selEsc].io); } -void setEscLo(uint8_t selEsc) +inline void setEscLo(uint8_t selEsc) { IOLo(escHardware[selEsc].io); } -void setEscInput(uint8_t selEsc) +inline void setEscInput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } -void setEscOutput(uint8_t selEsc) +inline void setEscOutput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } -// Initialize 4way ESC interface -// initializes internal structures -// returns number of ESCs available -int esc4wayInit(void) +uint8_t esc4wayInit(void) { // StopPwmAllMotors(); + pwmDisableMotors(); + escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - int escIdx = 0; for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escIdx].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); - escIdx++; + escHardware[escCount].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } } - escCount = escIdx; return escCount; } -// stat BLHeli 4way interface -// sets all ESC lines as output + hi -void esc4wayStart(void) -{ - pwmDisableMotors(); // prevent updating PWM registers - for (int i = 0; i < escCount; i++) { - setEscInput(i); - setEscHi(i); - } -} - -// stops BLHeli 4way interface -// returns all claimed pins back to PWM drivers, reenables PWM void esc4wayRelease(void) { - for(int i = 0; i < escCount; i++) { - IOConfigGPIO(escHardware[i].io, IOCFG_AF_PP); // see pwmOutConfig() - setEscOutput(i); - setEscLo(i); + while (escCount > 0) { + escCount--; + IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP); + setEscLo(escCount); } - escCount = 0; pwmEnableMotors(); } -// BLHeliSuite packet framing -// for reference, see 'Manuals/BLHeliSuite 4w-if protocol.pdf' from BLHeliSuite + +#define SET_DISCONNECTED DeviceInfo.words[0] = 0 + +#define INTF_MODE_IDX 3 // index for DeviceInfostate + +// Interface related only +// establish and test connection to the Interface + // Send Structure -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) CRC16_Hi CRC16_Lo +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo // Return -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo -// esc4wayCmd_e in public header +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' -typedef enum { - // not commands, but keep naming consistent with BLHeli suite - cmd_Remote_Escape = 0x2E, // '.' - cmd_Local_Escape = 0x2F, // '/' -} syn_4way_e; +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK +// get Protocol Version Number 01..255 +#define cmd_ProtocolGetVersion 0x31 // '1' version +// RETURN: uint8_t VersionNumber + ACK + +// get Version String +#define cmd_InterfaceGetName 0x32 // '2' name +// RETURN: String + ACK + +//get Version Number 01..255 +#define cmd_InterfaceGetVersion 0x33 // '3' version +// RETURN: uint8_t AVersionNumber + ACK + + +// Exit / Restart Interface - can be used to switch to Box Mode +#define cmd_InterfaceExit 0x34 // '4' exit +// RETURN: ACK + +// Reset the Device connected to the Interface +#define cmd_DeviceReset 0x35 // '5' reset +// RETURN: ACK + +// Get the Device ID connected +// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 +// RETURN: uint8_t DeviceID + ACK + +// Initialize Flash Access for Device connected +#define cmd_DeviceInitFlash 0x37 // '7' init flash access +// RETURN: ACK + +// Erase the whole Device Memory of connected Device +#define cmd_DeviceEraseAll 0x38 // '8' erase all +// RETURN: ACK + +// Erase one Page of Device Memory of connected Device +#define cmd_DevicePageErase 0x39 // '9' page erase +// PARAM: uint8_t APageNumber +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceRead 0x3A // ':' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWrite 0x3B // ';' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set C2CK low infinite ) permanent Reset state +#define cmd_DeviceC2CK_LOW 0x3C // '<' +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceReadEEprom 0x3D // '=' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWriteEEprom 0x3E // '>' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set Interface Mode +#define cmd_InterfaceSetMode 0x3F // '?' +// #define imC2 0 +// #define imSIL_BLB 1 +// #define imATM_BLB 2 +// #define imSK 3 +// PARAM: uint8_t Mode +// RETURN: ACK or ACK_I_INVALID_CHANNEL + +// responses +#define ACK_OK 0x00 +// #define ACK_I_UNKNOWN_ERROR 0x01 +#define ACK_I_INVALID_CMD 0x02 +#define ACK_I_INVALID_CRC 0x03 +#define ACK_I_VERIFY_ERROR 0x04 +// #define ACK_D_INVALID_COMMAND 0x05 +// #define ACK_D_COMMAND_FAILED 0x06 +// #define ACK_D_UNKNOWN_ERROR 0x07 + +#define ACK_I_INVALID_CHANNEL 0x08 +#define ACK_I_INVALID_PARAM 0x09 +#define ACK_D_GENERAL_ERROR 0x0F /* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz Copyright (c) 2005, 2007 Joerg Wunsch @@ -221,12 +298,11 @@ typedef enum { CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) -{ +uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { int i; crc = crc ^ ((uint16_t)data << 8); - for (i = 0; i < 8; i++){ + for (i=0; i < 8; i++){ if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -236,388 +312,514 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) } // * End copyright -static uint16_t signaturesAtmel[] = {0x9307, 0x930A, 0x930F, 0x940B, 0}; -static uint16_t signaturesSilabs[] = {0xF310, 0xF330, 0xF410, 0xF390, 0xF850, 0xE8B1, 0xE8B2, 0}; -static bool signatureMatch(uint16_t signature, uint16_t *list) +#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ + (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) + +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ + (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ + (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ + (pDeviceInfo->words[0] == 0xE8B2)) + +static uint8_t CurrentInterfaceMode; + +static uint8_t Connect(uint8_32_u *pDeviceInfo) { - for(; *list; list++) - if(signature == *list) - return true; - return false; -} - -static uint8_t currentInterfaceMode; - -// Try connecting to device -// 3 attempts are made, trying both STK and BL protocols. -static uint8_t connect(escDeviceInfo_t *pDeviceInfo) -{ - for (int try = 0; try < 3; try++) { -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (Stk_ConnectEx(pDeviceInfo) && signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imSK; + for (uint8_t I = 0; I < 3; ++I) { + #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { + CurrentInterfaceMode = imSK; return 1; + } else { + if (BL_ConnectEx(pDeviceInfo)) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } + } } -#endif -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if (BL_ConnectEx(pDeviceInfo)) { - if(signatureMatch(pDeviceInfo->signature, signaturesSilabs)) { - currentInterfaceMode = imSIL_BLB; + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; return 1; - } - if(signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imATM_BLB; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; return 1; } } -#endif + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo)) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } + #endif } return 0; } static serialPort_t *port; -static uint16_t crcIn, crcOut; -static uint8_t readByte(void) +static uint8_t ReadByte(void) { // need timeout? while (!serialRxBytesWaiting(port)); return serialRead(port); } -static uint8_t readByteCrc(void) +static uint8_16_u CRC_in; +static uint8_t ReadByteCrc(void) { - uint8_t b = readByte(); - crcIn = _crc_xmodem_update(crcIn, b); + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); return b; } -static void writeByte(uint8_t b) +static void WriteByte(uint8_t b) { serialWrite(port, b); } -static void writeByteCrc(uint8_t b) +static uint8_16_u CRCout; +static void WriteByteCrc(uint8_t b) { - writeByte(b); - crcOut = _crc_xmodem_update(crcOut, b); + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); } -// handle 4way interface on serial port -// esc4wayStart / esc4wayRelease in called internally -// 256 bytes buffer is allocated on stack -void esc4wayProcess(serialPort_t *serial) +void esc4wayProcess(serialPort_t *mspPort) { - uint8_t command; - uint16_t addr; - int inLen; - int outLen; - uint8_t paramBuf[256]; - uint8_t replyAck; - esc4wayStart(); + uint8_t ParamBuf[256]; + uint8_t ESC; + uint8_t I_PARAM_LEN; + uint8_t CMD; + uint8_t ACK_OUT; + uint8_16_u CRC_check; + uint8_16_u Dummy; + uint8_t O_PARAM_LEN; + uint8_t *O_PARAM; + uint8_t *InBuff; + ioMem_t ioMem; - port = serial; + port = mspPort; -#ifdef BEEPER + // Start here with UART Main loop + #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); -#endif + #endif + bool isExitScheduled = false; - esc4wayExitRequested = false; - while(!esc4wayExitRequested) { + while(1) { // restart looking for new sequence from host - crcIn = 0; - - uint8_t esc = readByteCrc(); - if(esc != cmd_Local_Escape) - continue; // wait for sync character + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); RX_LED_ON; - command = readByteCrc(); - addr = readByteCrc() << 8; - addr |= readByteCrc(); + Dummy.word = 0; + O_PARAM = &Dummy.bytes[0]; + O_PARAM_LEN = 1; + CMD = ReadByteCrc(); + ioMem.D_FLASH_ADDR_H = ReadByteCrc(); + ioMem.D_FLASH_ADDR_L = ReadByteCrc(); + I_PARAM_LEN = ReadByteCrc(); - inLen = readByteCrc(); - if(inLen == 0) - inLen = 0x100; // len ==0 -> param is 256B + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); - for(int i = 0; i < inLen; i++) - paramBuf[i] = readByteCrc(); + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); - readByteCrc(); readByteCrc(); // update input CRC - - outLen = 0; // output handling code will send single zero byte if necessary - replyAck = esc4wayAck_OK; - - if(crcIn != 0) // CRC of correct message == 0 - replyAck = esc4wayAck_I_INVALID_CRC; - - TX_LED_ON; - if (replyAck == esc4wayAck_OK) - replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); RX_LED_OFF; - // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) - if(!outLen) { - paramBuf[0] = 0; - outLen = 1; + if(CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; } - crcOut = 0; + if (ACK_OUT == ACK_OK) + { + // wtf.D_FLASH_ADDR_H=Adress_H; + // wtf.D_FLASH_ADDR_L=Adress_L; + ioMem.D_PTR_I = ParamBuf; + + switch(CMD) { + // ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + { + if (isMcuConnected()){ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + { + if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_SignOn()) { // SetStateDisconnected(); + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + } + if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; + } + break; + } + case cmd_ProtocolGetVersion: + { + // Only interface itself, no matter what Device + Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; + break; + } + + case cmd_InterfaceGetName: + { + // Only interface itself, no matter what Device + // O_PARAM_LEN=16; + O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; + break; + } + + case cmd_InterfaceGetVersion: + { + // Only interface itself, no matter what Device + // Dummy = iUart_res_InterfVersion; + O_PARAM_LEN = 2; + Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; + Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; + break; + } + case cmd_InterfaceExit: + { + isExitScheduled = true; + break; + } + case cmd_InterfaceSetMode: + { + #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (ParamBuf[0] == imSK) { + #endif + CurrentInterfaceMode = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_PARAM; + } + break; + } + + case cmd_DeviceReset: + { + if (ParamBuf[0] < escCount) { + // Channel may change here + selected_esc = ParamBuf[0]; + } + else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + BL_SendCMDRunRestartBootloader(&DeviceInfo); + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + break; + } + #endif + } + SET_DISCONNECTED; + break; + } + case cmd_DeviceInitFlash: + { + SET_DISCONNECTED; + if (ParamBuf[0] < escCount) { + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + selected_esc = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + O_PARAM_LEN = DeviceInfoSize; //4 + O_PARAM = (uint8_t *)&DeviceInfo; + if(Connect(&DeviceInfo)) { + DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; + } else { + SET_DISCONNECTED; + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + { + switch(CurrentInterfaceMode) + { + case imSK: + { + if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + { + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + { + Dummy.bytes[0] = ParamBuf[0]; + //Address = Page * 512 + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + ioMem.D_FLASH_ADDR_L = 0; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + //*** Device Memory Read Ops *** + case cmd_DeviceRead: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if(!Stk_ReadFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + case cmd_DeviceReadEEprom: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H = Adress_H; + wtf.D_FLASH_ADDR_L = Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + if (!BL_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if(ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + //*** Device Memory Write Ops *** + case cmd_DeviceWrite: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if (!BL_WriteFlash(&ioMem)) { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_WriteFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + } + break; + } + + case cmd_DeviceWriteEEprom: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + ACK_OUT = ACK_D_GENERAL_ERROR; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + case imATM_BLB: + { + if (BL_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (Stk_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + } + break; + } + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + } + } + } + + CRCout.word = 0; + + TX_LED_ON; serialBeginWrite(port); - writeByteCrc(cmd_Remote_Escape); - writeByteCrc(command); - writeByteCrc(addr >> 8); - writeByteCrc(addr & 0xff); - writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B - for(int i = 0; i < outLen % 0x100; i++) - writeByteCrc(paramBuf[i]); - writeByteCrc(replyAck); - writeByte(crcOut >> 8); - writeByte(crcOut & 0xff); + WriteByteCrc(cmd_Remote_Escape); + WriteByteCrc(CMD); + WriteByteCrc(ioMem.D_FLASH_ADDR_H); + WriteByteCrc(ioMem.D_FLASH_ADDR_L); + WriteByteCrc(O_PARAM_LEN); + + i=O_PARAM_LEN; + do { + WriteByteCrc(*O_PARAM); + O_PARAM++; + i--; + } while (i > 0); + + WriteByteCrc(ACK_OUT); + WriteByte(CRCout.bytes[1]); + WriteByte(CRCout.bytes[0]); serialEndWrite(port); - -#ifdef STM32F4 - delay(50); -#endif TX_LED_OFF; - } - - esc4wayRelease(); + if (isExitScheduled) { + esc4wayRelease(); + return; + } + }; } -// handle 4Way interface command -// command - received command, will be sent back in reply -// addr - from received header -// data - buffer used both for received parameters and returned data. -// Should be 256B long ; TODO - implement limited buffer size -// inLen - received input length -// outLen - size of data to return, max 256, initialized to zero -// single '\0' byte will be send if outLen is zero (protocol limitation) -esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen) -{ - ioMem_t ioMem; - ioMem.addr = addr; // default flash operation address - ioMem.data = data; // command data buffer is used for read and write commands - switch(command) { -// ******* Interface related stuff ******* - case cmd_InterfaceTestAlive: - if (!isMcuConnected()) - return esc4wayAck_OK; - - switch(currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - case imSIL_BLB: - if (BL_SendCMDKeepAlive()) - return esc4wayAck_OK; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - if (Stk_SignOn()) - return esc4wayAck_OK; - break; -#endif - } - setDisconnected(); - return esc4wayAck_D_GENERAL_ERROR; - - case cmd_ProtocolGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_PROTOCOL_VER; - *outLen = 1; - return esc4wayAck_OK; - - case cmd_InterfaceGetName: - // Only interface itself, no matter what Device - // outLen=16; - memcpy(data, SERIAL_4WAY_INTERFACE_NAME_STR, strlen(SERIAL_4WAY_INTERFACE_NAME_STR)); - *outLen = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); - return esc4wayAck_OK; - - case cmd_InterfaceGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_VERSION_HI; - data[1] = SERIAL_4WAY_VERSION_LO; - *outLen = 2; - return esc4wayAck_OK; - - case cmd_InterfaceExit: - esc4wayExitRequested = true; - return esc4wayAck_OK; - - case cmd_InterfaceSetMode: - switch(data[0]) { -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - case imSIL_BLB: - case imATM_BLB: -#endif -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - case imSK: -#endif - currentInterfaceMode = data[0]; - break; - default: - return esc4wayAck_I_INVALID_PARAM; - } - return esc4wayAck_OK; - - case cmd_DeviceReset: - if(data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - // Channel may change here - escSelected = data[0]; - switch (currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - case imATM_BLB: - BL_SendCMDRunRestartBootloader(); - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - break; -#endif - } - setDisconnected(); - return esc4wayAck_OK; - - case cmd_DeviceInitFlash: { - setDisconnected(); - if (data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - //Channel may change here - //ESC_LO or ESC_HI; Halt state for prev channel - int replyAck = esc4wayAck_OK; - escSelected = data[0]; - if(!connect(&deviceInfo)) { - setDisconnected(); - replyAck = esc4wayAck_D_GENERAL_ERROR; - } - deviceInfo.interfaceMode = currentInterfaceMode; - memcpy(data, &deviceInfo, sizeof(deviceInfo)); - *outLen = sizeof(deviceInfo); - return replyAck; - } - -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case cmd_DeviceEraseAll: - switch(currentInterfaceMode) { - case imSK: - if (!Stk_Chip_Erase()) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case cmd_DevicePageErase: - switch (currentInterfaceMode) { - case imSIL_BLB: - *outLen = 1; // return block number (from incoming packet) - // Address = Page * 512 - ioMem.addr = data[0] << 9; - if (!BL_PageErase(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -//*** Device Memory Read Ops *** - -// macros to mix interface with (single bit) memory type for switch statement -#define M_FLASH 0 -#define M_EEPROM 1 -#define INTFMEM(interface, memory) (((interface) << 1) | (memory)) - - case cmd_DeviceReadEEprom: - case cmd_DeviceRead: { - int len = data[0]; - if(len == 0) - len = 0x100; - ioMem.len = len; - switch(INTFMEM(currentInterfaceMode, (command == cmd_DeviceRead) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - if(!BL_ReadFlashSIL(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_FLASH): - if(!BL_ReadFlashATM(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if(!BL_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - // INTFMEM(imSIL_BLB, M_EEPROM): no eeprom on Silabs -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if(!Stk_ReadFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - *outLen = ioMem.len; - return esc4wayAck_OK; - } - -//*** Device Memory Write Ops *** - case cmd_DeviceWrite: - case cmd_DeviceWriteEEprom: - ioMem.len = inLen; - switch (INTFMEM(currentInterfaceMode, (command == cmd_DeviceWrite) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - case INTFMEM(imATM_BLB, M_FLASH): - if (!BL_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if (!BL_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if (!Stk_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#undef M_FLASH -#undef M_EEPROM -#undef INTFMEM - default: - return esc4wayAck_I_INVALID_CMD; - } - // should not get here -} #endif diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index 6c86b44c84..b1bba5ff69 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -15,127 +15,36 @@ * along with Cleanflight. If not, see . * Author: 4712 */ - #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "serial_4way_impl.h" #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER -typedef enum { - imC2 = 0, - imSIL_BLB = 1, - imATM_BLB = 2, - imSK = 3, -} esc4wayInterfaceMode_e; +#define imC2 0 +#define imSIL_BLB 1 +#define imATM_BLB 2 +#define imSK 3 -typedef enum { -// Test Interface still present - cmd_InterfaceTestAlive = 0x30, // '0' alive -// RETURN: ACK +extern uint8_t selected_esc; -// get Protocol Version Number 01..255 - cmd_ProtocolGetVersion = 0x31, // '1' version -// RETURN: uint8_t VersionNumber + ACK +extern ioMem_t ioMem; -// get Version String - cmd_InterfaceGetName = 0x32, // '2' name -// RETURN: String + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[2]; + uint16_t word; +} uint8_16_u; -//get Version Number 01..255 - cmd_InterfaceGetVersion = 0x33, // '3' version -// RETURN: uint16_t VersionNumber + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[4]; + uint16_t words[2]; + uint32_t dword; +} uint8_32_u; -// Exit / Restart Interface - can be used to switch to Box Mode - cmd_InterfaceExit = 0x34, // '4' exit -// RETURN: ACK +//extern uint8_32_u DeviceInfo; -// Reset the Device connected to the Interface - cmd_DeviceReset = 0x35, // '5' reset -// PARAM: uint8_t escId -// RETURN: ACK - -// Get the Device ID connected -// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106 -// RETURN: uint8_t DeviceID + ACK - -// Initialize Flash Access for Device connected -// Autodetects interface protocol; retruns device signature and protocol - cmd_DeviceInitFlash = 0x37, // '7' init flash access -// PARAM: uint8_t escId -// RETURN: uint8_t deviceInfo[4] + ACK - -// Erase the whole Device Memory of connected Device - cmd_DeviceEraseAll = 0x38, // '8' erase all -// RETURN: ACK - -// Erase one Page of Device Memory of connected Device - cmd_DevicePageErase = 0x39, // '9' page erase -// PARAM: uint8_t PageNumber (512B pages) -// RETURN: APageNumber ACK - -// Read to Buffer from FLASH Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceRead = 0x3A, // ':' read Device -// PARAM: [ADRESS] uint8_t BuffLen -// RETURN: [ADRESS, len] Buffer[0..256] ACK - -// Write Buffer to FLASH Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceWrite = 0x3B, // ';' write -// PARAM: [ADRESS + BuffLen] Buffer[1..256] -// RETURN: ACK - -// Set C2CK low infinite - permanent Reset state (unimplemented) - cmd_DeviceC2CK_LOW = 0x3C, // '<' -// RETURN: ACK - -// Read to Buffer from EEPROM Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceReadEEprom = 0x3D, // '=' read Device -// PARAM: [ADRESS] uint8_t BuffLen -// RETURN: [ADRESS + BuffLen] + Buffer[1..256] ACK - -// Write Buffer to EEPROM Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceWriteEEprom = 0x3E, // '>' write -// PARAM: [ADRESS + BuffLen] Buffer[1..256] -// RETURN: ACK - -// Set Interface Mode - cmd_InterfaceSetMode = 0x3F, // '?' -// PARAM: uint8_t Mode (interfaceMode_e) -// RETURN: ACK -} esc4wayCmd_e; - -// responses -typedef enum { - esc4wayAck_OK = 0x00, -// esc4wayAck_I_UNKNOWN_ERROR = 0x01, - esc4wayAck_I_INVALID_CMD = 0x02, - esc4wayAck_I_INVALID_CRC = 0x03, - esc4wayAck_I_VERIFY_ERROR = 0x04, -// esc4wayAck_D_INVALID_COMMAND = 0x05, -// esc4wayAck_D_COMMAND_FAILED = 0x06, -// esc4wayAck_D_UNKNOWN_ERROR = 0x07, - esc4wayAck_I_INVALID_CHANNEL = 0x08, - esc4wayAck_I_INVALID_PARAM = 0x09, - esc4wayAck_D_GENERAL_ERROR = 0x0f, -} esc4wayAck_e; - -typedef struct escDeviceInfo_s { - uint16_t signature; // lower 16 bit of signature - uint8_t signature2; // top 8 bit of signature for SK / BootMsg last char from BL - uint8_t interfaceMode; -} escDeviceInfo_t; - -extern bool esc4wayExitRequested; // flag that exit was requested. Set by esc4wayProcessCmd, used internally by esc4wayProcess - -int esc4wayInit(void); -void esc4wayStart(void); +bool isMcuConnected(void); +uint8_t esc4wayInit(void); +void esc4wayProcess(serialPort_t *mspPort); void esc4wayRelease(void); -void esc4wayProcess(serialPort_t *serial); -esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen); - -#endif diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 01a598e8b2..2a10dcdb4a 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -24,271 +24,289 @@ #include #include "platform.h" -#include "common/utils.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/buf_writer.h" #include "drivers/pwm_mapping.h" -#include "drivers/gpio.h" #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) // Bootloader commands // RunCmd #define RestartBootloader 0 -#define ExitBootloader 1 +#define ExitBootloader 1 -#define CMD_RUN 0x00 -#define CMD_PROG_FLASH 0x01 -#define CMD_ERASE_FLASH 0x02 +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 #define CMD_READ_FLASH_SIL 0x03 -#define CMD_VERIFY_FLASH 0x03 -#define CMD_READ_EEPROM 0x04 -#define CMD_PROG_EEPROM 0x05 -#define CMD_READ_SRAM 0x06 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 #define CMD_READ_FLASH_ATM 0x07 -#define CMD_KEEP_ALIVE 0xFD -#define CMD_SET_ADDRESS 0xFF -#define CMD_SET_BUFFER 0xFE +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE -#define CMD_BOOTINIT 0x07 -#define CMD_BOOTSIGN 0x08 +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 // Bootloader result codes -#define BR_SUCCESS 0x30 -#define BR_ERRORCOMMAND 0xC1 -#define BR_ERRORCRC 0xC2 -#define BR_NONE 0xFF +#define brSUCCESS 0x30 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF -#define START_BIT_TIMEOUT 2000 // 2ms -#define BIT_TIME 52 // 52uS -#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS -#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS -#define START_BIT_TIME (BIT_TIME_3_4) +#define START_BIT_TIMEOUT_MS 2 -static int suart_getc(void) +#define BIT_TIME (52) //52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS +#define START_BIT_TIME (BIT_TIME_HALVE + 1) +//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) + +static uint8_t suart_getc_(uint8_t *bt) { uint32_t btime; uint32_t start_time; - uint32_t wait_time = micros() + START_BIT_TIMEOUT; + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; while (ESC_IS_HI) { // check for startbit begin - if (micros() >= wait_time) { - return -1; + if (millis() >= wait_time) { + return 0; } } // start bit start_time = micros(); btime = start_time + START_BIT_TIME; uint16_t bitmask = 0; - for(int bit = 0; bit < 10; bit++) { - while (cmp32(micros(), btime) < 0); + uint8_t bit = 0; + while (micros() < btime); + while(1) { if (ESC_IS_HI) + { bitmask |= (1 << bit); + } btime = btime + BIT_TIME; + bit++; + if (bit == 10) break; + while (micros() < btime); } // check start bit and stop bit - if ((bitmask & (1 << 0)) || (!(bitmask & (1 << 9)))) { - return -1; + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; } - return bitmask >> 1; + *bt = bitmask >> 1; + return 1; } -static void suart_putc(uint8_t byte) +static void suart_putc_(uint8_t *tx_b) { - // send one idle bit first (stopbit from previous byte) - uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10); + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); uint32_t btime = micros(); while(1) { - if(bitmask & 1) + if(bitmask & 1) { ESC_SET_HI; // 1 - else + } + else { ESC_SET_LO; // 0 + } btime = btime + BIT_TIME; - bitmask >>= 1; - if (bitmask == 0) - break; // stopbit shifted out - but don't wait - while (cmp32(micros(), btime) < 0); + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait + while (micros() < btime); } } -static uint16_t crc16Byte(uint16_t from, uint8_t byte) +static uint8_16_u CRC_16; +static uint8_16_u LastCRC_16; + +static void ByteCrc(uint8_t *bt) { - uint16_t crc16 = from; - for (int i = 0; i < 8; i++) { - if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) { - crc16 >>= 1; - crc16 ^= 0xA001; + uint8_t xb = *bt; + for (uint8_t i = 0; i < 8; i++) + { + if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) { + CRC_16.word = CRC_16.word >> 1; + CRC_16.word = CRC_16.word ^ 0xA001; } else { - crc16 >>= 1; + CRC_16.word = CRC_16.word >> 1; } - byte >>= 1; + xb = xb >> 1; } - return crc16; } -static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc) +static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) { - int crc = 0; - int c; + // len 0 means 256 + CRC_16.word = 0; + LastCRC_16.word = 0; + uint8_t LastACK = brNONE; + do { + if(!suart_getc_(pstring)) goto timeout; + ByteCrc(pstring); + pstring++; + len--; + } while(len > 0); - uint8_t lastACK = BR_NONE; - for(int i = 0; i < len; i++) { - int c; - if ((c = suart_getc()) < 0) goto timeout; - crc = crc16Byte(crc, c); - pstring[i] = c; - } - - if(checkCrc) { - // With CRC read 3 more - for(int i = 0; i < 2; i++) { // checksum 2 CRC bytes - if ((c = suart_getc()) < 0) goto timeout; - crc = crc16Byte(crc, c); + if(isMcuConnected()) { + //With CRC read 3 more + if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; + if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; + if(!suart_getc_(&LastACK)) goto timeout; + if (CRC_16.word != LastCRC_16.word) { + LastACK = brERRORCRC; } - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; - if (crc != 0) // CRC of correct message is 0 - lastACK = BR_ERRORCRC; } else { - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; + if(!suart_getc_(&LastACK)) goto timeout; } timeout: - return (lastACK == BR_SUCCESS); + return (LastACK == brSUCCESS); } -static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc) +static void BL_SendBuf(uint8_t *pstring, uint8_t len) { ESC_OUTPUT; - uint16_t crc = 0; - for(int i = 0; i < len; i++) { - suart_putc(pstring[i]); - crc = crc16Byte(crc, pstring[i]); - } - if (appendCrc) { - suart_putc(crc & 0xff); - suart_putc(crc >> 8); + CRC_16.word=0; + do { + suart_putc_(pstring); + ByteCrc(pstring); + pstring++; + len--; + } while (len > 0); + + if (isMcuConnected()) { + suart_putc_(&CRC_16.bytes[0]); + suart_putc_(&CRC_16.bytes[1]); } ESC_INPUT; } -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) { -#define BOOT_MSG_LEN 4 -#define DevSignHi (BOOT_MSG_LEN) -#define DevSignLo (BOOT_MSG_LEN + 1) + #define BootMsgLen 4 + #define DevSignHi (BootMsgLen) + #define DevSignLo (BootMsgLen+1) - memset(pDeviceInfo, 0, sizeof(*pDeviceInfo)); - uint8_t bootInfo[BOOT_MSG_LEN + 4]; - static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471"; + //DeviceInfo.dword=0; is set before + uint8_t BootInfo[9]; + uint8_t BootMsg[BootMsgLen-1] = "471"; // x * 0 + 9 #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - // SK message was sent during autodetection, use longer preamble - uint8_t bootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 21); #else - uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 17); #endif - BL_SendBuf(bootInit, sizeof(bootInit), false); - if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false)) + if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { return 0; + } // BootInfo has no CRC (ACK byte already analyzed... ) // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) - if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK - return 0; + for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK + if (BootInfo[i] != BootMsg[i]) { + return (0); + } + } - pDeviceInfo->signature2 = bootInfo[BOOT_MSG_LEN - 1]; // taken from bootloaderMsg part, ascii 'c' now - pDeviceInfo->signature = (bootInfo[DevSignHi] << 8) | bootInfo[DevSignLo]; // SIGNATURE_001, SIGNATURE_002 - return 1; + //only 2 bytes used $1E9307 -> 0x9307 + pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1]; + pDeviceInfo->bytes[1] = BootInfo[DevSignHi]; + pDeviceInfo->bytes[0] = BootInfo[DevSignLo]; + return (1); } -static uint8_t BL_GetACK(int timeout) +static uint8_t BL_GetACK(uint32_t Timeout) { - int c; - while ((c = suart_getc()) < 0) - if(--timeout < 0) // timeout=1 -> 1 retry - return BR_NONE; - return c; + uint8_t LastACK = brNONE; + while (!(suart_getc_(&LastACK)) && (Timeout)) { + Timeout--; + } ; + return (LastACK); } -uint8_t BL_SendCMDKeepAlive(void) +uint8_t BL_SendCMDKeepAlive(void) { uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - if (BL_GetACK(1) != BR_ERRORCOMMAND) + BL_SendBuf(sCMD, 2); + if (BL_GetACK(1) != brERRORCOMMAND) { return 0; + } return 1; } -void BL_SendCMDRunRestartBootloader(void) +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) { uint8_t sCMD[] = {RestartBootloader, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00) + pDeviceInfo->bytes[0] = 1; + BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) return; } static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if(pMem->addr == 0xffff) - return 1; - uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff }; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(2) == BR_SUCCESS; + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; + BL_SendBuf(sCMD, 4); + return (BL_GetACK(2) == brSUCCESS); } static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) { - uint16_t len = pMem->len; - uint8_t sCMD[] = {CMD_SET_BUFFER, 0, len >> 8, len & 0xff}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - if (BL_GetACK(2) != BR_NONE) - return 0; - BL_SendBuf(pMem->data, len, true); - return BL_GetACK(40) == BR_SUCCESS; + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES}; + if (pMem->D_NUM_BYTES == 0) { + // set high byte + sCMD[2] = 1; + } + BL_SendBuf(sCMD, 4); + if (BL_GetACK(2) != brNONE) return 0; + BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES); + return (BL_GetACK(40) == brSUCCESS); } static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - unsigned len = pMem->len; - uint8_t sCMD[] = {cmd, len & 0xff}; // 0x100 is sent a 0x00 here - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_ReadBuf(pMem->data, len, true); + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES}; + BL_SendBuf(sCMD, 2); + return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES )); + } + return 0; } static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - if (!BL_SendCMDSetBuffer(pMem)) - return 0; - uint8_t sCMD[] = {cmd, 0x01}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(timeout) == BR_SUCCESS; + if (BL_SendCMDSetAddress(pMem)) { + if (!BL_SendCMDSetBuffer(pMem)) return 0; + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(timeout) == brSUCCESS); + } + return 0; } -uint8_t BL_ReadFlashATM(ioMem_t *pMem) +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { - return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + if(interface_mode == imATM_BLB) { + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + } else { + return BL_ReadA(CMD_READ_FLASH_SIL, pMem); + } } - -uint8_t BL_ReadFlashSIL(ioMem_t *pMem) -{ - return BL_ReadA(CMD_READ_FLASH_SIL, pMem); -} - - + + uint8_t BL_ReadEEprom(ioMem_t *pMem) { return BL_ReadA(CMD_READ_EEPROM, pMem); @@ -296,22 +314,23 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem) uint8_t BL_PageErase(ioMem_t *pMem) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - - uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(40 * 1000 / START_BIT_TIMEOUT) == BR_SUCCESS; + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS); + } + return 0; } uint8_t BL_WriteEEprom(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); } uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } #endif +#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 9ed08db61c..39cfaaa3d9 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -20,17 +20,12 @@ #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - void BL_SendBootInit(void); -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t BL_SendCMDKeepAlive(void); -uint8_t BL_WriteEEprom(ioMem_t *pMem); -uint8_t BL_ReadEEprom(ioMem_t *pMem); uint8_t BL_PageErase(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_WriteEEprom(ioMem_t *pMem); uint8_t BL_WriteFlash(ioMem_t *pMem); -uint8_t BL_ReadFlashATM(ioMem_t *pMem); -uint8_t BL_ReadFlashSIL(ioMem_t *pMem); -void BL_SendCMDRunRestartBootloader(void); - -#endif +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index ca1012b284..a8f83bfae8 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -15,12 +15,15 @@ * along with Cleanflight. If not, see . * Author: 4712 */ +#pragma once + +#include "drivers/io.h" typedef struct { IO_t io; } escHardware_t; -extern uint8_t escSelected; +extern uint8_t selected_esc; bool isEscHi(uint8_t selEsc); bool isEscLo(uint8_t selEsc); @@ -29,15 +32,17 @@ void setEscLo(uint8_t selEsc); void setEscInput(uint8_t selEsc); void setEscOutput(uint8_t selEsc); -#define ESC_IS_HI isEscHi(escSelected) -#define ESC_IS_LO isEscLo(escSelected) -#define ESC_SET_HI setEscHi(escSelected) -#define ESC_SET_LO setEscLo(escSelected) -#define ESC_INPUT setEscInput(escSelected) -#define ESC_OUTPUT setEscOutput(escSelected) +#define ESC_IS_HI isEscHi(selected_esc) +#define ESC_IS_LO isEscLo(selected_esc) +#define ESC_SET_HI setEscHi(selected_esc) +#define ESC_SET_LO setEscLo(selected_esc) +#define ESC_INPUT setEscInput(selected_esc) +#define ESC_OUTPUT setEscOutput(selected_esc) typedef struct ioMem_s { - uint16_t len; - uint16_t addr; - uint8_t *data; + uint8_t D_NUM_BYTES; + uint8_t D_FLASH_ADDR_H; + uint8_t D_FLASH_ADDR_L; + uint8_t *D_PTR_I; } ioMem_t; + diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 9c81c89317..342a0c8b06 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -16,45 +16,40 @@ * Author: 4712 * have a look at https://github.com/sim-/tgy/blob/master/boot.inc * for info about the stk500v2 implementation - */ + */ #include #include #include #include #include "platform.h" -#include "common/utils.h" -#include "drivers/gpio.h" -#include "drivers/buf_writer.h" -#include "drivers/pwm_mapping.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" -#include "drivers/system.h" #include "config/config.h" #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_stk500v2.h" +#include "drivers/system.h" +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER -#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#define BIT_LO_US (32) //32uS +#define BIT_HI_US (2*BIT_LO_US) +static uint8_t StkInBuf[16]; -#define BIT_LO_US 32 //32uS -#define BIT_HI_US (2 * BIT_LO_US) - -static uint8_t stkInBuf[16]; - -#define STK_BIT_TIMEOUT 250 // micro seconds +#define STK_BIT_TIMEOUT 250 // micro seconds #define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms #define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms -#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5ms -#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s -#define WaitPinLo while (ESC_IS_HI) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } -#define WaitPinHi while (ESC_IS_LO) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } +#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;} -static uint32_t lastBitTime; -static uint32_t hiLoTsh; +static uint32_t LastBitTime; +static uint32_t HiLoTsh; static uint8_t SeqNumber; static uint8_t StkCmd; @@ -77,10 +72,10 @@ static uint8_t ckSumOut; #define STATUS_CMD_OK 0x00 -#define CmdFlashEepromRead 0xA0 -#define EnterIspCmd1 0xAC -#define EnterIspCmd2 0x53 -#define SPI_SIGNATURE_READ 0x30 +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define signature_r 0x30 #define delay_us(x) delayMicroseconds(x) #define IRQ_OFF // dummy @@ -89,7 +84,7 @@ static uint8_t ckSumOut; static void StkSendByte(uint8_t dat) { ckSumOut ^= dat; - for (uint8_t i = 0; i < 8; i++) { + for (uint8_t i = 0; i < 8; i++) { if (dat & 0x01) { // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). ESC_SET_HI; @@ -111,7 +106,7 @@ static void StkSendByte(uint8_t dat) } } -static void StkSendPacketHeader(uint16_t len) +static void StkSendPacketHeader(void) { IRQ_OFF; ESC_OUTPUT; @@ -121,9 +116,6 @@ static void StkSendPacketHeader(uint16_t len) ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); - StkSendByte(len >> 8); - StkSendByte(len & 0xff); - StkSendByte(TOKEN); } static void StkSendPacketFooter(void) @@ -135,14 +127,16 @@ static void StkSendPacketFooter(void) IRQ_ON; } + + static int8_t ReadBit(void) { uint32_t btimer = micros(); uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; - lastBitTime = micros() - btimer; - if (lastBitTime <= hiLoTsh) { + LastBitTime = micros() - btimer; + if (LastBitTime <= HiLoTsh) { timeout_timer = timeout_timer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; @@ -155,27 +149,30 @@ timeout: return -1; } -static int ReadByte(void) +static uint8_t ReadByte(uint8_t *bt) { - uint8_t byte = 0; - for (int i = 0; i < 8; i++) { + *bt = 0; + for (uint8_t i = 0; i < 8; i++) { int8_t bit = ReadBit(); - if (bit < 0) - return -1; // timeout - byte |= bit << i; + if (bit == -1) goto timeout; + if (bit == 1) { + *bt |= (1 << i); + } } - ckSumIn ^= byte; - return byte; + ckSumIn ^=*bt; + return 1; +timeout: + return 0; } static uint8_t StkReadLeader(void) { // Reset learned timing - hiLoTsh = BIT_HI_US + BIT_LO_US; + HiLoTsh = BIT_HI_US + BIT_LO_US; // Wait for the first bit - int waitcycl; //250uS each + uint32_t waitcycl; //250uS each if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; @@ -184,54 +181,57 @@ static uint8_t StkReadLeader(void) } else { waitcycl= STK_WAITCYLCES; } - while(ReadBit() < 0 && --waitcycl > 0); - if (waitcycl <= 0) - goto timeout; - - // Skip the first bits - for (int i = 0; i < 10; i++) { - if (ReadBit() < 0) - goto timeout; + for ( ; waitcycl >0 ; waitcycl--) { + //check is not timeout + if (ReadBit() >- 1) break; } - // learn timing (0.75 * lastBitTime) - hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2); + //Skip the first bits + if (waitcycl == 0){ + goto timeout; + } + + for (uint8_t i = 0; i < 10; i++) { + if (ReadBit() == -1) goto timeout; + } + + // learn timing + HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); // Read until we get a 0 bit - int bit; + int8_t bit; do { bit = ReadBit(); - if (bit < 0) - goto timeout; + if (bit == -1) goto timeout; } while (bit > 0); return 1; timeout: return 0; } -static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen) +static uint8_t StkRcvPacket(uint8_t *pstring) { - int byte; - int len; + uint8_t bt = 0; + uint8_16_u Len; IRQ_OFF; if (!StkReadLeader()) goto Err; ckSumIn=0; - if ((byte = ReadByte()) < 0 || (byte != MESSAGE_START)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != SeqNumber)) goto Err; - len = ReadByte() << 8; - len |= ReadByte(); - if(len < 1 || len >= 256 + 4) // will catch timeout too; limit length to max expected size - goto Err; - if ((byte = ReadByte()) < 0 || (byte != TOKEN)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != StkCmd)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != STATUS_CMD_OK)) goto Err; - for (int i = 0; i < len - 2; i++) { - if ((byte = ReadByte()) < 0) goto Err; - if(i < maxLen) // limit saved length (buffer is only 256B, but memory read reply contains additional status + 1 unknown byte) - pstring[i] = byte; + if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err; + if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err; + ReadByte(&Len.bytes[1]); + if (Len.bytes[1] > 1) goto Err; + ReadByte(&Len.bytes[0]); + if (Len.bytes[0] < 1) goto Err; + if (!ReadByte(&bt) || (bt != TOKEN)) goto Err; + if (!ReadByte(&bt) || (bt != StkCmd)) goto Err; + if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err; + for (uint16_t i = 0; i < (Len.word - 2); i++) + { + if (!ReadByte(pstring)) goto Err; + pstring++; } - ReadByte(); // read checksum + ReadByte(&bt); if (ckSumIn != 0) goto Err; IRQ_ON; return 1; @@ -240,26 +240,27 @@ Err: return 0; } -static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr) +static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) { - StkCmd = CMD_SPI_MULTI; - StkSendPacketHeader(8); + StkCmd= CMD_SPI_MULTI; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(8); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_SPI_MULTI); - StkSendByte(4); // NumTX - StkSendByte(4); // NumRX - StkSendByte(0); // RxStartAdr - StkSendByte(subcmd); // {TxData} Cmd - StkSendByte(addr >> 8); // {TxData} AdrHi - StkSendByte(addr & 0xff); // {TxData} AdrLow - StkSendByte(0); // {TxData} 0 + StkSendByte(4); // NumTX + StkSendByte(4); // NumRX + StkSendByte(0); // RxStartAdr + StkSendByte(Cmd); // {TxData} Cmd + StkSendByte(AdrHi); // {TxData} AdrHi + StkSendByte(AdrLo); // {TxData} AdrLoch + StkSendByte(0); // {TxData} 0 StkSendPacketFooter(); - if (StkRcvPacket(stkInBuf, sizeof(stkInBuf))) { // NumRX + 3 - if ((stkInBuf[0] == 0x00) - && ((stkInBuf[1] == subcmd) || (stkInBuf[1] == 0x00 /* ignore zero returns */)) - && (stkInBuf[2] == 0x00)) { - *resByte = stkInBuf[3]; - } - return 1; + if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3 + if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) { + *ResByte = StkInBuf[3]; + } + return 1; } return 0; } @@ -267,37 +268,61 @@ static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t add static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { // ignore 0xFFFF - // assume address is set before and we read or write the immediately following memory - if((pMem->addr == 0xffff)) - return 1; + // assume address is set before and we read or write the immediately following package + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; StkCmd = CMD_LOAD_ADDRESS; - StkSendPacketHeader(5); + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(5); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_LOAD_ADDRESS); StkSendByte(0); StkSendByte(0); - StkSendByte(pMem->addr >> 8); - StkSendByte(pMem->addr & 0xff); + StkSendByte(pMem->D_FLASH_ADDR_H); + StkSendByte(pMem->D_FLASH_ADDR_L); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *)StkInBuf)); } static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(4); + uint8_t LenHi; + if (pMem->D_NUM_BYTES>0) { + LenHi=0; + } else { + LenHi=1; + } + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(4); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(StkCmd); - StkSendByte(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(pMem->D_NUM_BYTES); StkSendByte(CmdFlashEepromRead); StkSendPacketFooter(); - return StkRcvPacket(pMem->data, pMem->len); + return (StkRcvPacket(pMem->D_PTR_I)); } static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(pMem->len + 10); + uint8_16_u Len; + uint8_t LenLo = pMem->D_NUM_BYTES; + uint8_t LenHi; + if (LenLo) { + LenHi = 0; + Len.word = LenLo + 10; + } else { + LenHi = 1; + Len.word = 256 + 10; + } + StkSendPacketHeader(); + StkSendByte(Len.bytes[1]); // high byte Msg len + StkSendByte(Len.bytes[0]); // low byte Msg len + StkSendByte(TOKEN); StkSendByte(StkCmd); - StkSendByte(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(LenLo); StkSendByte(0); // mode StkSendByte(0); // delay StkSendByte(0); // cmd1 @@ -305,82 +330,92 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) StkSendByte(0); // cmd3 StkSendByte(0); // poll1 StkSendByte(0); // poll2 - for(int i = 0; i < pMem->len; i++) - StkSendByte(pMem->data[i]); + do { + StkSendByte(*pMem->D_PTR_I); + pMem->D_PTR_I++; + LenLo--; + } while (LenLo); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return StkRcvPacket((void *)StkInBuf); } uint8_t Stk_SignOn(void) { - StkCmd = CMD_SIGN_ON; - StkSendPacketHeader(1); + StkCmd=CMD_SIGN_ON; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(1); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_SIGN_ON); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *) StkInBuf)); } -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) { - if (!Stk_SignOn()) - return 0; - uint8_t signature[3]; // device signature, MSB first - for(unsigned i = 0; i < sizeof(signature); i++) { - if (!_CMD_SPI_MULTI_EX(&signature[i], SPI_SIGNATURE_READ, i)) - return 0; + if (Stk_SignOn()) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) { + return 1; + } + } } - // convert signature to little endian - pDeviceInfo->signature = (signature[1] << 8) | signature[2]; - pDeviceInfo->signature2 = signature[0]; - return 1; + return 0; } uint8_t Stk_Chip_Erase(void) { StkCmd = CMD_CHIP_ERASE_ISP; - StkSendPacketHeader(7); - StkSendByte(StkCmd); + StkSendPacketHeader(); + StkSendByte(0); // high byte Msg len + StkSendByte(7); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_CHIP_ERASE_ISP); StkSendByte(20); // ChipErase_eraseDelay atmega8 - StkSendByte(0); // ChipErase_pollMethod atmega8 + StkSendByte(0); // ChipErase_pollMethod atmega8 StkSendByte(0xAC); StkSendByte(0x88); StkSendByte(0x13); StkSendByte(0x76); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket(StkInBuf)); } uint8_t Stk_ReadFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_FLASH_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_FLASH_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_ReadEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_EEPROM_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_EEPROM_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_FLASH_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_FLASH_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_EEPROM_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } - +#endif #endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index 8f2fddcf40..80ca89826d 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -17,14 +17,11 @@ */ #pragma once -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - uint8_t Stk_SignOn(void); -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t Stk_ReadEEprom(ioMem_t *pMem); uint8_t Stk_WriteEEprom(ioMem_t *pMem); uint8_t Stk_ReadFlash(ioMem_t *pMem); uint8_t Stk_WriteFlash(ioMem_t *pMem); uint8_t Stk_Chip_Erase(void); -#endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c0f3a8c417..fa822f8314 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,13 +112,14 @@ static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); -void cliDfu(char *cmdLine); +void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); void cliDumpProfile(uint8_t profileIndex); void cliDumpRateProfile(uint8_t rateProfileIndex) ; static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); +static void cliName(char *cmdline); static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); @@ -155,6 +156,7 @@ static void cliMap(char *cmdline); #ifdef LED_STRIP static void cliLed(char *cmdline); static void cliColor(char *cmdline); +static void cliModeColor(char *cmdline); #endif #ifndef USE_QUAD_MIXER_ONLY @@ -263,6 +265,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), + CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), @@ -340,6 +343,7 @@ const clicmd_t cmdTable[] = { #ifdef VTX CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif + CLI_COMMAND_DEF("name", "Name of craft", NULL, cliName), }; #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) @@ -387,7 +391,7 @@ static const char * const lookupTableBlackboxDevice[] = { static const char * const lookupTablePidController[] = { - "UNUSED", "INT", "FLOAT" + "LEGACY", "BETAFLIGHT" }; static const char * const lookupTableSerialRX[] = { @@ -451,6 +455,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "MIXER", "AIRMODE", "PIDLOOP", + "NOTCH", }; #ifdef OSD static const char * const lookupTableOsdType[] = { @@ -486,7 +491,7 @@ typedef enum { TABLE_GPS_SBAS_MODE, #endif #ifdef BLACKBOX - TABLE_BLACKBOX_DEVICE, + TABLE_BLACKBOX_DEVICE, #endif TABLE_CURRENT_SENSOR, TABLE_GIMBAL_MODE, @@ -598,7 +603,6 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently @@ -677,7 +681,6 @@ const clivalue_t valueTable[] = { { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } }, - { "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, @@ -696,6 +699,8 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, + { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, + { "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -762,8 +767,15 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, + { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, + { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, + { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, + { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, + { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, + + { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, @@ -812,7 +824,7 @@ const clivalue_t valueTable[] = { { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef USE_RTC6705 - { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, + { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } }, #endif #ifdef OSD @@ -1399,20 +1411,20 @@ static void cliLed(char *cmdline) char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); cliPrintf("led %u %s\r\n", i, ledConfigBuffer); } } else { ptr = cmdline; i = atoi(ptr); - if (i < MAX_LED_STRIP_LENGTH) { + if (i < LED_MAX_STRIP_LENGTH) { ptr = strchr(cmdline, ' '); if (!parseLedStripConfig(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1); + cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); } } } @@ -1423,7 +1435,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { cliPrintf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, @@ -1434,16 +1446,57 @@ static void cliColor(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); - if (i < CONFIGURABLE_COLOR_COUNT) { + if (i < LED_CONFIGURABLE_COLOR_COUNT) { ptr = strchr(cmdline, ' '); if (!parseColor(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); } } } + +static void cliModeColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = modeColors[i].color[j]; + cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = specialColors.color[j]; + cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } + } else { + enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; + int args[ARGS_COUNT]; + int argNo = 0; + char* ptr = strtok(cmdline, " "); + while (ptr && argNo < ARGS_COUNT) { + args[argNo++] = atoi(ptr); + ptr = strtok(NULL, " "); + } + + if (ptr != NULL || argNo != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int modeIdx = args[MODE]; + int funIdx = args[FUNCTION]; + int color = args[COLOR]; + if(!setModeColor(modeIdx, funIdx, color)) { + cliShowParseError(); + return; + } + // values are validated + cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color); + } +} #endif #ifdef USE_SERVOS @@ -1918,7 +1971,6 @@ typedef enum { DUMP_ALL = (1 << 3), } dumpFlags_e; - static const char* const sectionBreak = "\r\n"; #define printSectionBreak() cliPrintf((char *)sectionBreak) @@ -1941,7 +1993,7 @@ static void cliDump(char *cmdline) dumpMask = DUMP_PROFILE; // only } if (strcasecmp(cmdline, "rates") == 0) { - dumpMask = DUMP_RATES; + dumpMask = DUMP_RATES; } if (strcasecmp(cmdline, "all") == 0) { @@ -1953,7 +2005,10 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); - cliPrint("\r\n# dump master\r\n"); + printSectionBreak(); + cliPrint("\r\n# name\r\n"); + cliName(NULL); + cliPrint("\r\n# mixer\r\n"); #ifndef USE_QUAD_MIXER_ONLY @@ -2014,8 +2069,7 @@ static void cliDump(char *cmdline) #endif #endif - cliPrint("\r\n\r\n# feature\r\n"); - + cliPrint("\r\n# feature\r\n"); mask = featureMask(); for (i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) @@ -2035,10 +2089,8 @@ static void cliDump(char *cmdline) #endif } - #ifdef BEEPER - cliPrint("\r\n\r\n# beeper\r\n"); - + cliPrint("\r\n# beeper\r\n"); uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); for (int i = 0; i < (beeperCount-2); i++) { @@ -2049,40 +2101,37 @@ static void cliDump(char *cmdline) } #endif - - cliPrint("\r\n\r\n# map\r\n"); - + cliPrint("\r\n# map\r\n"); for (i = 0; i < 8; i++) buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; cliPrintf("map %s\r\n", buf); - cliPrint("\r\n\r\n# serial\r\n"); + cliPrint("\r\n# serial\r\n"); cliSerial(""); #ifdef LED_STRIP - cliPrint("\r\n\r\n# led\r\n"); + cliPrint("\r\n# led\r\n"); cliLed(""); - cliPrint("\r\n\r\n# color\r\n"); + cliPrint("\r\n# color\r\n"); cliColor(""); + + cliPrint("\r\n# mode_color\r\n"); + cliModeColor(""); #endif cliPrint("\r\n# aux\r\n"); - cliAux(""); cliPrint("\r\n# adjrange\r\n"); - cliAdjustmentRange(""); cliPrintf("\r\n# rxrange\r\n"); - cliRxRange(""); #ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); - cliServo(""); // print servo directions @@ -2102,11 +2151,10 @@ static void cliDump(char *cmdline) #ifdef VTX cliPrint("\r\n# vtx\r\n"); - cliVtx(""); #endif - printSectionBreak(); + cliPrint("\r\n# master\r\n"); dumpValues(MASTER_VALUE); cliPrint("\r\n# rxfail\r\n"); @@ -2124,7 +2172,6 @@ static void cliDump(char *cmdline) cliDumpRateProfile(rateCount); cliPrint("\r\n# restore original rateprofile selection\r\n"); - changeControlRateProfile(currentRateIndex); cliRateProfile(""); #ifdef USE_SLOW_SERIAL_CLI @@ -2133,7 +2180,6 @@ static void cliDump(char *cmdline) } cliPrint("\r\n# restore original profile selection\r\n"); - changeProfile(activeProfile); cliProfile(""); printSectionBreak(); @@ -2156,30 +2202,23 @@ static void cliDump(char *cmdline) void cliDumpProfile(uint8_t profileIndex) { - if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values - return; - - changeProfile(profileIndex); - cliPrint("\r\n# profile\r\n"); - cliProfile(""); - cliPrintf("############################# PROFILE VALUES ####################################\r\n"); - - printSectionBreak(); - dumpValues(PROFILE_VALUE); - - cliRateProfile(""); + if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values + return; + changeProfile(profileIndex); + cliPrint("\r\n# profile\r\n"); + cliProfile(""); + printSectionBreak(); + dumpValues(PROFILE_VALUE); } void cliDumpRateProfile(uint8_t rateProfileIndex) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; - changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE); } @@ -2494,6 +2533,18 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); } +static void cliName(char *cmdline) +{ + uint32_t len = strlen(cmdline); + if (len > 0) { + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } + cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); + + return; +} + static void cliPlaySound(char *cmdline) { #if FLASH_SIZE <= 64 @@ -2681,7 +2732,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) break; } } -static void cliPrintVarRange(const clivalue_t *var) +static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { case (MODE_DIRECT): { @@ -2693,7 +2744,7 @@ static void cliPrintVarRange(const clivalue_t *var) cliPrint("Allowed values:"); uint8_t i; for (i = 0; i < tableEntry->valueCount ; i++) { - if (i > 0) + if (i > 0) cliPrint(","); cliPrintf(" %s", tableEntry->values[i]); } @@ -2904,6 +2955,10 @@ static void cliStatus(char *cmdline) #endif cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t)); + +#ifdef USE_SDCARD + cliSdInfo(NULL); +#endif } #ifndef SKIP_TASK_STATISTICS diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 80e4717fde..1dd23a3846 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -59,6 +59,7 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/osd.h" #include "io/vtx.h" +#include "io/msp_protocol.h" #include "telemetry/telemetry.h" @@ -104,77 +105,10 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c extern void resetPidProfile(pidProfile_t *pidProfile); -void setGyroSamplingSpeed(uint16_t looptime) { - uint16_t gyroSampleRate = 1000; - uint8_t maxDivider = 1; - - 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; - gyroSampleRate = 125; - maxDivider = 8; - masterConfig.pid_process_denom = 1; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - if (looptime < 250) { - masterConfig.acc_hardware = ACC_NONE; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - masterConfig.pid_process_denom = 2; - } else if (looptime < 375) { - masterConfig.acc_hardware = CONFIG_FASTLOOP_PREFERRED_ACC; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - masterConfig.pid_process_denom = 2; - } - masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); - } else { - masterConfig.gyro_lpf = 0; - masterConfig.gyro_sync_denom = 8; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - } -#else - if (looptime < 1000) { - masterConfig.gyro_lpf = 0; - masterConfig.acc_hardware = ACC_NONE; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - gyroSampleRate = 125; - maxDivider = 8; - masterConfig.pid_process_denom = 1; - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { - masterConfig.pid_process_denom = 2; - } - if (looptime < 250) { - masterConfig.pid_process_denom = 4; - } else if (looptime < 375) { - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { - masterConfig.pid_process_denom = 3; - } else { - masterConfig.pid_process_denom = 2; - } - } - masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); - } else { - masterConfig.gyro_lpf = 0; - masterConfig.gyro_sync_denom = 8; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - masterConfig.pid_process_denom = 1; - } -#endif - } -} - void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. +static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; typedef struct box_e { const uint8_t boxId; // see boxId_e @@ -214,6 +148,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXAIRMODE, "AIR MODE;", 28 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29}, + { BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30}, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -253,6 +188,8 @@ STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; STATIC_UNIT_TESTED mspPort_t *currentPort; STATIC_UNIT_TESTED bufWriter_t *writer; +#define RATEPROFILE_MASK (1 << 7) + static void serialize8(uint8_t a) { bufWriterAppend(writer, a); @@ -542,6 +479,8 @@ void mspInit(serialConfig_t *serialConfig) if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; + activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } @@ -659,7 +598,7 @@ static uint32_t packFlightModeFlags(void) static bool processOutCommand(uint8_t cmdMSP) { uint32_t i; - + uint8_t len; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; @@ -737,7 +676,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(12); + headSerialReply(15); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); @@ -746,8 +685,18 @@ static bool processOutCommand(uint8_t cmdMSP) #endif serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); serialize32(packFlightModeFlags()); - serialize8(masterConfig.current_profile_index); + serialize8(getCurrentProfile()); serialize16(constrain(averageSystemLoadPercent, 0, 100)); + serialize8(MAX_PROFILE_COUNT); + serialize8(getCurrentControlRateProfile()); + break; + + case MSP_NAME: + len = strlen(masterConfig.name); + headSerialReply(len); + for (uint8_t i=0; ih); serialize8(color->s); @@ -1152,14 +1101,27 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 7); - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + headSerialReply(LED_MAX_STRIP_LENGTH * 4); + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - serialize8(GET_LED_X(ledConfig)); - serialize8(GET_LED_Y(ledConfig)); - serialize8(ledConfig->color); + serialize32(*ledConfig); + } + break; + + case MSP_LED_STRIP_MODECOLOR: + headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + serialize8(i); + serialize8(j); + serialize8(masterConfig.modeColors[i].color[j]); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + serialize8(LED_MODE_COUNT); + serialize8(j); + serialize8(masterConfig.specialColors.color[j]); } break; #endif @@ -1237,18 +1199,18 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_3D: - headSerialReply(2 * 4); + headSerialReply(2 * 3); serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_RC_DEADBAND: - headSerialReply(3); + headSerialReply(5); serialize8(masterConfig.rcControlsConfig.deadband); serialize8(masterConfig.rcControlsConfig.yaw_deadband); serialize8(masterConfig.rcControlsConfig.alt_hold_deadband); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: headSerialReply(3); @@ -1276,14 +1238,14 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); - serialize8(masterConfig.batteryConfig.vbatPidCompensation); + serialize8(currentProfile->pidProfile.vbatPidCompensation); break; case MSP_SPECIAL_PARAMETERS: headSerialReply(1 + 2 + 1 + 2); serialize8(currentControlRateProfile->rcYawRate8); serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(masterConfig.escAndServoConfig.escDesyncProtection); + serialize16(currentProfile->pidProfile.accelerationLimitPercent); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1302,8 +1264,7 @@ static bool processInCommand(void) { uint32_t i; uint16_t tmp; - uint8_t rate; - uint8_t oldPid; + uint8_t value; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1313,14 +1274,23 @@ static bool processInCommand(void) #endif switch (currentPort->cmdMSP) { case MSP_SELECT_SETTING: - if (!ARMING_FLAG(ARMED)) { - masterConfig.current_profile_index = read8(); - if (masterConfig.current_profile_index > 1) { - masterConfig.current_profile_index = 0; + value = read8(); + if ((value & RATEPROFILE_MASK) == 0) { + if (!ARMING_FLAG(ARMED)) { + if (value >= MAX_PROFILE_COUNT) { + value = 0; + } + changeProfile(value); } - writeEEPROM(); - readEEPROM(); + } else { + value = value & ~RATEPROFILE_MASK; + + if (value >= MAX_RATEPROFILES) { + value = 0; + } + changeControlRateProfile(value); } + break; case MSP_SET_HEAD: magHold = read16(); @@ -1350,13 +1320,11 @@ static bool processInCommand(void) masterConfig.disarm_kill_switch = read8(); break; case MSP_SET_LOOP_TIME: - setGyroSamplingSpeed(read16()); + read16(); break; case MSP_SET_PID_CONTROLLER: - oldPid = currentProfile->pidProfile.pidController; - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); + currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); pidSetController(currentProfile->pidProfile.pidController); - if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID break; case MSP_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { @@ -1410,11 +1378,11 @@ static bool processInCommand(void) currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { - rate = read8(); - currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + value = read8(); + currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); } - rate = read8(); - currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + value = read8(); + currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX); currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->tpa_breakpoint = read16(); @@ -1774,7 +1742,7 @@ static bool processInCommand(void) #ifdef LED_STRIP case MSP_SET_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); @@ -1785,31 +1753,26 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { headSerialError(0); break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = read8(); - + *ledConfig = read32(); reevaluateLedConfig(); } break; + + case MSP_SET_LED_STRIP_MODECOLOR: + { + ledModeIndex_e modeIdx = read8(); + int funIdx = read8(); + int color = read8(); + + if (!setModeColor(modeIdx, funIdx, color)) + return false; + } + break; #endif case MSP_REBOOT: isRebootScheduled = true; @@ -1855,19 +1818,26 @@ static bool processInCommand(void) currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); - masterConfig.batteryConfig.vbatPidCompensation = read8(); + currentProfile->pidProfile.vbatPidCompensation = read8(); break; case MSP_SET_SPECIAL_PARAMETERS: currentControlRateProfile->rcYawRate8 = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.rcSmoothInterval = read8(); - masterConfig.escAndServoConfig.escDesyncProtection = read16(); + currentProfile->pidProfile.accelerationLimitPercent = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); masterConfig.baro_hardware = read8(); masterConfig.mag_hardware = read8(); break; + + case MSP_SET_NAME: + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) { + masterConfig.name[i] = read8(); + } + break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! return false; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 1a63b2b20d..2778da7e22 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -20,267 +20,6 @@ #include "io/serial.h" #include "drivers/serial.h" -/** - * MSP Guidelines, emphasis is used to clarify. - * - * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed. - * - * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier. - * - * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version - * if the API doesn't match EXACTLY. - * - * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command. - * If no response is obtained then client MAY try the legacy MSP_IDENT command. - * - * API consumers should ALWAYS handle communication failures gracefully and attempt to continue - * without the information if possible. Clients MAY log/display a suitable message. - * - * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION. - * - * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time - * the API client was written and handle command failures gracefully. Clients MAY disable - * functionality that depends on the commands while still leaving other functionality intact. - * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state - * that the newer API version may cause problems before using API commands that change FC state. - * - * It is for this reason that each MSP command should be specific as possible, such that changes - * to commands break as little client functionality as possible. - * - * API client authors MAY use a compatibility matrix/table when determining if they can support - * a given command from a given flight controller at a given api version level. - * - * Developers MUST NOT create new MSP commands that do more than one thing. - * - * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools - * that use the API and the users of those tools. - */ - -#define MSP_PROTOCOL_VERSION 0 - -#define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR - -#define API_VERSION_LENGTH 2 - -#define MULTIWII_IDENTIFIER "MWII"; -#define CLEANFLIGHT_IDENTIFIER "CLFL" -#define BETAFLIGHT_IDENTIFIER "BTFL" -#define BASEFLIGHT_IDENTIFIER "BAFL"; - -#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -extern const char * const flightControllerIdentifier; - -#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 -#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF - -static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; -#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. -#define BOARD_HARDWARE_REVISION_LENGTH 2 - -// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version. -#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) -#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30) - -// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask. -#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31) -#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30) -#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29) -#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28) - -#define CAP_DYNBALANCE ((uint32_t)1 << 2) -#define CAP_FLAPS ((uint32_t)1 << 3) -#define CAP_NAVCAP ((uint32_t)1 << 4) -#define CAP_EXTAUX ((uint32_t)1 << 5) - -#define MSP_API_VERSION 1 //out message -#define MSP_FC_VARIANT 2 //out message -#define MSP_FC_VERSION 3 //out message -#define MSP_BOARD_INFO 4 //out message -#define MSP_BUILD_INFO 5 //out message - -// -// MSP commands for Cleanflight original features -// -#define MSP_MODE_RANGES 34 //out message Returns all mode ranges -#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range - -#define MSP_FEATURE 36 -#define MSP_SET_FEATURE 37 - -#define MSP_BOARD_ALIGNMENT 38 -#define MSP_SET_BOARD_ALIGNMENT 39 - -#define MSP_CURRENT_METER_CONFIG 40 -#define MSP_SET_CURRENT_METER_CONFIG 41 - -#define MSP_MIXER 42 -#define MSP_SET_MIXER 43 - -#define MSP_RX_CONFIG 44 -#define MSP_SET_RX_CONFIG 45 - -#define MSP_LED_COLORS 46 -#define MSP_SET_LED_COLORS 47 - -#define MSP_LED_STRIP_CONFIG 48 -#define MSP_SET_LED_STRIP_CONFIG 49 - -#define MSP_RSSI_CONFIG 50 -#define MSP_SET_RSSI_CONFIG 51 - -#define MSP_ADJUSTMENT_RANGES 52 -#define MSP_SET_ADJUSTMENT_RANGE 53 - -// private - only to be used by the configurator, the commands are likely to change -#define MSP_CF_SERIAL_CONFIG 54 -#define MSP_SET_CF_SERIAL_CONFIG 55 - -#define MSP_VOLTAGE_METER_CONFIG 56 -#define MSP_SET_VOLTAGE_METER_CONFIG 57 - -#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] - -#define MSP_PID_CONTROLLER 59 -#define MSP_SET_PID_CONTROLLER 60 - -#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters -#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters - -#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip -#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip -#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip - -#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter -#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter - -#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings -#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings - -#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings -#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings - -#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card - -#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings -#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings - -#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings -#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings - -#define MSP_OSD_CONFIG 84 //in message Get osd settings -#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings - -#define MSP_OSD_CHAR_READ 86 //in message Get osd settings -#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings - -#define MSP_VTX_CONFIG 88 //in message Get vtx settings -#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings - -// -// Baseflight MSP commands (if enabled they exist in Cleanflight) -// -#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) -#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP - -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. -// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save - -#define MSP_REBOOT 68 //in message reboot settings - -// DEPRECATED - Use MSP_BUILD_INFO instead -#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion - -// Betaflight Additional Commands -#define MSP_PID_ADVANCED_CONFIG 90 -#define MSP_SET_PID_ADVANCED_CONFIG 91 - -#define MSP_FILTER_CONFIG 92 -#define MSP_SET_FILTER_CONFIG 93 - -#define MSP_ADVANCED_TUNING 94 -#define MSP_SET_ADVANCED_TUNING 95 - -#define MSP_SENSOR_CONFIG 96 -#define MSP_SET_SENSOR_CONFIG 97 - -#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility -#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility - -// -// Multwii original MSP commands -// - -// DEPRECATED - See MSP_API_VERSION and MSP_MIXER -#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable - - -#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number -#define MSP_RAW_IMU 102 //out message 9 DOF -#define MSP_SERVO 103 //out message servos -#define MSP_MOTOR 104 //out message motors -#define MSP_RC 105 //out message rc channels and more -#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course -#define MSP_COMP_GPS 107 //out message distance home, direction home -#define MSP_ATTITUDE 108 //out message 2 angles 1 heading -#define MSP_ALTITUDE 109 //out message altitude, variometer -#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX -#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define MSP_PID 112 //out message P I D coeff (9 are used currently) -#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) -#define MSP_MISC 114 //out message powermeter trig -#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI -#define MSP_BOXNAMES 116 //out message the aux switch names -#define MSP_PIDNAMES 117 //out message the PID names -#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold -#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes -#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. -#define MSP_NAV_STATUS 121 //out message Returns navigation status -#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters -#define MSP_3D 124 //out message Settings needed for reversible ESCs -#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll -#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag - -#define MSP_SET_RAW_RC 200 //in message 8 rc chan -#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed -#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) -#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) -#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo -#define MSP_ACC_CALIBRATION 205 //in message no param -#define MSP_MAG_CALIBRATION 206 //in message no param -#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use -#define MSP_RESET_CONF 208 //in message no param -#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) -#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) -#define MSP_SET_HEAD 211 //in message define a new heading hold direction -#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings -#define MSP_SET_MOTOR 214 //in message PropBalance function -#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom -#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs -#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll -#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults -#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag - -// #define MSP_BIND 240 //in message no param -// #define MSP_ALARMS 242 - -#define MSP_EEPROM_WRITE 250 //in message no param - -#define MSP_DEBUGMSG 253 //out message debug string buffer -#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 - -// Additional commands that are not compatible with MultiWii -#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc -#define MSP_UID 160 //out message Unique device ID -#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) -#define MSP_ACC_TRIM 240 //out message get acc angle trim values -#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values -#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface - // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 942a770547..fdd5e506f9 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - + #pragma once #define VTX_BAND_MIN 1 diff --git a/src/main/main.c b/src/main/main.c index 8cf45ac7bb..63869ab52c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -134,7 +134,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void osdInit(void); @@ -310,12 +310,12 @@ void init(void) featureClear(FEATURE_ONESHOT125); } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm; + bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM - pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); + pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; - pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; + pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; @@ -323,7 +323,6 @@ void init(void) if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors - use_unsyncedPwm = false; } #ifdef CC3D pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; @@ -544,7 +543,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/mw.c b/src/main/mw.c index 5865a5118e..371961e013 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,8 +184,28 @@ float calculateRate(int axis, int16_t rc) { angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; } + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) + return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + else + return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) +} - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection +void scaleRcCommandToFpvCamAngle(void) { + //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed + static uint8_t lastFpvCamAngleDegrees = 0; + static float cosFactor = 1.0; + static float sinFactor = 0.0; + + if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){ + lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees; + cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); + sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); + } + + int16_t roll = angleRate[ROLL]; + int16_t yaw = angleRate[YAW]; + angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); + angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); } void processRcCommand(void) @@ -208,6 +228,11 @@ void processRcCommand(void) if (isRXDataNew) { for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]); + // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) + if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { + scaleRcCommandToFpvCamAngle(); + } + for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; @@ -230,24 +255,6 @@ void processRcCommand(void) } } -void scaleRcCommandToFpvCamAngle(void) { - //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed - static uint8_t lastFpvCamAngleDegrees = 0; - static float cosFactor = 1.0; - static float sinFactor = 0.0; - - if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){ - lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees; - cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); - sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); - } - - int16_t roll = rcCommand[ROLL]; - int16_t yaw = rcCommand[YAW]; - rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); - rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); -} - static void updateRcCommands(void) { // PITCH & ROLL only dynamic PID adjustment, depending on throttle value @@ -310,11 +317,6 @@ static void updateRcCommands(void) rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } - - // experimental scaling of RC command to FPV cam angle - if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) { - scaleRcCommandToFpvCamAngle(); - } } static void updateLEDs(void) @@ -483,7 +485,7 @@ void updateMagHold(void) void processRx(void) { static bool armedBeeperOn = false; - static bool wasAirmodeIsActivated; + static bool airmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTime); @@ -507,15 +509,21 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { - if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset + if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset } else { - wasAirmodeIsActivated = false; + airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ - if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) { + if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); + if (currentProfile->pidProfile.zeroThrottleStabilisation) + pidStabilisationState(PID_STABILISATION_ON); + else + pidStabilisationState(PID_STABILISATION_OFF); + } else { + pidStabilisationState(PID_STABILISATION_ON); } // When armed and motors aren't spinning, do beeps and then disarm @@ -763,7 +771,7 @@ void subTaskMotorUpdate(void) previousMotorUpdateTime = startTime; } - mixTable(); + mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS filterServos(); diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 0e68854897..bbf4848bb7 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -226,7 +226,7 @@ static void xBusUnpackRJ01Frame(void) uint8_t outerCrc = 0; uint8_t i = 0; - // When using the Align RJ01 receiver with + // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: @@ -234,7 +234,7 @@ static void xBusUnpackRJ01Frame(void) // Compared to a standard MODE B frame that only // contains the "middle" package. // Hence, at the moment, the unknown header and footer - // of the RJ01 MODEB packages are discarded. + // of the RJ01 MODEB packages are discarded. // However, the LAST byte (CRC_OUTER) is infact an 8-bit // CRC for the whole package, using the Dallas-One-Wire CRC // method. diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 18299e4904..d7852c55bf 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -168,7 +168,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_LEDSTRIP] = { .taskName = "LEDSTRIP", .taskFunc = taskLedStrip, - .desiredPeriod = 1000000 / 10, // 10 Hz + .desiredPeriod = 1000000 / 100, // 100 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5de9c3fce9..d8cc423c1a 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -190,7 +190,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } accFilterInitialised = true; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 59fa7ef9ae..2bb1f973f0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -70,7 +70,7 @@ static void updateBatteryVoltage(void) if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; if (!vbatFilterIsInitialised) { - biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update + biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update vbatFilterIsInitialised = true; } vbatSample = biquadFilterApply(&vbatFilter, vbatSample); @@ -91,7 +91,7 @@ void updateBattery(void) /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; /* wait for VBatt to stabilise then we can calc number of cells - (using the filtered value takes a long time to ramp up) + (using the filtered value takes a long time to ramp up) We only do this on the ground so don't care if we do block, not worse than original code anyway*/ delay(VBATTERY_STABLE_DELAY); @@ -212,7 +212,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea fix12_t calculateVbatPidCompensation(void) { fix12_t batteryScaler; - if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) { + if (feature(FEATURE_VBAT) && batteryCellCount > 1) { uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount; batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage)); } else { @@ -224,7 +224,7 @@ fix12_t calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount); + return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); } uint8_t calculateBatteryCapacityRemainingPercentage(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 21197e07fc..e52541db70 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -43,7 +43,6 @@ typedef struct batteryConfig_s { uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V - uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4f425dc1d2..40a30ebb7c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -27,12 +27,14 @@ #include "common/filter.h" #include "drivers/sensor.h" +#include "drivers/system.h" #include "drivers/accgyro.h" -#include "sensors/sensors.h" + #include "io/beeper.h" #include "io/statusindicator.h" -#include "sensors/boardalignment.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" #include "sensors/gyro.h" gyro_t gyro; // gyro access functions @@ -43,21 +45,27 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; +static uint16_t gyroSoftNotchHz; +static uint8_t gyroSoftNotchQ; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; + gyroSoftNotchHz = gyro_soft_notch_hz; + gyroSoftNotchQ = gyro_soft_notch_q; } void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); + biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -157,7 +165,17 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]); + float sample = (float) gyroADC[axis]; + if (gyroSoftNotchHz) { + sample = biquadFilterApply(&gyroFilterNotch[axis], sample); + } + + if (debugMode == DEBUG_NOTCH && axis < 2){ + debug[axis*2 + 0] = gyroADC[axis]; + debug[axis*2 + 1] = lrintf(sample); + } + + gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);; gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2239eb6816..4d2153fa0d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,7 +40,7 @@ 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(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index eb206627b4..38d81a1f4f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -89,7 +89,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #if defined(MPU_INT_EXTI) static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) }; return &mpuIntExtiConfig; -#elif defined(USE_HARDWARE_REVISION_DETECTION) +#elif defined(USE_HARDWARE_REVISION_DETECTION) return selectMPUIntExtiConfigByHardwareRevision(); #else return NULL; @@ -118,9 +118,15 @@ static bool fakeGyroReadTemp(int16_t *tempData) return true; } + +static bool fakeGyroInitStatus(void) { + return true; +} + bool fakeGyroDetect(gyro_t *gyro) { gyro->init = fakeGyroInit; + gyro->intStatus = fakeGyroInitStatus; gyro->read = fakeGyroRead; gyro->temperature = fakeGyroReadTemp; gyro->scale = 1.0f / 16.4f; @@ -143,6 +149,7 @@ bool fakeAccDetect(acc_t *acc) { acc->init = fakeAccInit; acc->read = fakeAccRead; + acc->acc_1G = 512*8; acc->revisionCode = 0; return true; } @@ -584,10 +591,14 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator) +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, + uint8_t accHardwareToUse, + uint8_t magHardwareToUse, + uint8_t baroHardwareToUse, + int16_t magDeclinationFromConfig, + uint8_t gyroLpf, + uint8_t gyroSyncDenominator) { - int16_t deg, min; - memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); @@ -605,7 +616,6 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a detectAcc(accHardwareToUse); detectBaro(baroHardwareToUse); - // Now time to init things, acc first if (sensors(SENSOR_ACC)) { acc.acc_1G = 256; // set default @@ -623,9 +633,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (sensors(SENSOR_MAG)) { // calculate magnetic declination - deg = magDeclinationFromConfig / 100; - min = magDeclinationFromConfig % 100; - + const int16_t deg = magDeclinationFromConfig / 100; + const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } else { magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 3e60de5031..e4c97dfc55 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -21,18 +21,14 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PB5 // Blue LED - PB5 +#define LED0 PB5 // Blue LED - PB5 -#define BEEPER PA0 - - -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define BEEPER PA0 // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -41,20 +37,18 @@ #define ACC #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define USE_ACC_MPU6050 - -#define ACC_MPU6050_ALIGN CW180_DEG +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 //#define BARO //#define USE_BARO_MS5611 @@ -66,31 +60,30 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 -#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 -#define UART2_RX_PIN PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 - -#define UART3_TX_PIN PB10 //(AF7) -#define UART3_RX_PIN PB11 //(AF7) +#define UART3_TX_PIN PB10 //(AF7) +#define UART3_RX_PIN PB11 //(AF7) #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 #define USE_SPI #define USE_SPI_DEVICE_2 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 //#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #undef GPS #define DISPLAY @@ -99,10 +92,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA5 -//#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +//#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 @@ -135,17 +128,18 @@ #define SPEKTRUM_BIND // USART2, PB4 -#define BIND_PIN PB4 +#define BIND_PIN PB4 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) // #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 8c693a98cf..271c9292c4 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -84,9 +84,14 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; + masterConfig.gyro_sync_denom = 2; + masterConfig.pid_process_denom = 1; + currentProfile->pidProfile.P8[ROLL] = 90; + currentProfile->pidProfile.I8[ROLL] = 44; + currentProfile->pidProfile.D8[ROLL] = 60; + currentProfile->pidProfile.P8[PITCH] = 90; + currentProfile->pidProfile.I8[PITCH] = 44; + currentProfile->pidProfile.D8[PITCH] = 60; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index 7e60ddc55f..9b23c614d1 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ #pragma once - + #include "drivers/exti.h" typedef enum awf3HardwareRevision_t { diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index b05cdb33a4..8ce5876683 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -23,17 +23,17 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_HARDWARE_REVISION_DETECTION -#define HW_PIN PB2 +#define HW_PIN PB2 // LED's V1 -#define LED0 PB4 // LED - PB4 -#define LED1 PB5 // LED - PB5 +#define LED0 PB4 +#define LED1 PB5 // LED's V2 -#define LED0_A PB8 // LED - PB8 -#define LED1_A PB9 // LED - PB9 +#define LED0_A PB8 +#define LED1_A PB9 -#define BEEPER PA5 // LED - PA5 +#define BEEPER PA5 #define USABLE_TIMER_CHANNEL_COUNT 11 @@ -48,15 +48,15 @@ #define USE_GYRO_MPU6050 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6050_ALIGN CW270_DEG -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6050_ALIGN CW270_DEG -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG // No baro support. //#define BARO @@ -65,29 +65,28 @@ // option to use MPU9150 or MPU9250 integrated AK89xx Mag #define MAG #define USE_MAG_AK8963 - -#define MAG_AK8963_ALIGN CW0_DEG_FLIP +#define MAG_AK8963_ALIGN CW0_DEG_FLIP #define USE_VCP #define USE_UART1 // Not connected - TX (PB6) RX PB7 (AF7) #define USE_UART2 // Receiver - RX (PA3) #define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10) -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 // PA2 -#define UART2_RX_PIN PA3 // PA3 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 // SPI3 // PA15 38 SPI3_NSS @@ -98,22 +97,22 @@ #define USE_SPI #define USE_SPI_DEVICE_3 -#define MPU6500_CS_PIN PA15 -#define MPU6500_SPI_INSTANCE SPI3 +#define MPU6500_CS_PIN PA15 +#define MPU6500_SPI_INSTANCE SPI3 #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define VBAT_SCALE_DEFAULT 20 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define VBAT_SCALE_DEFAULT 20 #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB12 (Pin 25) -#define BINDPLUG_PIN PB12 +#define BINDPLUG_PIN PB12 #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP @@ -124,12 +123,11 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 8c693a98cf..56086358c1 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -84,9 +84,14 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; + masterConfig.gyro_sync_denom = 1; + masterConfig.pid_process_denom = 1; + currentProfile->pidProfile.P8[ROLL] = 90; + currentProfile->pidProfile.I8[ROLL] = 44; + currentProfile->pidProfile.D8[ROLL] = 60; + currentProfile->pidProfile.P8[PITCH] = 90; + currentProfile->pidProfile.I8[PITCH] = 44; + currentProfile->pidProfile.D8[PITCH] = 60; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 909026c021..5bd61457fc 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -23,39 +23,39 @@ #define USBD_PRODUCT_STRING "AlienFlight F4" -#define LED0 PC12 -#define LED1 PD2 +#define LED0 PC12 +#define LED1 PD2 -#define BEEPER PC13 +#define BEEPER PC13 -#define INVERTER PC15 -#define INVERTER_USART USART2 +#define INVERTER PC15 +#define INVERTER_USART USART2 // MPU interrupt //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define MPU_INT_EXTI PC14 +#define MPU_INT_EXTI PC14 #define USE_EXTI -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define MAG #define USE_MAG_HMC5883 #define USE_MAG_AK8963 -#define MAG_HMC5883_ALIGN CW180_DEG -#define MAG_AK8963_ALIGN CW270_DEG +#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_AK8963_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -65,14 +65,14 @@ //#define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PB10 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB -#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn +#define SDCARD_DETECT_PIN PB10 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz @@ -93,57 +93,53 @@ //#define USE_FLASHFS //#define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 13 - #define USE_VCP #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 //inverter +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 //inverter //#define USE_UART3 -//#define UART3_RX_PIN PB11 -//#define UART3_TX_PIN PB10 +//#define UART3_RX_PIN PB11 +//#define UART3_TX_PIN PB10 #define USE_UART4 -#define UART4_RX_PIN PC10 -#define UART4_TX_PIN PC11 +#define UART4_RX_PIN PC10 +#define UART4_TX_PIN PC11 //#define USE_UART5 -//#define UART5_RX_PIN PD2 -//#define UART5_TX_PIN PC12 +//#define UART5_RX_PIN PD2 +//#define UART5_TX_PIN PC12 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #define USE_SPI - #define USE_SPI_DEVICE_1 - #define USE_SPI_DEVICE_2 -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PC2 -#define SPI2_MOSI_PIN PC3 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 #define USE_SPI_DEVICE_3 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) -//#define I2C_DEVICE_EXT (I2CDEV_2) -#define I2C1_SCL PB6 -#define I2C1_SDA PB7 +#define I2C_DEVICE (I2CDEV_1) +//#define I2C_DEVICE_EXT (I2CDEV_2) +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 #define USE_ADC //#define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC4 -#define EXTERNAL1_ADC_GPIO_PIN PC5 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC4 +#define EXTERNAL1_ADC_GPIO_PIN PC5 // LED strip configuration using RC5 pin. //#define LED_STRIP @@ -156,11 +152,11 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB2 (Pin 28) -#define BINDPLUG_PIN PB2 +#define BINDPLUG_PIN PB2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -172,10 +168,11 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) +#define USABLE_TIMER_CHANNEL_COUNT 13 +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index f005e5d1a0..b85e50f539 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -33,7 +33,7 @@ const uint16_t multiPPM[] = { }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM1 | (MAP_TO_PWM_INPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index e83499615a..34bdcf8aa7 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -20,40 +20,40 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "BlueJayF4" +#define USBD_PRODUCT_STRING "BlueJayF4" #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI -#define INVERTER PB15 -#define INVERTER_USART USART6 +#define LED0 PB6 +#define LED1 PB5 +#define LED2 PB4 -#define BEEPER PB7 +#define BEEPER PB7 #define BEEPER_INVERTED -#define LED0 PB6 -#define LED1 PB5 -#define LED2 PB4 +#define INVERTER PB15 +#define INVERTER_USART USART6 -#define MPU6500_CS_PIN PC4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PC4 +#define MPU6500_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG //#define MAG //#define USE_MAG_AK8963 #define BARO #define USE_BARO_MS5611 -#define MS5611_I2C_INSTANCE I2CDEV_1 +#define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD @@ -76,8 +76,8 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN PB3 -//#define M25P16_SPI_INSTANCE SPI3 +//#define M25P16_CS_PIN PB3 +//#define M25P16_SPI_INSTANCE SPI3 //#define USE_FLASHFS //#define USE_FLASH_M25P16 @@ -96,22 +96,21 @@ //#define VBUS_SENSING_ENABLED #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 @@ -134,15 +133,15 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define USE_ADC -#define VBAT_ADC_PIN PC3 +#define VBAT_ADC_PIN PC3 #define LED_STRIP // LED Strip can run off Pin 6 (PB1) of the ESC outputs. -#define WS2811_PIN PB1 +#define WS2811_PIN PB1 #define WS2811_TIMER TIM3 #define WS2811_TIMER_CHANNEL TIM_Channel_4 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER @@ -155,15 +154,15 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a1425bc538..c06f397138 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -17,25 +17,25 @@ #define TARGET_BOARD_IDENTIFIER "CC3D" // CopterControl 3D -#define LED0 PB3 // PB3 (LED) +#define LED0 PB3 -#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO +#define INVERTER_USART USART1 -#define BEEPER PB15 // PB15 (Beeper) -#define BEEPER_OPT PB2 // PB15 (Beeper) +#define BEEPER PB15 +#define BEEPER_OPT PB2 #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA3 +#define MPU_INT_EXTI PA3 -#define MPU6000_CS_GPIO GPIOA -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 -#define M25P16_CS_GPIO GPIOB -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_GPIO GPIOB +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -72,18 +72,18 @@ #define USE_UART1 #define USE_UART3 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #ifdef USE_UART1_RX_DMA #undef USE_UART1_RX_DMA #endif -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_SPI #define USE_SPI_DEVICE_1 @@ -93,15 +93,15 @@ #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PB0 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PB0 #define LED_STRIP -#define WS2811_PIN PB4 -#define WS2811_TIMER TIM3 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER +#define WS2811_PIN PB4 +#define WS2811_TIMER TIM3 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #define SPEKTRUM_BIND // USART3, PB11 (Flexport) @@ -111,16 +111,17 @@ #define DISPLAY #define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +#define SONAR_ECHO_PIN PB0 +#define SONAR_TRIGGER_PIN PB5 #undef GPS #undef BARO +#undef MAG #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP -//#define SKIP_PID_LUXFLOAT +#undef LED_STRIP #undef DISPLAY #undef SONAR #endif @@ -128,8 +129,8 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM // IO - from schematics -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC ( BIT(14) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(14) ) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 52bea95ba2..5623a8a87f 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -22,18 +22,16 @@ #ifndef STM32F3DISCOVERY #define STM32F3DISCOVERY -#endif - -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#endif + +#define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1 PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED -#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 18 - #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 @@ -81,16 +79,14 @@ #define ACC #define USE_ACC_MPU6050 #define USE_ACC_LSM303DLHC - -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MPU6050_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 #define MAG #define USE_MAG_AK8975 - -#define MAG_AK8975_ALIGN CW90_DEG_FLIP +#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define USE_VCP #define USE_UART1 @@ -101,20 +97,20 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)|BIT(5)|BIT(6)|BIT(10)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)|BIT(5)|BIT(6)|BIT(10)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) - -#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)) +#define USABLE_TIMER_CHANNEL_COUNT 18 +#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/CJMCU/hardware_revision.h b/src/main/target/CJMCU/hardware_revision.h index 4b3c13d674..89e3478c13 100755 --- a/src/main/target/CJMCU/hardware_revision.h +++ b/src/main/target/CJMCU/hardware_revision.h @@ -17,7 +17,7 @@ #pragma once #include "drivers/exti.h" - + typedef enum cjmcuHardwareRevision_t { UNKNOWN = 0, REV_1, // Blue LED3 diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index d84e98eae1..4af75b2b2e 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -20,9 +20,9 @@ #define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU #define USE_HARDWARE_REVISION_DETECTION -#define LED0 PC14 // PC14 (LED) -#define LED1 PC13 // PC13 (LED) -#define LED2 PC15 // PC15 (LED) +#define LED0 PC14 +#define LED1 PC13 +#define LED2 PC15 #define ACC #define USE_ACC_MPU6050 @@ -38,7 +38,7 @@ #define USE_UART1 #define USE_UART2 -#define SERIAL_PORT_COUNT 2 +#define SERIAL_PORT_COUNT 2 #define USE_I2C #define I2C_DEVICE (I2CDEV_1) @@ -49,7 +49,7 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #if (FLASH_SIZE > 64) @@ -64,8 +64,8 @@ #endif // IO - assuming all IOs on 48pin package TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index be0764a03b..c2cf9377dc 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -933,7 +933,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #ifdef LED_STRIP case BST_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; bstWrite16(color->h); bstWrite8(color->s); @@ -942,13 +942,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_LED_STRIP_CONFIG: - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - bstWrite16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - bstWrite16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - bstWrite8(GET_LED_X(ledConfig)); - bstWrite8(GET_LED_Y(ledConfig)); - bstWrite8(ledConfig->color); + bstWrite32(*ledConfig); } break; #endif @@ -1381,28 +1377,12 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) case BST_SET_LED_STRIP_CONFIG: { i = bstRead8(); - if (i >= MAX_LED_STRIP_LENGTH || bstReadDataSize() != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || bstReadDataSize() != (1 + 4)) { ret = BST_FAILED; break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = bstRead16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - - mask = bstRead16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - - mask = bstRead8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = bstRead8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = bstRead8(); - + *ledConfig = bstRead32(); reevaluateLedConfig(); } break; diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 65ab62d5d6..d4db45a6de 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -18,20 +18,27 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "CLBR" -#define BST_DEVICE_NAME "COLIBRI RACE" -#define BST_DEVICE_NAME_LENGTH 12 +#define BST_DEVICE_NAME "COLIBRI RACE" +#define BST_DEVICE_NAME_LENGTH 12 #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PC15 -#define LED1 PC14 -#define LED2 PC13 -#define BEEPER PB13 +#define LED0 PC15 +#define LED1 PC14 +#define LED2 PC13 + +#define BEEPER PB13 #define BEEPER_INVERTED +// MPU6500 interrupt #define USE_EXTI +#define MPU_INT_EXTI PA5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +//#define DEBUG_MPU_DATA_READY_INTERRUPT + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -47,23 +54,19 @@ #define MPU6000_CS_PIN SPI1_NSS_PIN #define MPU6000_SPI_INSTANCE SPI1 -#define USABLE_TIMER_CHANNEL_COUNT 11 - -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -80,34 +83,34 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PC4 -#define UART1_RX_PIN PC5 +#define UART1_TX_PIN PC4 +#define UART1_RX_PIN PC5 -#define UART2_TX_PIN PA14 -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL_PIN PA9 -#define I2C2_SDA_PIN PA10 +#define I2C2_SCL_PIN PA9 +#define I2C2_SDA_PIN PA10 #define USE_BST #define BST_DEVICE (BSTDEV_1) /* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ -#define BST_CRC_POLYNOM 0xD5 +#define BST_CRC_POLYNOM 0xD5 #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG @@ -119,14 +122,6 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - -// MPU6500 interrupt -#define USE_EXTI -#define MPU_INT_EXTI PA5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -135,11 +130,12 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index c813ceecba..05b642f088 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -43,7 +43,7 @@ const uint16_t multiPWM[] = { PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index e4c2f76ba0..2399ede8d1 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -20,17 +20,15 @@ #define TARGET_BOARD_IDENTIFIER "DOGE" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + // tqfp48 pin 34 -#define LED0 PA13 - +#define LED0 PA13 // tqfp48 pin 37 -#define LED1 PA14 - +#define LED1 PA14 // tqfp48 pin 38 -#define LED2 PA15 +#define LED2 PA15 -#define BEEPER PB2 +#define BEEPER PB2 #define BEEPER_INVERTED #define USE_SPI @@ -81,13 +79,13 @@ // #define USE_FAKE_GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG // ?? +#define GYRO_MPU6500_ALIGN CW270_DEG // ?? #define ACC // #define USE_FAKE_ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG // ?? +#define ACC_MPU6500_ALIGN CW270_DEG // ?? #define BARO #define USE_BARO_BMP280 @@ -100,20 +98,20 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 -#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 // mpu_int definition in sensors/initialisation.c #define USE_EXTI @@ -135,19 +133,19 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // Use UART3 for speksat -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // !!TODO - check the TARGET_IO_PORTs are correct -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index ee893670dd..12ec85f371 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -19,21 +19,22 @@ #define TARGET_BOARD_IDENTIFIER "EUF1" -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) +#define LED0 PB3 +#define LED1 PB4 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_EXTI -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 + +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 #define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB -#define MPU6500_CS_GPIO GPIOB -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define MPU6500_CS_GPIO GPIOB +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 #define GYRO #define USE_FAKE_GYRO @@ -44,7 +45,7 @@ #define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_MPU6050_ALIGN CW0_DEG #define ACC #define USE_FAKE_ACC @@ -55,7 +56,7 @@ //#define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MPU6050_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 @@ -66,7 +67,7 @@ #define USE_MAG_HMC5883 #define USE_MAG_AK8975 -#define MAG_AK8975_ALIGN CW180_DEG_FLIP +#define MAG_AK8975_ALIGN CW180_DEG_FLIP #define SONAR #define SONAR_TRIGGER_PIN PB0 @@ -80,12 +81,12 @@ #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 @@ -97,21 +98,20 @@ // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 // IO - stm32f103RCT6 in 64pin package (TODO) -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index e023d7e960..a8d80e823d 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -30,11 +30,11 @@ const uint16_t multiPWM[] = { PWM3 | (MAP_TO_PWM_INPUT << 8), PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), - PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -69,11 +69,11 @@ const uint16_t airPWM[] = { PWM3 | (MAP_TO_PWM_INPUT << 8), PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), - PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 @@ -101,7 +101,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT - + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 127a33d4bb..8b4b846c40 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -20,46 +20,47 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "Swift-Flyer F4BY" +#define USBD_PRODUCT_STRING "Swift-Flyer F4BY" -#define LED0 PE3 // Blue LED -#define LED1 PE2 // Red LED -#define LED2 PE1 // Blue LED -#define BEEPER PE5 +#define LED0 PE3 // Blue LED +#define LED1 PE2 // Red LED +#define LED2 PE1 // Blue LED -#define INVERTER PD3 -#define INVERTER_USART USART6 +#define BEEPER PE5 + +#define INVERTER PD3 +#define INVERTER_USART USART6 // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PB0 +#define MPU_INT_EXTI PB0 #define USE_EXTI -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_HMC5883_ALIGN CW90_DEG #define BARO #define USE_BARO_MS5611 #define USE_SDCARD -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PE15 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PE15 // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz @@ -76,43 +77,41 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USE_VCP -#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_PIN PA9 #define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 -#define UART2_RX_PIN PD6 -#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 #define USE_UART3 -#define UART3_RX_PIN PD9 -#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 #define USE_UART4 -#define UART4_RX_PIN PC11 -#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 //#define USE_UART5 - error in DMA!!! -//#define UART5_RX_PIN PD2 -//#define UART5_TX_PIN PC12 +//#define UART5_RX_PIN PD2 +//#define UART5_TX_PIN PC12 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 - -#define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 +#define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 #define USE_SPI - #define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 -#define SPI1_NSS_PIN NONE +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN NONE #define USE_SPI_DEVICE_2 #define SPI2_NSS_PIN NONE @@ -121,24 +120,23 @@ #define SPI2_MOSI_PIN PB15 #define USE_SPI_DEVICE_3 -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define I2C_DEVICE (I2CDEV_2) #define USE_I2C_PULLUP -#define I2C2_SCL PB10 -#define I2C2_SDA PB11 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC3 -#define CURRENT_METER_ADC_PIN PC2 -#define RSSI_ADC_PIN PC1 +#define USE_ADC +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC1 #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL @@ -148,15 +146,15 @@ #define SPEKTRUM_BIND // UART6, PC7 -#define BIND_PIN PC7 +#define BIND_PIN PC7 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff -#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 21b05ab5d4..bf4ff7b20a 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -76,7 +76,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - S2 { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10}, // PWM6 - S3 { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - S4 - + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index e67a920cc6..858b2d76e9 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -20,42 +20,38 @@ #define TARGET_BOARD_IDENTIFIER "FYF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define MPU_INT_EXTI PC4 -#define USE_EXTI #define CONFIG_PREFER_ACC_ON - -#define LED0 PC14 -#define BEEPER PC15 +#define LED0 PC14 + +#define BEEPER PC15 #define BEEPER_INVERTED +#define USE_EXTI +#define MPU_INT_EXTI PC4 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect - #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 + #define GYRO -#define ACC - -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG #define BARO #define USE_BARO_MS5611 @@ -102,8 +98,6 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USABLE_TIMER_CHANNEL_COUNT 8 - #define USB_IO #define USE_VCP @@ -113,31 +107,31 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 1 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 2 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) +#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) -#define I2C1_SCL_PIN PB8 -#define I2C1_SDA_PIN PB9 +#define I2C1_SCL_PIN PB8 +#define I2C1_SDA_PIN PB9 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PA1 -#define CURRENT_METER_ADC_PIN PA2 +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PA1 +#define CURRENT_METER_ADC_PIN PA2 #define LED_STRIP @@ -150,23 +144,24 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#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)) +#define USABLE_TIMER_CHANNEL_COUNT 8 +#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/FURYF4/target.h b/src/main/target/FURYF4/target.h index b9631d6124..c6263bcd47 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -20,44 +20,45 @@ #define TARGET_BOARD_IDENTIFIER "FYF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "FuryF4" +#define USBD_PRODUCT_STRING "FuryF4" -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PA8 +#define LED0 PB5 +#define LED1 PB4 + +#define BEEPER PA8 #define BEEPER_INVERTED #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 +#define MPU_INT_EXTI PC4 #define USE_EXTI #define BARO #define USE_BARO_MS5611 -#define MS5611_I2C_INSTANCE I2CDEV_1 +#define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD @@ -97,29 +98,29 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define M25P16_CS_PIN PB3 -#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 #define USABLE_TIMER_CHANNEL_COUNT 5 #define USE_VCP -#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -138,16 +139,16 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA #define USE_I2C_PULLUP -#define I2C1_SCL PB6 -#define I2C1_SDA PB7 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC1 -#define RSSI_ADC_GPIO_PIN PC2 -#define CURRENT_METER_ADC_PIN PC3 +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_GPIO_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL @@ -157,14 +158,14 @@ #define SPEKTRUM_BIND // USART3 Rx, PB11 -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8e41030edf..8dbd6e55ba 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 #define USABLE_TIMER_CHANNEL_COUNT 17 @@ -32,11 +32,11 @@ #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_BMP085 @@ -49,17 +49,17 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 3 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -67,16 +67,16 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE /* @@ -100,11 +100,10 @@ */ // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index e270422ad6..f0b36fd605 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM9 | (MAP_TO_PPM_INPUT << 8), + PWM12 | (MAP_TO_PPM_INPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -32,7 +32,6 @@ const uint16_t multiPPM[] = { PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; @@ -44,6 +43,7 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), PWM10 | (MAP_TO_PWM_INPUT << 8), @@ -65,8 +65,10 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_6}, { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_2}, - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, // TIM1_CH2N + { TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, // TIM1_CH3N + //{ TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + //{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index de059f7b46..7224db6b6a 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -17,57 +17,56 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "KISSFC" +#define TARGET_BOARD_IDENTIFIER "KISS" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR) -#define LED0 PB1 -#define BEEPER PB13 +#define LED0 PB1 + +#define BEEPER PB13 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_EXTI -#define MPU_INT_EXTI PB2 +#define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG - +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART2_TX_PIN PB3 -#define UART2_RX_PIN PB4 - -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_ADC -#define VBAT_SCALE_DEFAULT 160 -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 -//#define CURRENT_METER_ADC_PIN PA5 -//#define RSSI_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 160 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +//#define CURRENT_METER_ADC_PIN PA5 +//#define RSSI_ADC_PIN PB2 #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL @@ -75,12 +74,12 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define SPEKTRUM_BIND -#define BIND_PIN PB4 +#define BIND_PIN PB4 -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#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)) +#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.h b/src/main/target/LUX_RACE/target.h index ac7d2c06dd..b0a2bf7520 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -21,14 +21,22 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PC15 -#define LED1 PC14 -#define LED2 PC13 -#define BEEPER PB13 +#define LED0 PC15 +#define LED1 PC14 +#define LED2 PC13 + +#define BEEPER PB13 #define BEEPER_INVERTED +// MPU6500 interrupt +#define USE_EXTI +#define MPU_INT_EXTI PA5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -40,19 +48,15 @@ #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 -#define USABLE_TIMER_CHANNEL_COUNT 11 - -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define USB_IO @@ -60,26 +64,26 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PC4 -#define UART1_RX_PIN PC5 +#define UART1_TX_PIN PC4 +#define UART1_RX_PIN PC5 -#define UART2_TX_PIN PA14 -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define WS2811_PIN PA6 // TIM16_CH1 @@ -90,27 +94,21 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -// MPU6500 interrupt -#define USE_EXTI -#define MPU_INT_EXTI PA5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART1, PC5 -#define BIND_PIN PC5 +#define BIND_PIN PC5 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 958924fabe..fe465da9c9 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -21,18 +21,16 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PB5 // Blue LEDs - PB5 -//#define LED1 PB9 // Green LEDs - PB9 -#define BEEPER PA0 +#define LED0 PB5 // Blue LEDs - PB5 +//#define LED1 PB9 // Green LEDs - PB9 + +#define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -41,20 +39,20 @@ #define ACC #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 //#define BARO //#define USE_BARO_MS5611 @@ -66,42 +64,42 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 // PB3 -#define UART2_RX_PIN PB4 // PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C_DEVICE (I2CDEV_2) +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 #define USE_SPI #define USE_SPI_DEVICE_2 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) +//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) +#define SENSORS_SET (SENSOR_ACC) #undef GPS #define DISPLAY #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA5 -//#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +//#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 @@ -135,17 +133,18 @@ #define SPEKTRUM_BIND // USART2, PB4 -#define BIND_PIN PB4 +#define BIND_PIN PB4 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) // #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index f9fde08608..411f4e4154 100644 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -83,9 +83,6 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index adca1659a7..512746d003 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -22,21 +22,27 @@ #define BOARD_HAS_VOLTAGE_DIVIDER -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) +#define LED0 PB3 +#define LED1 PB4 -#define BEEPER PA12 // PA12 (Beeper) +#define BEEPER PA12 #ifdef AFROMINI #define BEEPER_INVERTED #endif -#define BARO_XCLR_PIN PC13 -#define BARO_EOC_PIN PC14 +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_PIN PC14 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_EXTI +#define MAG_INT_EXTI PC14 +#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MAG_DATA_READY_INTERRUPT +#define USE_MAG_DATA_READY_SIGNAL // SPI2 // PB15 28 SPI2_MOSI @@ -47,45 +53,33 @@ #define USE_SPI #define USE_SPI_DEVICE_2 -#define NAZE_SPI_INSTANCE SPI2 -#define NAZE_SPI_CS_GPIO GPIOB -#define NAZE_SPI_CS_PIN PB12 +#define NAZE_SPI_INSTANCE SPI2 +#define NAZE_SPI_CS_GPIO GPIOB +#define NAZE_SPI_CS_PIN PB12 #define NAZE_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO -#define M25P16_CS_PIN NAZE_SPI_CS_PIN -#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE - -#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL -#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO -#define MPU6500_CS_PIN NAZE_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO +#define M25P16_CS_PIN NAZE_SPI_CS_PIN +#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE +#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL +#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO +#define MPU6500_CS_PIN NAZE_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE #define USE_FLASHFS - #define USE_FLASH_M25P16 -#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC - -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL - -//#define DEBUG_MAG_DATA_READY_INTERRUPT -#define USE_MAG_DATA_READY_SIGNAL -#define MAG_INT_EXTI PC14 - #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 - -#define GYRO_MPU3050_ALIGN CW0_DEG -#define GYRO_MPU6050_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU3050_ALIGN CW0_DEG +#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG #define ACC #define USE_ACC_ADXL345 @@ -95,11 +89,11 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_ADXL345_ALIGN CW270_DEG -#define ACC_MPU6050_ALIGN CW0_DEG -#define ACC_MMA8452_ALIGN CW90_DEG -#define ACC_BMA280_ALIGN CW0_DEG -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_ADXL345_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MMA8452_ALIGN CW90_DEG +#define ACC_BMA280_ALIGN CW0_DEG +#define ACC_MPU6500_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 @@ -108,8 +102,7 @@ #define MAG #define USE_MAG_HMC5883 - -#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_HMC5883_ALIGN CW180_DEG #define SONAR #define SONAR_TRIGGER_PIN PB0 @@ -124,7 +117,7 @@ #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 @@ -134,8 +127,8 @@ #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 // USART3 only on NAZE32_SP - Flex Port -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) @@ -145,23 +138,23 @@ // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #undef GPS #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -186,8 +179,8 @@ #endif // ALIENFLIGHTF1 // IO - assuming all IOs on 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index c526310333..bb270cb4fc 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -23,12 +23,12 @@ //#define OLIMEXINO_UNCUT_LED2_E_JUMPER #ifdef OLIMEXINO_UNCUT_LED1_E_JUMPER -#define LED0 PA5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green +#define LED0 PA5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green #endif #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER // "LED2" is using one of the PWM pins (CH2/PWM2), so we must not use PWM2 unless the jumper is cut. @See pwmInit() -#define LED1 PA1 // D3, PA1/UART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow +#define LED1 PA1 // D3, PA1/UART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow #endif #define GYRO @@ -59,14 +59,14 @@ #define USE_MAG_HMC5883 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USE_UART1 #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 @@ -79,17 +79,16 @@ #define I2C_DEVICE (I2CDEV_2) #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 // IO - assuming all IOs on smt32f103rb LQFP64 package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index b9119c5fd3..87c5c286a9 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -21,9 +21,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 10 // 6 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. @@ -35,19 +35,19 @@ #define MPU_INT_EXTI PC13 #define USE_EXTI -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG -#define BMP280_SPI_INSTANCE SPI1 -#define BMP280_CS_PIN PA13 +#define BMP280_SPI_INSTANCE SPI1 +#define BMP280_CS_PIN PA13 #define BARO #define USE_BARO_BMP280 @@ -60,13 +60,12 @@ //#define MAG_AK8975_ALIGN CW90_DEG_FLIP //#define SONAR -//#define SONAR_ECHO_PIN PB1 -//#define SONAR_TRIGGER_PIN PB0 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION - -#define USB_DETECT_PIN PB5 +#define USB_DETECT_PIN PB5 #define USE_VCP #define USE_UART1 @@ -74,14 +73,14 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) //#define USE_I2C //#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA @@ -100,8 +99,8 @@ #define OSD // include the max7456 driver #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI1 -#define MAX7456_SPI_CS_PIN PB1 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PB1 //#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 //#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER @@ -169,27 +168,28 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define BUTTONS -#define BUTTON_A_PORT GPIOB -#define BUTTON_A_PIN Pin_1 -#define BUTTON_B_PORT GPIOB -#define BUTTON_B_PIN Pin_0 +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define HARDWARE_BIND_PLUG -#define BINDPLUG_PIN PB0 +#define BINDPLUG_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c new file mode 100644 index 0000000000..7dd021055e --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.c @@ -0,0 +1,103 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + 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), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (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 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + 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 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + 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 + 0xFFFF +}; + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT +}; diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h new file mode 100644 index 0000000000..fccca3016b --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.h @@ -0,0 +1,133 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "OBF4" + +#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "OmnibusF4" +#ifdef OPBL +#define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PB5 +#define LED1 PB4 +#define BEEPER PB4 +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC4 +#define USE_EXTI + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW90_DEG + +//#define USE_MAG_NAZA +//#define MAG_NAZA_ALIGN CW180_DEG_FLIP + +#define BARO +#define USE_BARO_MS5611 + +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 + +#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 +#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 +#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER + +//#define PITOT +//#define USE_PITOT_MS4525 +//#define MS4525_BUS I2C_DEVICE_EXT + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_INSTANCE SPI3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USABLE_TIMER_CHANNEL_COUNT 12 + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 + +#define USE_USART1 +#define USART1_RX_PIN PA10 +#define USART1_TX_PIN PA9 +#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_USART3 +#define USART3_RX_PIN PB11 +#define USART3_TX_PIN PB10 + +#define USE_USART6 +#define USART6_RX_PIN PC7 +#define USART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) + +#define USE_ADC +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_GPIO_PIN PA0 + + +#define SENSORS_SET (SENSOR_ACC) + + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk new file mode 100644 index 0000000000..18034c1332 --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.mk @@ -0,0 +1,10 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/max7456.c \ + io/osd.c + diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 938af8f98b..111f6fc476 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -22,63 +22,63 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB9 -#define LED1 PB5 +#define LED0 PB9 +#define LED1 PB5 -#define BEEPER PA0 +#define BEEPER PA0 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 9 // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL #define GYRO -#define ACC - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 // PB3 -#define UART2_RX_PIN PB4 // PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_SPI #define USE_SPI_DEVICE_2 -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #define TELEMETRY #define BLACKBOX #define SERIAL_RX #define USE_SERVOS -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define CURRENT_METER_ADC_PIN PA2 -#define VBAT_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define CURRENT_METER_ADC_PIN PA2 +#define VBAT_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 @@ -124,16 +124,17 @@ #define SPEKTRUM_BIND // USART3, PB11 -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -// #define TARGET_IO_PORTF (BIT(0)|BIT(1)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +// #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 417f889c9f..66e83fa3d5 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -19,36 +19,36 @@ #define TARGET_BOARD_IDENTIFIER "103R" -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) -#define LED2 PD2 // PD2 (LED) - Labelled LED4 +#define LED0 PB3 +#define LED1 PB4 +#define LED2 PD2 // PD2 (LED) - Labelled LED4 -#define BEEPER PA12 // PA12 (Beeper) +#define BEEPER PA12 // PA12 (Beeper) -#define BARO_XCLR_GPIO GPIOC -#define BARO_XCLR_PIN PC13 -#define BARO_EOC_GPIO GPIOC -#define BARO_EOC_PIN PC14 -#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC +#define BARO_XCLR_GPIO GPIOC +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_GPIO GPIOC +#define BARO_EOC_PIN PC14 +#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_SPI #define USE_SPI_DEVICE_2 -#define PORT103R_SPI_INSTANCE SPI2 -#define PORT103R_SPI_CS_PIN PB12 +#define PORT103R_SPI_INSTANCE SPI2 +#define PORT103R_SPI_CS_PIN PB12 // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_PIN PORT103R_SPI_CS_PIN -#define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define M25P16_CS_PIN PORT103R_SPI_CS_PIN +#define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6000_CS_PIN PORT103R_SPI_CS_PIN -#define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define MPU6000_CS_PIN PORT103R_SPI_CS_PIN +#define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6500_CS_PIN PORT103R_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define MPU6500_CS_PIN PORT103R_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE #define GYRO #define USE_FAKE_GYRO @@ -93,37 +93,35 @@ #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define I2C_DEVICE (I2CDEV_2) // #define SOFT_I2C // enable to test software i2c // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) - +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 03f60516d4..67f7327285 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -22,28 +22,28 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - 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), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (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 - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + 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), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (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 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), - PWM3 | (MAP_TO_PWM_INPUT << 8), - PWM4 | (MAP_TO_PWM_INPUT << 8), - PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + 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 @@ -54,7 +54,7 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_SERVO_OUTPUT << 8), @@ -70,12 +70,12 @@ const uint16_t airPPM[] = { }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), - PWM3 | (MAP_TO_PWM_INPUT << 8), - PWM4 | (MAP_TO_PWM_INPUT << 8), - PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + 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 @@ -85,21 +85,17 @@ const uint16_t airPWM[] = { 0xFFFF }; - const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S6_OUT }; - - diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 05c58aa979..9aab5b4b10 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -21,40 +21,40 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "Revolution" +#define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PB4 -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define LED0 PB5 +#define LED1 PB4 +#define BEEPER PB4 +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 -#define USE_EXTI #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_HMC5883_ALIGN CW90_DEG //#define USE_MAG_NAZA -//#define MAG_NAZA_ALIGN CW180_DEG_FLIP +//#define MAG_NAZA_ALIGN CW180_DEG_FLIP #define BARO #define USE_BARO_MS5611 @@ -63,31 +63,29 @@ //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT -#define M25P16_CS_PIN PB3 -#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_VCP -#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_PIN PC5 #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -103,22 +101,23 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define CURRENT_METER_ADC_PIN PC1 -#define VBAT_ADC_PIN PC2 -#define RSSI_ADC_GPIO_PIN PA0 +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_GPIO_PIN PA0 + - #define SENSORS_SET (SENSOR_ACC) -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 6e461c5eb2..448d235b81 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -25,24 +25,26 @@ #define USBD_SERIALNUMBER_STRING "0x8010000" #endif -#define LED0 PC14 -#define LED1 PC13 -#define BEEPER PC13 -#define INVERTER PC15 -#define INVERTER_USART USART2 //Sbus on USART 2 of nano. +#define LED0 PC14 +#define LED1 PC13 -#define MPU9250_CS_PIN PB12 -#define MPU9250_SPI_INSTANCE SPI2 +#define BEEPER PC13 + +#define INVERTER PC15 +#define INVERTER_USART USART2 //Sbus on USART 2 of nano. + +#define MPU9250_CS_PIN PB12 +#define MPU9250_SPI_INSTANCE SPI2 #define ACC #define USE_ACC_MPU9250 #define USE_ACC_SPI_MPU9250 -#define ACC_MPU9250_ALIGN CW270_DEG +#define ACC_MPU9250_ALIGN CW270_DEG #define GYRO #define USE_GYRO_MPU9250 #define USE_GYRO_SPI_MPU9250 -#define GYRO_MPU9250_ALIGN CW270_DEG +#define GYRO_MPU9250_ALIGN CW270_DEG //#define MAG //#define USE_MAG_HMC5883 @@ -51,26 +53,24 @@ #define USE_BARO_MS5611 // MPU9250 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PA15 #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PA15 -#define USE_EXTI - -#define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_VCP -#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_PIN PA9 #define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 #define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 -#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 +#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 #define USE_SPI //#define USE_SPI_DEVICE_1 @@ -81,9 +81,9 @@ #define I2C_DEVICE (I2CDEV_3) #define USE_ADC -#define CURRENT_METER_ADC_PIN PA7 -#define VBAT_ADC_PIN PA6 -#define RSSI_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PA7 +#define VBAT_ADC_PIN PA6 +#define RSSI_ADC_PIN PA5 #define GPS #define BLACKBOX @@ -92,10 +92,10 @@ #define USE_SERVOS #define USE_CLI -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) +#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 14fd760c8e..67d2232e68 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -21,29 +21,24 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready - #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_BMP280 @@ -52,34 +47,34 @@ #define USE_FLASH_M25P16 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 // PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -88,12 +83,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP @@ -109,15 +104,16 @@ #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 06e9f3cfee..09a5678174 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -21,23 +21,23 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define USABLE_TIMER_CHANNEL_COUNT 10 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP +#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW0_DEG_FLIP +#define ACC_MPU6050_ALIGN CW0_DEG_FLIP #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -49,22 +49,22 @@ #define USE_SOFTSERIAL1 // Telemetry #define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 //Not connected -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 //Not connected +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 -#define SOFTSERIAL_1_TIMER TIM15 +#define SOFTSERIAL_1_TIMER TIM15 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 7 //Not connected #define SOFTSERIAL_1_TIMER_TX_HARDWARE 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PA4, 5, 6, 7 @@ -81,9 +81,9 @@ #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PB2 -#define VBAT_SCALE_DEFAULT 77 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 77 #define LED_STRIP @@ -103,17 +103,17 @@ #define SPEKTRUM_BIND // USART2, PA15 -#define BIND_PIN PA15 +#define BIND_PIN PA15 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) //#define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index 0630b527db..b52fbd00e4 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - // No PPM + PWM7 | (MAP_TO_PPM_INPUT << 8), PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -43,21 +43,21 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - // No PPM - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_PPM_INPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -73,6 +73,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 42f2378c66..1632d2046d 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -19,14 +19,14 @@ #define TARGET_BOARD_IDENTIFIER "SIRF" -#define LED0 PB2 -#define BEEPER PA1 +#define LED0 PB2 +#define BEEPER PA1 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA8 +#define MPU_INT_EXTI PA8 #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO @@ -40,18 +40,18 @@ #define USE_ACC_SPI_MPU6500 // MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG -#define GYRO_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 // MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG -#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define USB_IO @@ -59,16 +59,16 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK -#define UART2_RX_PIN PA3 // PA15 +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 // PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #undef USE_I2C @@ -93,17 +93,17 @@ #define SPI3_MOSI_PIN PB5 #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI3 -#define MAX7456_SPI_CS_PIN PA15 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 #define MAX7456_DMA_CHANNEL_TX DMA2_Channel2 #define MAX7456_DMA_CHANNEL_RX DMA2_Channel1 #define MAX7456_DMA_IRQ_HANDLER_ID DMA2_CH1_HANDLER #define USE_RTC6705 -#define RTC6705_SPIDATA_PIN PC15 -#define RTC6705_SPILE_PIN PC14 -#define RTC6705_SPICLK_PIN PC13 +#define RTC6705_SPIDATA_PIN PC15 +#define RTC6705_SPILE_PIN PC14 +#define RTC6705_SPICLK_PIN PC13 #define USE_SDCARD #define USE_SDCARD_SPI2 @@ -124,32 +124,33 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define OSD #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_SERVOS -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 6 -#define USED_TIMERS (TIM_N(3) | TIM_N(4)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 945fb1fe9d..19cc56e950 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -15,4 +15,3 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c - diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index f82be07689..ab498ff5ac 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -21,30 +21,26 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB4 // Blue (Rev 1 & 2) - PB4 -#define LED1 PB5 // Green (Rev 1) / Red (Rev 2) - PB5 +#define LED0 PB4 // Blue (Rev 1 & 2) - PB4 +#define LED1 PB5 // Green (Rev 1) / Red (Rev 2) - PB5 -#define BEEPER PA1 +#define BEEPER PA1 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 11 - // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. #define GYRO #define USE_GYRO_MPU6050 - -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 - -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -53,22 +49,22 @@ #define MAG #define USE_MAG_AK8975 -#define MAG_AK8975_ALIGN CW180_DEG_FLIP +#define MAG_AK8975_ALIGN CW180_DEG_FLIP #define USE_VCP #define USE_UART1 // Conn 1 - TX (PB6) RX PB7 (AF7) #define USE_UART2 // Input - RX (PA3) #define USE_UART3 // Servo out - 10/RX (PB11) 11/TX (PB10) -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 // PA2 - Clashes with PWM6 input. -#define UART2_RX_PIN PA3 // PA3 +#define UART2_TX_PIN PA2 // PA2 - Clashes with PWM6 input. +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) // Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP? @@ -76,11 +72,11 @@ #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA7 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA7 -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP #if 1 @@ -115,21 +111,20 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 //#define SONAR -//#define SONAR_ECHO_PIN PB1 -//#define SONAR_TRIGGER_PIN PA2 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN PA2 // available IO pins (from schematics) //#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) //#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)) // !!TODO - check following lines are correct -#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 2380fd1008..4b2ac2e943 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -21,27 +21,24 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready - #define USE_EXTI #define MPU_INT_EXTI PC13 +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -51,37 +48,36 @@ #define MAG #define USE_MAG_AK8975 #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW270_DEG +#define MAG_HMC5883_ALIGN CW270_DEG #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MAG_INT_EXTI PC14 - +#define MAG_INT_EXTI PC14 #define USE_FLASHFS #define USE_FLASH_M25P16 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 #define SOFTSERIAL_2_TIMER TIM3 @@ -97,12 +93,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP @@ -121,16 +117,16 @@ #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 0bae984252..94117a125c 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -20,10 +20,10 @@ #define TARGET_BOARD_IDENTIFIER "SPEV" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PB8 -#define BEEPER PC15 +#define LED0 PB8 + +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip @@ -31,7 +31,7 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -40,25 +40,22 @@ #define GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define ACC -#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 -//#define MAG -//#define USE_MPU9250_MAG // Enables bypass configuration -//#define USE_MAG_AK8963 +#define MAG +#define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External -//#define MAG_AK8963_ALIGN CW90_DEG_FLIP +#define MAG_AK8963_ALIGN CW90_DEG_FLIP //#define SONAR @@ -68,19 +65,19 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -117,13 +114,12 @@ #define MPU6500_CS_PIN PB9 #define MPU6500_SPI_INSTANCE SPI1 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #define USE_LED_STRIP_ON_DMA1_CHANNEL2 @@ -149,21 +145,20 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index d76558a9c2..799da79d47 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -24,9 +24,9 @@ // early prototype had slightly different pin mappings. //#define SPRACINGF3MINI_MKII_REVA -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. @@ -34,24 +34,22 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO //#define USE_FAKE_GYRO #define USE_GYRO_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG #define ACC //#define USE_FAKE_ACC #define USE_ACC_MPU6500 - -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 @@ -60,40 +58,39 @@ #define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8975 #define USE_MAG_HMC5883 // External - -#define MAG_AK8975_ALIGN CW90_DEG_FLIP +#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION -#define USB_DETECT_PIN PB5 +#define USB_DETECT_PIN PB5 #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define SOFTSERIAL_1_TIMER TIM2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -124,10 +121,8 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - - +#define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 #define CURRENT_METER_ADC_PIN PA5 @@ -141,7 +136,6 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA #define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA @@ -158,27 +152,29 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define BUTTONS -#define BUTTON_A_PORT GPIOB -#define BUTTON_A_PIN Pin_1 -#define BUTTON_B_PORT GPIOB -#define BUTTON_B_PIN Pin_0 +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define HARDWARE_BIND_PLUG -#define BINDPLUG_PIN PB0 +#define BINDPLUG_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 8ac7eca05f..592833bc48 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,12 +31,12 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1 PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED -#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED #define USE_SPI @@ -50,9 +50,9 @@ //#define USE_SD_CARD // -//#define SD_DETECT_PIN PC14 -//#define SD_CS_PIN PB12 -//#define SD_SPI_INSTANCE SPI2 +//#define SD_DETECT_PIN PC14 +//#define SD_CS_PIN PB12 +//#define SD_SPI_INSTANCE SPI2 //#define USE_FLASHFS //#define USE_FLASH_M25P16 @@ -74,39 +74,39 @@ #define GYRO #define USE_GYRO_L3GD20 -#define L3GD20_SPI SPI1 -#define L3GD20_CS_PIN PE3 -#define GYRO_L3GD20_ALIGN CW270_DEG +#define L3GD20_SPI SPI1 +#define L3GD20_CS_PIN PE3 +#define GYRO_L3GD20_ALIGN CW270_DEG // Support the GY-91 MPU9250 dev board #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN PC14 -#define MPU6500_SPI_INSTANCE SPI2 -#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP +#define MPU6500_CS_PIN PC14 +#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP #define ACC #define USE_ACC_LSM303DLHC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG_FLIP +#define ACC_MPU6500_ALIGN CW270_DEG_FLIP //#define BARO -//#define BMP280_CS_PIN PB12 -//#define BMP280_SPI_INSTANCE SPI2 +//#define BMP280_CS_PIN PB12 +//#define BMP280_SPI_INSTANCE SPI2 //#define USE_BARO_BMP280 //#define USE_BARO_SPI_BMP280 #define OSD #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN //#define USE_SDCARD //#define USE_SDCARD_SPI2 // -//#define SDCARD_SPI_INSTANCE SPI2 -//#define SDCARD_SPI_CS_PIN PB12 +//#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_PIN PB12 //// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: //#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 //// Divide to under 25MHz for normal operation: @@ -125,7 +125,7 @@ #define USE_VCP #define USE_UART1 #define USE_UART2 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 3 // uart2 gpio for shared serial rx/ppm //#define UART2_TX_PIN GPIO_Pin_5 // PD5 @@ -136,14 +136,14 @@ //#define UART2_RX_PINSOURCE GPIO_PinSource6 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define WS2811_PIN PB8 // TIM16_CH1 @@ -153,19 +153,17 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define LED_STRIP -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - 303 in 100pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF 0x00ff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0x00ff - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 4a38e233fb..024a3a6c28 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -22,33 +22,31 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PC14 -#define BEEPER PC15 +#define LED0 PC14 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 17 - - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_EXTI @@ -60,27 +58,27 @@ #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK -#define UART2_RX_PIN PA3 // PA15 +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 @@ -100,12 +98,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP @@ -119,21 +117,20 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index dc9f80cb23..c65abb7d92 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -21,32 +21,30 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG #define ACC #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 BARO #define USE_BARO_BMP280 @@ -54,17 +52,17 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 3 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -74,7 +72,7 @@ #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 - + #define MPU6500_CS_PIN PB9 #define MPU6500_SPI_INSTANCE SPI1 @@ -84,25 +82,25 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/common.h b/src/main/target/common.h index c9a9a0f1b3..65ab02c871 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - + #pragma once #define I2C1_OVERCLOCK true diff --git a/src/main/target/system_stm32f30x.c b/src/main/target/system_stm32f30x.c index 09ba3fa47c..5d62e47aa3 100644 --- a/src/main/target/system_stm32f30x.c +++ b/src/main/target/system_stm32f30x.c @@ -9,17 +9,17 @@ * and is generated by the clock configuration tool * stm32f30x_Clock_Configuration_V1.0.0.xls * - * 1. This file provides two functions and one global variable to be called from + * 1. This file provides two functions and one global variable to be called from * user application: * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier * and Divider factors, AHB/APBx prescalers and Flash settings), - * depending on the configuration made in the clock xls tool. - * This function is called at startup just after reset and + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and * before branch to main program. This call is made inside * the "startup_stm32f30x.s" file. * * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used - * by the user application to setup the SysTick + * by the user application to setup the SysTick * timer or configure other parameters. * * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must @@ -31,7 +31,7 @@ * configure the system clock before to branch to main program. * * 3. If the system clock source selected by user fails to startup, the SystemInit() - * function will do nothing and HSI still used as system clock source. User can + * function will do nothing and HSI still used as system clock source. User can * add some code to deal with this issue inside the SetSysClock() function. * * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define @@ -79,8 +79,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -93,8 +93,8 @@ /** @addtogroup stm32f30x_system * @{ - */ - + */ + /** @addtogroup STM32F30x_System_Private_Includes * @{ */ @@ -113,13 +113,13 @@ uint32_t hse_value = HSE_VALUE; * @{ */ /*!< Uncomment the following line if you need to relocate your vector Table in - Internal SRAM. */ + Internal SRAM. */ /* #define VECT_TAB_SRAM */ -#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ /** * @} - */ + */ /* Private macro -------------------------------------------------------------*/ @@ -151,7 +151,7 @@ void SetSysClock(void); /** * @brief Setup the microcontroller system - * Initialize the Embedded Flash Interface, the PLL and update the + * Initialize the Embedded Flash Interface, the PLL and update the * SystemFrequency variable. * @param None * @retval None @@ -184,19 +184,19 @@ void SystemInit(void) /* Reset USARTSW[1:0], I2CSW and TIMs bits */ RCC->CFGR3 &= (uint32_t)0xFF00FCCC; - + /* Disable all interrupts */ RCC->CIR = 0x00000000; - /* Configure the System clock source, PLL Multiplier and Divider factors, + /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ //SetSysClock(); // called from main() - + #ifdef VECT_TAB_SRAM SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ #else SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ -#endif +#endif } /** @@ -204,25 +204,25 @@ void SystemInit(void) * The SystemCoreClock variable contains the core clock (HCLK), it can * be used by the user application to setup the SysTick timer or configure * other parameters. - * + * * @note Each time the core clock (HCLK) changes, this function must be called * to update SystemCoreClock variable value. Otherwise, any configuration - * based on this variable will be incorrect. - * - * @note - The system frequency computed by this function is not the real - * frequency in the chip. It is calculated based on the predefined + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined * constant and the selected clock source: * * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) * * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) * - * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) * or HSI_VALUE(*) multiplied/divided by the PLL factors. - * + * * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value * 8 MHz) but the real value may vary depending on the variations - * in voltage and temperature. + * in voltage and temperature. * * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value * 8 MHz), user has to ensure that HSE_VALUE is same as the real @@ -231,7 +231,7 @@ void SystemInit(void) * * - The result of this function could be not correct when using fractional * value for HSE crystal. - * + * * @param None * @retval None */ @@ -241,7 +241,7 @@ void SystemCoreClockUpdate (void) /* Get SYSCLK source -------------------------------------------------------*/ tmp = RCC->CFGR & RCC_CFGR_SWS; - + switch (tmp) { case 0x00: /* HSI used as system clock */ @@ -255,7 +255,7 @@ void SystemCoreClockUpdate (void) pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; pllmull = ( pllmull >> 18) + 2; - + if (pllsource == 0x00) { /* HSI oscillator clock divided by 2 selected as PLL clock entry */ @@ -265,8 +265,8 @@ void SystemCoreClockUpdate (void) { prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; /* HSE oscillator clock selected as PREDIV1 clock entry */ - SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; - } + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } break; default: /* HSI used as system clock */ SystemCoreClock = HSI_VALUE; @@ -276,7 +276,7 @@ void SystemCoreClockUpdate (void) /* Get HCLK prescaler */ tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + SystemCoreClock >>= tmp; } /** @@ -298,7 +298,7 @@ void SetSysClock(void) /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ /* Enable HSE */ RCC->CR |= ((uint32_t)RCC_CR_HSEON); - + /* Wait till HSE is ready and if Time out is reached exit */ do { @@ -319,13 +319,13 @@ void SetSysClock(void) { /* Enable Prefetch Buffer and set Flash Latency */ FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; - + /* HCLK = SYSCLK / 1 */ RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; - + /* PCLK2 = HCLK / 1 */ RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; - + /* PCLK1 = HCLK / 2 */ RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; diff --git a/src/main/target/system_stm32f30x.h b/src/main/target/system_stm32f30x.h index 4f999d3058..b213ad0b1e 100644 --- a/src/main/target/system_stm32f30x.h +++ b/src/main/target/system_stm32f30x.h @@ -4,7 +4,7 @@ * @author MCD Application Team * @version V1.1.1 * @date 28-March-2014 - * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. ****************************************************************************** * @attention * @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -31,8 +31,8 @@ /** @addtogroup stm32f30x_system * @{ - */ - + */ + /** * @brief Define to prevent recursive inclusion */ @@ -41,7 +41,7 @@ #ifdef __cplusplus extern "C" { -#endif +#endif /* Exported types ------------------------------------------------------------*/ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ @@ -52,7 +52,7 @@ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Cloc /** @addtogroup STM32F30x_System_Exported_Functions * @{ */ - + extern void SystemInit(void); extern void SystemCoreClockUpdate(void); @@ -69,8 +69,8 @@ extern void SystemCoreClockUpdate(void); /** * @} */ - + /** * @} - */ + */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/system_stm32f4xx.h b/src/main/target/system_stm32f4xx.h index e266366801..45c811bb06 100644 --- a/src/main/target/system_stm32f4xx.h +++ b/src/main/target/system_stm32f4xx.h @@ -4,8 +4,8 @@ * @author MCD Application Team * @version V1.6.1 * @date 21-October-2015 - * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. - ****************************************************************************** + * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. + ****************************************************************************** * @attention * *

© COPYRIGHT 2015 STMicroelectronics

@@ -16,21 +16,21 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * - ****************************************************************************** - */ + ****************************************************************************** + */ #ifndef __SYSTEM_STM32F4XX_H #define __SYSTEM_STM32F4XX_H #ifdef __cplusplus extern "C" { -#endif +#endif extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ extern void SystemInit(void); diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index 7384b1e0b4..d7552ea0fb 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -68,13 +68,13 @@ void Set_System(void) { #if !defined(STM32L1XX_MD) && !defined(STM32L1XX_HD) && !defined(STM32L1XX_MD_PLUS) GPIO_InitTypeDef GPIO_InitStructure; -#endif /* STM32L1XX_MD && STM32L1XX_XD */ +#endif /* STM32L1XX_MD && STM32L1XX_XD */ #if defined(USB_USE_EXTERNAL_PULLUP) GPIO_InitTypeDef GPIO_InitStructure; -#endif /* USB_USE_EXTERNAL_PULLUP */ +#endif /* USB_USE_EXTERNAL_PULLUP */ - /*!< At this stage the microcontroller clock setting is already configured, + /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to @@ -83,7 +83,7 @@ void Set_System(void) #if defined(STM32L1XX_MD) || defined(STM32L1XX_HD)|| defined(STM32L1XX_MD_PLUS) || defined(STM32F37X) || defined(STM32F303xC) /* Enable the SYSCFG module clock */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); -#endif /* STM32L1XX_XD */ +#endif /* STM32L1XX_XD */ /*Pull down PA12 to create USB Disconnect Pulse*/ // HJI #if defined(STM32F303xC) // HJI @@ -277,7 +277,7 @@ static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len) /******************************************************************************* * Function Name : Send DATA . - * Description : send the data received from the STM32 to the PC through USB + * Description : send the data received from the STM32 to the PC through USB * Input : None. * Output : None. * Return : None. diff --git a/src/main/vcp/platform_config.h b/src/main/vcp/platform_config.h index 35715588e9..3f09726229 100644 --- a/src/main/vcp/platform_config.h +++ b/src/main/vcp/platform_config.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/stm32_it.h b/src/main/vcp/stm32_it.h index 959a1e6db0..f3758882bf 100644 --- a/src/main/vcp/stm32_it.h +++ b/src/main/vcp/stm32_it.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_conf.h b/src/main/vcp/usb_conf.h index 6b322297ea..c0c17673ff 100644 --- a/src/main/vcp/usb_conf.h +++ b/src/main/vcp/usb_conf.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_desc.c b/src/main/vcp/usb_desc.c index 64ead56f62..3db4216b68 100644 --- a/src/main/vcp/usb_desc.c +++ b/src/main/vcp/usb_desc.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_desc.h b/src/main/vcp/usb_desc.h index 92f38341a3..9a21291c0c 100644 --- a/src/main/vcp/usb_desc.h +++ b/src/main/vcp/usb_desc.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_istr.h b/src/main/vcp/usb_istr.h index 1d72d91c67..7285da6852 100644 --- a/src/main/vcp/usb_istr.h +++ b/src/main/vcp/usb_istr.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_prop.c b/src/main/vcp/usb_prop.c index b070ce9cca..c8558de342 100644 --- a/src/main/vcp/usb_prop.c +++ b/src/main/vcp/usb_prop.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_prop.h b/src/main/vcp/usb_prop.h index 12d050c38d..f8e1d20249 100644 --- a/src/main/vcp/usb_prop.h +++ b/src/main/vcp/usb_prop.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcpf4/stm32f4xx_it.h b/src/main/vcpf4/stm32f4xx_it.h index 74f17c9cce..dd000126fd 100644 --- a/src/main/vcpf4/stm32f4xx_it.h +++ b/src/main/vcpf4/stm32f4xx_it.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file GPIO/IOToggle/stm32f4xx_it.h + * @file GPIO/IOToggle/stm32f4xx_it.h * @author MCD Application Team * @version V1.0.0 * @date 19-September-2011 @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_IT_H @@ -25,7 +25,7 @@ #ifdef __cplusplus extern "C" { -#endif +#endif /* Includes ------------------------------------------------------------------*/ #include "stm32f4xx.h" diff --git a/src/main/vcpf4/usb_conf.h b/src/main/vcpf4/usb_conf.h index ea8d49bcf1..bd5bbe14f1 100644 --- a/src/main/vcpf4/usb_conf.h +++ b/src/main/vcpf4/usb_conf.h @@ -30,15 +30,15 @@ /** @addtogroup USB_OTG_DRIVER * @{ */ - + /** @defgroup USB_CONF * @brief USB low level driver configuration file * @{ - */ + */ /** @defgroup USB_CONF_Exported_Defines * @{ - */ + */ /* USB Core and PHY interface configuration. Tip: To avoid modifying these defines each time you need to change the USB @@ -66,7 +66,7 @@ #endif /* USE_I2C_PHY */ -#ifdef USE_USB_OTG_FS +#ifdef USE_USB_OTG_FS #define USB_OTG_FS_CORE #endif @@ -76,35 +76,35 @@ /******************************************************************************* * FIFO Size Configuration in Device mode -* -* (i) Receive data FIFO size = RAM for setup packets + +* +* (i) Receive data FIFO size = RAM for setup packets + * OUT endpoint control information + * data OUT packets + miscellaneous * Space = ONE 32-bits words * --> RAM for setup packets = 10 spaces -* (n is the nbr of CTRL EPs the device core supports) +* (n is the nbr of CTRL EPs the device core supports) * --> OUT EP CTRL info = 1 space -* (one space for status information written to the FIFO along with each +* (one space for status information written to the FIFO along with each * received packet) -* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces +* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces * (MINIMUM to receive packets) -* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces +* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces * (if high-bandwidth EP is enabled or multiple isochronous EPs) * --> miscellaneous = 1 space per OUT EP -* (one space for transfer complete status information also pushed to the +* (one space for transfer complete status information also pushed to the * FIFO with each endpoint's last packet) * -* (ii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for +* (ii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for * that particular IN EP. More space allocated in the IN EP Tx FIFO results * in a better performance on the USB and can hide latencies on the AHB. * * (iii) TXn min size = 16 words. (n : Transmit FIFO index) -* (iv) When a TxFIFO is not used, the Configuration should be as follows: +* (iv) When a TxFIFO is not used, the Configuration should be as follows: * case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txm can use the space allocated for Txn. * case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txn should be configured with the minimum space of 16 words -* (v) The FIFO is used optimally when used TxFIFOs are allocated in the top +* (v) The FIFO is used optimally when used TxFIFOs are allocated in the top * of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones. *******************************************************************************/ @@ -126,7 +126,7 @@ * then the space must be at least two times the maximum packet size for * that channel. *******************************************************************************/ - + /****************** USB OTG HS CONFIGURATION **********************************/ #ifdef USB_OTG_HS_CORE #define RX_FIFO_HS_SIZE 512 @@ -211,20 +211,20 @@ #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined (__GNUC__) /* GNU Compiler */ #define __ALIGN_END __attribute__ ((aligned (4))) - #define __ALIGN_BEGIN + #define __ALIGN_BEGIN #else #define __ALIGN_END #if defined (__CC_ARM) /* ARM Compiler */ - #define __ALIGN_BEGIN __align(4) - #elif defined (__ICCARM__) /* IAR Compiler */ - #define __ALIGN_BEGIN - #elif defined (__TASKING__) /* TASKING Compiler */ #define __ALIGN_BEGIN __align(4) - #endif /* __CC_ARM */ - #endif /* __GNUC__ */ + #elif defined (__ICCARM__) /* IAR Compiler */ + #define __ALIGN_BEGIN + #elif defined (__TASKING__) /* TASKING Compiler */ + #define __ALIGN_BEGIN __align(4) + #endif /* __CC_ARM */ + #endif /* __GNUC__ */ #else #define __ALIGN_BEGIN - #define __ALIGN_END + #define __ALIGN_END #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* __packed keyword used to decrease the data type alignment to 1-byte */ @@ -242,37 +242,37 @@ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Types * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Variables * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_FunctionsPrototype * @{ - */ + */ /** * @} - */ + */ #endif //__USB_CONF__H__ @@ -280,10 +280,10 @@ /** * @} - */ + */ /** * @} - */ + */ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 61e26a1131..879b2edc66 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -19,8 +19,8 @@ ****************************************************************************** */ -#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED -#pragma data_alignment = 4 +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED +#pragma data_alignment = 4 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* Includes ------------------------------------------------------------------*/ @@ -34,7 +34,7 @@ LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ -/* These are external variables imported from CDC core to be used for IN +/* These are external variables imported from CDC core to be used for IN transfer management. */ extern uint8_t APP_Rx_Buffer[]; /* Write CDC received data in this buffer. These data will be sent over USB IN endpoint @@ -105,7 +105,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) assert_param(Len>=sizeof(LINE_CODING)); switch (Cmd) { - /* Not needed for this driver, AT modem commands */ + /* Not needed for this driver, AT modem commands */ case SEND_ENCAPSULATED_COMMAND: case GET_ENCAPSULATED_RESPONSE: break; @@ -116,18 +116,18 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) case CLEAR_COMM_FEATURE: break; - + //Note - hw flow control on UART 1-3 and 6 only - case SET_LINE_CODING: + case SET_LINE_CODING: ust_cpy(&g_lc, plc); //Copy into structure to save for later break; - - + + case GET_LINE_CODING: ust_cpy(plc, &g_lc); break; - + case SET_CONTROL_LINE_STATE: /* Not needed for this driver */ //RSW - This tells how to set RTS and DTR @@ -153,7 +153,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) { - if (USB_Tx_State) + if (USB_Tx_State) return 0; VCP_DataTx(ptrBuffer, sendLength); @@ -166,7 +166,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) * this function. * @param Buf: Buffer of data to be sent * @param Len: Number of data to be sent (in bytes) - * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL + * @retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { @@ -177,7 +177,7 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) return USBD_OK; } -uint8_t usbAvailable(void) +uint8_t usbAvailable(void) { return (usbData.bufferInPosition != usbData.bufferOutPosition); } @@ -191,18 +191,17 @@ uint8_t usbAvailable(void) *******************************************************************************/ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { - uint32_t ch = 0; + uint32_t count = 0; - while (usbAvailable() && ch < len) { - recvBuf[ch] = usbData.buffer[usbData.bufferOutPosition]; + while (usbAvailable() && count < len) { + recvBuf[count] = usbData.buffer[usbData.bufferOutPosition]; usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE; - ch++; + count++; receiveLength--; } - return ch; + return count; } - /** * @brief VCP_DataRx * Data received over USB OUT endpoint are sent over CDC interface diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index df1e29001c..553017ff28 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -59,7 +59,7 @@ typedef enum _DEVICE_STATE { } DEVICE_STATE; /* Exported typef ------------------------------------------------------------*/ -/* The following structures groups all needed parameters to be configured for the +/* The following structures groups all needed parameters to be configured for the ComPort. These parameters can modified on the fly by the host through CDC class command class requests. */ typedef struct diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index a32176efe8..02ceeae545 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -60,36 +60,36 @@ #define APP_FOPS VCP_fops /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Types * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Variables * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_FunctionsPrototype * @{ - */ + */ /** * @} - */ + */ #endif //__USBD_CONF__H__ diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index ab1a3e1d35..a0cf881916 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Includes ------------------------------------------------------------------*/ #include "usbd_core.h" @@ -32,22 +32,22 @@ */ -/** @defgroup USBD_DESC +/** @defgroup USBD_DESC * @brief USBD descriptors module * @{ - */ + */ /** @defgroup USBD_DESC_Private_TypesDefinitions * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Defines * @{ - */ + */ #define USBD_VID 0x0483 @@ -55,7 +55,7 @@ /** @defgroup USB_String_Descriptors * @{ - */ + */ #define USBD_LANGID_STRING 0x409 #define USBD_MANUFACTURER_STRING "BetaFlight" @@ -83,36 +83,36 @@ #define USBD_INTERFACE_FS_STRING "VCP Interface" /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Variables * @{ - */ + */ USBD_DEVICE USR_desc = { USBD_USR_DeviceDescriptor, - USBD_USR_LangIDStrDescriptor, + USBD_USR_LangIDStrDescriptor, USBD_USR_ManufacturerStrDescriptor, USBD_USR_ProductStrDescriptor, USBD_USR_SerialStrDescriptor, USBD_USR_ConfigStrDescriptor, USBD_USR_InterfaceStrDescriptor, - + }; #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ @@ -140,7 +140,7 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ @@ -160,36 +160,36 @@ __ALIGN_BEGIN uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALI #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = { - USB_SIZ_STRING_LANGID, - USB_DESC_TYPE_STRING, + USB_SIZ_STRING_LANGID, + USB_DESC_TYPE_STRING, LOBYTE(USBD_LANGID_STRING), - HIBYTE(USBD_LANGID_STRING), + HIBYTE(USBD_LANGID_STRING), }; /** * @} - */ + */ /** @defgroup USBD_DESC_Private_FunctionPrototypes * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Functions * @{ - */ + */ /** -* @brief USBD_USR_DeviceDescriptor +* @brief USBD_USR_DeviceDescriptor * return the device descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -203,7 +203,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_LangIDStrDescriptor +* @brief USBD_USR_LangIDStrDescriptor * return the LangID string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -212,13 +212,13 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) { (void)speed; - *length = sizeof(USBD_LangIDDesc); + *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; } /** -* @brief USBD_USR_ProductStrDescriptor +* @brief USBD_USR_ProductStrDescriptor * return the product string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -226,8 +226,8 @@ uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) { + - if(speed == 0) USBD_GetString ((uint8_t*)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); else @@ -237,7 +237,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_ManufacturerStrDescriptor +* @brief USBD_USR_ManufacturerStrDescriptor * return the manufacturer string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -251,7 +251,7 @@ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_SerialStrDescriptor +* @brief USBD_USR_SerialStrDescriptor * return the serial number string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -268,7 +268,7 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_ConfigStrDescriptor +* @brief USBD_USR_ConfigStrDescriptor * return the configuration string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -281,12 +281,12 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) else USBD_GetString ((uint8_t*)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length); - return USBD_StrDesc; + return USBD_StrDesc; } /** -* @brief USBD_USR_InterfaceStrDescriptor +* @brief USBD_USR_InterfaceStrDescriptor * return the interface string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -299,22 +299,22 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) else USBD_GetString ((uint8_t*)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length); - return USBD_StrDesc; + return USBD_StrDesc; } /** * @} - */ + */ /** * @} - */ + */ /** * @} - */ + */ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_desc.h b/src/main/vcpf4/usbd_desc.h index ed999dc62a..f67799cf42 100644 --- a/src/main/vcpf4/usbd_desc.h +++ b/src/main/vcpf4/usbd_desc.h @@ -6,18 +6,18 @@ * @date 19-September-2011 * @brief header file for the usbd_desc.c file ****************************************************************************** - * @attention + * @attention * * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -30,11 +30,11 @@ /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY * @{ */ - + /** @defgroup USB_DESC * @brief general defines for the usb device library file * @{ - */ + */ /** @defgroup USB_DESC_Exported_Defines * @{ @@ -49,7 +49,7 @@ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_TypesDefinitions @@ -57,33 +57,33 @@ */ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_Variables * @{ - */ + */ extern uint8_t USBD_DeviceDesc [USB_SIZ_DEVICE_DESC]; extern uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ]; -extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; +extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]; extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID]; -extern USBD_DEVICE USR_desc; +extern USBD_DEVICE USR_desc; /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_FunctionsPrototype * @{ - */ + */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length); @@ -95,20 +95,20 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length); uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); #ifdef USB_SUPPORT_USER_STRING_DESC -uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); -#endif /* USB_SUPPORT_USER_STRING_DESC */ - +uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); +#endif /* USB_SUPPORT_USER_STRING_DESC */ + /** * @} - */ + */ #endif /* __USBD_DESC_H */ /** * @} - */ + */ /** * @} -*/ +*/ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_usr.c b/src/main/vcpf4/usbd_usr.c index 958e772322..b3de6d44a6 100644 --- a/src/main/vcpf4/usbd_usr.c +++ b/src/main/vcpf4/usbd_usr.c @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ #include "usbd_usr.h" #include "usbd_ioreq.h" @@ -29,25 +29,25 @@ USBD_Usr_cb_TypeDef USR_cb = USBD_USR_DeviceConfigured, USBD_USR_DeviceSuspended, USBD_USR_DeviceResumed, - + USBD_USR_DeviceConnected, USBD_USR_DeviceDisconnected, }; /** -* @brief USBD_USR_Init +* @brief USBD_USR_Init * Displays the message on LCD for host lib initialization * @param None * @retval None */ void USBD_USR_Init(void) -{ +{ } /** -* @brief USBD_USR_DeviceReset +* @brief USBD_USR_DeviceReset * Displays the message on LCD on device Reset Event * @param speed : device speed * @retval None @@ -56,14 +56,14 @@ void USBD_USR_DeviceReset(uint8_t speed ) { switch (speed) { - case USB_OTG_SPEED_HIGH: + case USB_OTG_SPEED_HIGH: break; - case USB_OTG_SPEED_FULL: + case USB_OTG_SPEED_FULL: break; default: break; - + } } @@ -101,7 +101,7 @@ void USBD_USR_DeviceDisconnected (void) } /** -* @brief USBD_USR_DeviceSuspended +* @brief USBD_USR_DeviceSuspended * Displays the message on LCD on device suspend Event * @param None * @retval None @@ -113,7 +113,7 @@ void USBD_USR_DeviceSuspended(void) /** -* @brief USBD_USR_DeviceResumed +* @brief USBD_USR_DeviceResumed * Displays the message on LCD on device resume Event * @param None * @retval None