1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00

Rebased on to master

This commit is contained in:
jflyper 2016-10-29 02:01:48 +09:00
commit 8816678333
48 changed files with 205 additions and 371 deletions

View file

@ -36,6 +36,7 @@
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/pwm_output.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
@ -895,7 +896,17 @@ void stopInTestMode(void)
*/ */
bool inMotorTestMode(void) { bool inMotorTestMode(void) {
static uint32_t resetTime = 0; static uint32_t resetTime = 0;
uint16_t inactiveMotorCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.motorConfig.mincommand); uint16_t inactiveMotorCommand;
if (feature(FEATURE_3D)) {
inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d;
#ifdef USE_DSHOT
} else if (isMotorProtocolDshot()) {
inactiveMotorCommand = DSHOT_DISARM_COMMAND;
#endif
} else {
inactiveMotorCommand = masterConfig.motorConfig.mincommand;
}
int i; int i;
bool atLeastOneMotorActivated = false; bool atLeastOneMotorActivated = false;

View file

@ -20,8 +20,4 @@
#include "platform.h" #include "platform.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "flight/mixer.h"
#include "build_config.h" #include "build_config.h"

View file

@ -76,7 +76,7 @@
#if defined(STM32F40_41xxx) #if defined(STM32F40_41xxx)
#define FLASH_PAGE_COUNT 4 // just to make calculations work #define FLASH_PAGE_COUNT 4 // just to make calculations work
#elif defined (STM32F411xE) #elif defined (STM32F411xE)
#define FLASH_PAGE_COUNT 4 // just to make calculations work #define FLASH_PAGE_COUNT 3 // just to make calculations work
#elif defined (STM32F745xx) #elif defined (STM32F745xx)
#define FLASH_PAGE_COUNT 4 // just to make calculations work #define FLASH_PAGE_COUNT 4 // just to make calculations work
#else #else

View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#include "sensor.h"
typedef struct mag_s { typedef struct mag_s {
sensorInitFuncPtr init; // initialize function sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function

View file

@ -158,7 +158,9 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
#ifdef USE_UART1_RX_DMA #ifdef USE_UART1_RX_DMA
s->rxDMAChannel = DMA1_Channel5; s->rxDMAChannel = DMA1_Channel5;
#endif #endif
#ifdef USE_UART1_TX_DMA
s->txDMAChannel = DMA1_Channel4; s->txDMAChannel = DMA1_Channel4;
#endif
s->USARTx = USART1; s->USARTx = USART1;

View file

@ -382,11 +382,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
for (index = 0; index < SERIAL_PORT_COUNT; index++) { for (index = 0; index < SERIAL_PORT_COUNT; index++) {
serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index]; serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
#ifdef USE_VCP
serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_500000;
#else
serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200; serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
#endif
serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600; serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO; serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO;
serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200; serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200;

View file

@ -35,18 +35,13 @@
#include "common/streambuf.h" #include "common/streambuf.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/flash.h" #include "drivers/flash.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/sdcard.h" #include "drivers/sdcard.h"
#include "drivers/buf_writer.h"
#include "drivers/max7456.h" #include "drivers/max7456.h"
#include "drivers/vtx_soft_spi_rtc6705.h" #include "drivers/vtx_soft_spi_rtc6705.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
@ -651,10 +646,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
continue; continue;
} }
if (isMotorProtocolDshot())
sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator sbufWriteU16(dst, convertMotorToExternal(motor[i]));
else
sbufWriteU16(dst, motor[i]);
} }
break; break;
case MSP_RC: case MSP_RC:
@ -1323,8 +1316,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break; break;
case MSP_SET_MOTOR: case MSP_SET_MOTOR:
for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 for (i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
motor_disarmed[i] = sbufReadU16(src); motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
}
break; break;
case MSP_SET_SERVO_CONFIGURATION: case MSP_SET_SERVO_CONFIGURATION:
#ifdef USE_SERVOS #ifdef USE_SERVOS

View file

@ -29,13 +29,8 @@
#include "common/utils.h" #include "common/utils.h"
#include "common/filter.h" #include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/timer.h"
#include "drivers/gyro_sync.h" #include "drivers/gyro_sync.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"

View file

@ -23,6 +23,8 @@
#include "platform.h" #include "platform.h"
#include "blackbox/blackbox.h"
#include "build/build_config.h" #include "build/build_config.h"
#include "common/axis.h" #include "common/axis.h"
@ -30,14 +32,21 @@
#include "config/feature.h" #include "config/feature.h"
#include "drivers/system.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/mw.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "drivers/system.h" #include "io/cms.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "io/gps.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/vtx.h"
#include "io/display.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "sensors/battery.h" #include "sensors/battery.h"
@ -47,22 +56,10 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "io/cms.h"
#include "io/gps.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/vtx.h"
#include "io/display.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "blackbox/blackbox.h"
#include "fc/mw.h"
static motorConfig_t *motorConfig; static motorConfig_t *motorConfig;
static pidProfile_t *pidProfile; static pidProfile_t *pidProfile;

View file

@ -28,12 +28,6 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/sonar_hcsr04.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "sensors/sonar.h" #include "sensors/sonar.h"
@ -42,7 +36,6 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "io/motors.h" #include "io/motors.h"
#include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"

View file

@ -31,9 +31,6 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"

View file

@ -47,6 +47,13 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "config/feature.h" #include "config/feature.h"
#include "config/config_master.h"
#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
#define EXTERNAL_CONVERSION_MIN_VALUE 1000
#define EXTERNAL_CONVERSION_MAX_VALUE 2000
uint8_t motorCount; uint8_t motorCount;
@ -234,21 +241,26 @@ static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, d
static float rcCommandThrottleRange; static float rcCommandThrottleRange;
bool isMotorProtocolDshot(void) { bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600) if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)
return true; return true;
else else
#endif
return false; return false;
} }
// Add here scaled ESC outputs for digital protol // Add here scaled ESC outputs for digital protol
void initEscEndpoints(void) { void initEscEndpoints(void) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND; disarmMotorOutput = DSHOT_DISARM_COMMAND;
minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset; minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset;
maxMotorOutputNormal = DSHOT_MAX_THROTTLE; maxMotorOutputNormal = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH; deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes
} else { } else
#endif
{
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
minMotorOutputNormal = motorConfig->minthrottle; minMotorOutputNormal = motorConfig->minthrottle;
maxMotorOutputNormal = motorConfig->maxthrottle; maxMotorOutputNormal = motorConfig->maxthrottle;
@ -359,7 +371,7 @@ void mixerResetDisarmedMotors(void)
int i; int i;
// set disarmed motor values // set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand; motor_disarmed[i] = disarmMotorOutput;
} }
void writeMotors(void) void writeMotors(void)
@ -373,7 +385,7 @@ void writeMotors(void)
} }
} }
void writeAllMotors(int16_t mc) static void writeAllMotors(int16_t mc)
{ {
// Sends commands to all motors // Sends commands to all motors
for (uint8_t i = 0; i < motorCount; i++) { for (uint8_t i = 0; i < motorCount; i++) {
@ -385,7 +397,7 @@ void writeAllMotors(int16_t mc)
void stopMotors(void) void stopMotors(void)
{ {
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand); writeAllMotors(disarmMotorOutput);
delay(50); // give the timers and ESCs a chance to react. delay(50); // give the timers and ESCs a chance to react.
} }
@ -511,14 +523,31 @@ void mixTable(pidProfile_t *pidProfile)
// Disarmed mode // Disarmed mode
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
if (isMotorProtocolDshot()) { motor[i] = motor_disarmed[i];
motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range
if (motor_disarmed[i] != disarmMotorOutput)
motor[i] = (motor_disarmed[i] - 1000) * 2; // TODO - Perhaps needs rescaling as it will never reach 2047 during motor tests
} else {
motor[i] = motor_disarmed[i];
}
} }
} }
} }
uint16_t convertExternalToMotor(uint16_t externalValue)
{
uint16_t motorValue = externalValue;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
#endif
return motorValue;
}
uint16_t convertMotorToExternal(uint16_t motorValue)
{
uint16_t externalValue = motorValue;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE);
}
#endif
return externalValue;
}

View file

@ -21,12 +21,30 @@
#define QUAD_MOTOR_COUNT 4 #define QUAD_MOTOR_COUNT 4
/*
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
0 = stop
1-5: beep
6: ESC info request (FW Version and SN sent over the tlm wire)
7: spin direction 1
8: spin direction 2
9: 3d mode off
10: 3d mode on
11: ESC settings request (saved settings over the TLM wire)
12: save Settings
3D Mode:
0 = stop
48 (low) - 1047 (high) -> positive direction
1048 (low) - 2047 (high) -> negative direction
*/
// Digital protocol has fixed values // Digital protocol has fixed values
#define DSHOT_DISARM_COMMAND 0 #define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48 #define DSHOT_MIN_THROTTLE 48
#define DSHOT_MAX_THROTTLE 2047 #define DSHOT_MAX_THROTTLE 2047
#define DSHOT_3D_DEADBAND_LOW 900 // TODO - not agreed yet #define DSHOT_3D_MAX_POSITIVE 1047 // TODO - Not working yet!! Mixer requires some throttle rescaling changes
#define DSHOT_3D_DEADBAND_HIGH 1100 // TODO - not agreed yet #define DSHOT_3D_MIN_NEGATIVE 1048// TODO - Not working yet!! Mixer requires some throttle rescaling changes
// Note: this is called MultiType/MULTITYPE_* in baseflight. // Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode typedef enum mixerMode
@ -104,7 +122,6 @@ void mixerUseConfigs(
airplaneConfig_t *airplaneConfigToUse, airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse); struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
@ -117,4 +134,7 @@ void syncMotors(bool enabled);
void writeMotors(void); void writeMotors(void);
void stopMotors(void); void stopMotors(void);
void stopPwmAllMotors(void); void stopPwmAllMotors(void);
bool isMotorProtocolDshot(void); bool isMotorProtocolDshot(void);
uint16_t convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(uint16_t motorValue);

View file

@ -30,12 +30,6 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"

View file

@ -28,24 +28,20 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/filter.h" #include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "io/gps.h" #include "fc/runtime_config.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/gtune.h" #include "flight/gtune.h"
#include "fc/runtime_config.h" #include "io/gps.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
extern float rcInput[3]; extern float rcInput[3];
extern float setpointRate[3]; extern float setpointRate[3];

View file

@ -23,14 +23,11 @@
#include "common/utils.h" #include "common/utils.h"
#include "drivers/gpio.h"
#include "drivers/sound_beeper.h" #include "drivers/sound_beeper.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "fc/rc_controls.h"
#include "io/statusindicator.h" #include "io/statusindicator.h"
#include "io/vtx.h" #include "io/vtx.h"

View file

@ -28,12 +28,8 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "drivers/serial.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/display_ug2864hsweg01.h" #include "drivers/display_ug2864hsweg01.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/maths.h" #include "common/maths.h"

View file

@ -34,9 +34,6 @@
#include "common/utils.h" #include "common/utils.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"

View file

@ -48,7 +48,6 @@ uint8_t cliMode = 0;
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/bus_i2c.h" #include "drivers/bus_i2c.h"
#include "drivers/flash.h" #include "drivers/flash.h"
#include "drivers/gpio.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/io_impl.h" #include "drivers/io_impl.h"
#include "drivers/timer.h" #include "drivers/timer.h"
@ -3097,11 +3096,11 @@ static void cliMotor(char *cmdline)
cliShowArgumentRangeError("value", 1000, 2000); cliShowArgumentRangeError("value", 1000, 2000);
return; return;
} else { } else {
motor_disarmed[motor_index] = motor_value; motor_disarmed[motor_index] = convertExternalToMotor(motor_value);
} }
} }
cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
} }
static void cliPlaySound(char *cmdline) static void cliPlaySound(char *cmdline)

View file

@ -21,9 +21,7 @@
#include "platform.h" #include "platform.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "statusindicator.h" #include "statusindicator.h"

View file

@ -24,8 +24,6 @@
#include "common/streambuf.h" #include "common/streambuf.h"
#include "common/utils.h" #include "common/utils.h"
#include "drivers/serial.h"
#include "io/serial.h" #include "io/serial.h"
#include "msp/msp.h" #include "msp/msp.h"

View file

@ -24,10 +24,6 @@
#include "common/utils.h" #include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/msp.h" #include "rx/msp.h"

View file

@ -27,7 +27,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h" #include "drivers/pwm_rx.h"
#include "fc/config.h" #include "fc/config.h"

View file

@ -30,10 +30,7 @@
#include "config/feature.h" #include "config/feature.h"
#include "drivers/serial.h"
#include "drivers/adc.h" #include "drivers/adc.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h" #include "drivers/pwm_rx.h"
#include "drivers/rx_spi.h" #include "drivers/rx_spi.h"
#include "drivers/system.h" #include "drivers/system.h"

View file

@ -27,11 +27,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/inverter.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h" #include "io/serial.h"
#ifdef TELEMETRY #ifdef TELEMETRY

View file

@ -29,8 +29,6 @@
#include "drivers/io_impl.h" #include "drivers/io_impl.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h" #include "io/serial.h"

View file

@ -26,8 +26,6 @@
#include "common/utils.h" #include "common/utils.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h" #include "io/serial.h"

View file

@ -33,8 +33,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h" #include "io/serial.h"
#ifdef TELEMETRY #ifdef TELEMETRY

View file

@ -25,8 +25,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h" #include "io/serial.h"
#ifdef TELEMETRY #ifdef TELEMETRY

View file

@ -26,8 +26,6 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "fc/config.h" #include "fc/config.h"

View file

@ -22,9 +22,6 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
#include "drivers/compass_hmc5883l.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
#include "fc/config.h" #include "fc/config.h"

View file

@ -17,6 +17,9 @@
#pragma once #pragma once
#include "drivers/compass.h"
// Type of magnetometer used/detected // Type of magnetometer used/detected
typedef enum { typedef enum {
MAG_DEFAULT = 0, MAG_DEFAULT = 0,

View file

@ -27,9 +27,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/filter.h" #include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/accgyro.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/statusindicator.h" #include "io/statusindicator.h"

View file

@ -28,10 +28,6 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "fc/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "config/feature.h" #include "config/feature.h"

View file

@ -14,26 +14,27 @@
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
//Target code By Hector "Hectech FPV" Hind //Target code By BorisB and Hector "Hectech FPV" Hind
#include <stdint.h> #include <stdint.h>
#include <platform.h> #include <platform.h>
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/dma.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel1, DMA1_CH1_HANDLER }, // PWM3 - PB8
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, DMA1_Channel4, DMA1_CH4_HANDLER }, // PWM4 - PB9
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // PWM6 - PA0
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP
}; };

View file

@ -14,28 +14,22 @@
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
//Target code By Hector "Hectech FPV" Hind //Target code By BorisB and Hector "Hectech FPV" Hind
#pragma once #pragma once
#define TARGET_BOARD_IDENTIFIER "BFFC" #define TARGET_BOARD_IDENTIFIER "BFF3"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
//#define LED0 PC14
#define BEEPER PC15 #define BEEPER PC15
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 17 #define USABLE_TIMER_CHANNEL_COUNT 10
#define USE_MAG_DATA_READY_SIGNAL #define MPU6000_CS_PIN PA15
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define MPU6000_CS_PIN PC13
#define MPU6000_SPI_INSTANCE SPI1 #define MPU6000_SPI_INSTANCE SPI1
#define GYRO #define GYRO
#define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6000
#define GYRO_MPU6000_ALIGN CW180_DEG #define GYRO_MPU6000_ALIGN CW180_DEG
@ -46,14 +40,18 @@
// MPU6000 interrupts // MPU6000 interrupts
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) #define EXTI_CALLBACK_HANDLER_COUNT 1
#define MPU_INT_EXTI PC13 #define MPU_INT_EXTI PC13
#define USE_EXTI #define USE_EXTI
#define USB_IO #define USE_DSHOT
//#define USE_FLASHFS // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4
//#define USE_FLASH_M25P16 #if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT)
#undef USE_UART1_TX_DMA
#endif
#define USB_IO
#define USE_VCP #define USE_VCP
#define USE_UART1 #define USE_UART1
@ -109,7 +107,6 @@
#define SDCARD_DETECT_PIN PC14 #define SDCARD_DETECT_PIN PC14
#define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_INSTANCE SPI2
//#define SDCARD_SPI_CS_GPIO SPI2_GPIO
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN #define SDCARD_SPI_CS_PIN SPI2_NSS_PIN
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128
@ -117,9 +114,6 @@
#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 #define SDCARD_DMA_CHANNEL_TX DMA1_Channel5
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1
//#define SDCARD_DMA_CHANNEL DMA_Channel_0
#define BOARD_HAS_VOLTAGE_DIVIDER #define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_ADC #define USE_ADC

View file

@ -1141,7 +1141,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
break; break;
case BST_SET_MOTOR: case BST_SET_MOTOR:
for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
motor_disarmed[i] = bstRead16(); motor_disarmed[i] = convertExternalToMotor(bstRead16());
break; break;
case BST_SET_SERVO_CONFIGURATION: case BST_SET_SERVO_CONFIGURATION:
#ifdef USE_SERVOS #ifdef USE_SERVOS

View file

@ -46,6 +46,7 @@ void targetConfiguration(master_t *config)
config->gyro_lpf = 1; config->gyro_lpf = 1;
config->gyro_soft_lpf_hz = 100; config->gyro_soft_lpf_hz = 100;
config->gyro_soft_notch_hz_1 = 0; config->gyro_soft_notch_hz_1 = 0;
config->gyro_soft_notch_hz_2 = 0;
/*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
config->rxConfig.channelRanges[channel].min = 1180; config->rxConfig.channelRanges[channel].min = 1180;
@ -53,12 +54,12 @@ void targetConfiguration(master_t *config)
}*/ }*/
for (int profileId = 0; profileId < 2; profileId++) { for (int profileId = 0; profileId < 2; profileId++) {
config->profile[profileId].pidProfile.P8[ROLL] = 55; config->profile[profileId].pidProfile.P8[ROLL] = 70;
config->profile[profileId].pidProfile.I8[ROLL] = 50; config->profile[profileId].pidProfile.I8[ROLL] = 70;
config->profile[profileId].pidProfile.D8[ROLL] = 25; config->profile[profileId].pidProfile.D8[ROLL] = 30;
config->profile[profileId].pidProfile.P8[PITCH] = 65; config->profile[profileId].pidProfile.P8[PITCH] = 80;
config->profile[profileId].pidProfile.I8[PITCH] = 60; config->profile[profileId].pidProfile.I8[PITCH] = 80;
config->profile[profileId].pidProfile.D8[PITCH] = 28; config->profile[profileId].pidProfile.D8[PITCH] = 30;
config->profile[profileId].pidProfile.P8[YAW] = 180; config->profile[profileId].pidProfile.P8[YAW] = 180;
config->profile[profileId].pidProfile.I8[YAW] = 45; config->profile[profileId].pidProfile.I8[YAW] = 45;
config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50; config->profile[profileId].pidProfile.P8[PIDLEVEL] = 50;
@ -66,12 +67,14 @@ void targetConfiguration(master_t *config)
config->profile[profileId].pidProfile.levelSensitivity = 1.0f; config->profile[profileId].pidProfile.levelSensitivity = 1.0f;
for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) {
config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 110; config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100;
config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110;
config->profile[profileId].controlRateProfile[rateProfileId].rcExpo8 = 20;
config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 80;
config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 80;
config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80;
config->profile[profileId].pidProfile.dtermSetpointWeight = 254;
config->profile[profileId].pidProfile.setpointRelaxRatio = 100; config->profile[profileId].pidProfile.setpointRelaxRatio = 100;
} }
} }

View file

@ -66,9 +66,9 @@
#define MAX7456_SPI_INSTANCE SPI3 #define MAX7456_SPI_INSTANCE SPI3
#define MAX7456_SPI_CS_PIN PA15 #define MAX7456_SPI_CS_PIN PA15
#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 //#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5
#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 //#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0
#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER
//#define PITOT //#define PITOT
//#define USE_PITOT_MS4525 //#define USE_PITOT_MS4525

View file

@ -21,27 +21,28 @@
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/timer.h" #include "drivers/timer.h"
#include "drivers/dma.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH1 - PA0 - *TIM2_CH1
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N
// Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype.
{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7)
{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, NULL, 0 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7)
{ TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH5 - PB4 - *TIM3_CH1
{ TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH6 - PB5 - *TIM3_CH2
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, DMA1_Channel7, DMA1_CH7_HANDLER }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
{ TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel1, DMA1_CH1_HANDLER },// PWM3 - PA11
{ TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, DMA1_Channel4, DMA1_CH4_HANDLER },// PWM4 - PA12
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM5 - PB8
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, NULL, 0 }, // PWM6 - PB9
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM7 - PA2
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, NULL, 0 }, // PWM8 - PA3
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, NULL, 0 }, // GPIO_TIMER / LED_STRIP
}; };

View file

@ -102,6 +102,13 @@
#define CURRENT_METER_ADC_PIN PA5 #define CURRENT_METER_ADC_PIN PA5
#define RSSI_ADC_PIN PB2 #define RSSI_ADC_PIN PB2
#define USE_DSHOT
// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4
#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT)
#undef USE_UART1_TX_DMA
#endif
#define LED_STRIP #define LED_STRIP
#define USE_LED_STRIP_ON_DMA1_CHANNEL2 #define USE_LED_STRIP_ON_DMA1_CHANNEL2

View file

@ -40,6 +40,7 @@
#ifdef STM32F1 #ifdef STM32F1
// Using RX DMA disables the use of receive callbacks // Using RX DMA disables the use of receive callbacks
#define USE_UART1_RX_DMA #define USE_UART1_RX_DMA
#define USE_UART1_TX_DMA
#endif #endif

View file

@ -346,21 +346,6 @@ uint32_t hse_value = HSE_VALUE;
/* #define DATA_IN_ExtSDRAM */ /* #define DATA_IN_ExtSDRAM */
#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
#if defined(STM32F410xx) || defined(STM32F411xE)
/*!< Uncomment the following line if you need to clock the STM32F410xx/STM32F411xE by HSE Bypass
through STLINK MCO pin of STM32F103 microcontroller. The frequency cannot be changed
and is fixed at 8 MHz.
Hardware configuration needed for Nucleo Board:
SB54, SB55 OFF
R35 removed
SB16, SB50 ON */
/* #define USE_HSE_BYPASS */
#if defined(USE_HSE_BYPASS)
#define HSE_BYPASS_INPUT_FREQUENCY 8000000
#endif /* USE_HSE_BYPASS */
#endif /* STM32F410xx || STM32F411xE */
/*!< Uncomment the following line if you need to relocate your vector Table in /*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */ Internal SRAM. */
/* #define VECT_TAB_SRAM */ /* #define VECT_TAB_SRAM */
@ -379,17 +364,10 @@ uint32_t hse_value = HSE_VALUE;
#elif defined (STM32F446xx) #elif defined (STM32F446xx)
#define PLL_M 8 #define PLL_M 8
#elif defined (STM32F410xx) || defined (STM32F411xE) #elif defined (STM32F410xx) || defined (STM32F411xE)
#if defined(USE_HSE_BYPASS) #define PLL_M 8
#define PLL_M 8
#else /* !USE_HSE_BYPASS */
#define PLL_M 8
#endif /* USE_HSE_BYPASS */
#else #else
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ #endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
#if defined(STM32F446xx) #if defined(STM32F446xx)
/* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */ /* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */
#define PLL_R 7 #define PLL_R 7
@ -399,24 +377,32 @@ uint32_t hse_value = HSE_VALUE;
#define PLL_N 360 #define PLL_N 360
/* SYSCLK = PLL_VCO / PLL_P */ /* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 2 #define PLL_P 2
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
#if defined (STM32F40_41xxx) #if defined (STM32F40_41xxx)
#define PLL_N 336 #define PLL_N 336
/* SYSCLK = PLL_VCO / PLL_P */ /* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 2 #define PLL_P 2
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
#endif /* STM32F40_41xxx */ #endif /* STM32F40_41xxx */
#if defined(STM32F401xx) #if defined(STM32F401xx)
#define PLL_N 336 #define PLL_N 336
/* SYSCLK = PLL_VCO / PLL_P */ /* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 4 #define PLL_P 4
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 7
#endif /* STM32F401xx */ #endif /* STM32F401xx */
#if defined(STM32F410xx) || defined(STM32F411xE) #if defined(STM32F410xx) || defined(STM32F411xE)
#define PLL_N 400 #define PLL_N 384
/* SYSCLK = PLL_VCO / PLL_P */ /* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P 4 #define PLL_P 4
/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */
#define PLL_Q 8
#endif /* STM32F410xx || STM32F411xE */ #endif /* STM32F410xx || STM32F411xE */
/******************************************************************************/ /******************************************************************************/
@ -450,7 +436,7 @@ uint32_t hse_value = HSE_VALUE;
#endif /* STM32F401xx */ #endif /* STM32F401xx */
#if defined(STM32F410xx) || defined(STM32F411xE) #if defined(STM32F410xx) || defined(STM32F411xE)
uint32_t SystemCoreClock = 100000000; uint32_t SystemCoreClock = 96000000;
#endif /* STM32F410xx || STM32F401xE */ #endif /* STM32F410xx || STM32F401xE */
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
@ -585,7 +571,6 @@ void SystemCoreClockUpdate(void)
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx) || defined(STM32F469_479xx)
if (pllsource != 0) if (pllsource != 0)
{ {
/* HSE used as PLL clock source */ /* HSE used as PLL clock source */
@ -596,21 +581,7 @@ void SystemCoreClockUpdate(void)
/* HSI used as PLL clock source */ /* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
} }
#elif defined(STM32F410xx) || defined(STM32F411xE)
#if defined(USE_HSE_BYPASS)
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_BYPASS_INPUT_FREQUENCY / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
#else
if (pllsource == 0)
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
#endif /* USE_HSE_BYPASS */
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F446xx || STM32F469_479xx */
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp; SystemCoreClock = pllvco/pllp;
break; break;
@ -657,7 +628,6 @@ void SystemCoreClockUpdate(void)
*/ */
void SetSysClock(void) void SetSysClock(void)
{ {
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx)|| defined(STM32F469_479xx)
/******************************************************************************/ /******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */ /* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/ /******************************************************************************/
@ -707,16 +677,22 @@ void SetSysClock(void)
RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; RCC->CFGR |= RCC_CFGR_PPRE1_DIV2;
#endif /* STM32F401xx */ #endif /* STM32F401xx */
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) #if defined(STM32F410xx) || defined(STM32F411xE)
/* Configure the main PLL */ /* PCLK2 = HCLK / 2*/
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | RCC->CFGR |= RCC_CFGR_PPRE2_DIV1;
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
#endif /* STM32F40_41xxx || STM32F401xx || STM32F427_437x || STM32F429_439xx || STM32F469_479xx */ /* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV2;
#endif /* STM32F410xx || STM32F411xE */
#if defined(STM32F446xx) #if defined(STM32F446xx)
/* Configure the main PLL */ /* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28);
#else
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
#endif /* STM32F446xx */ #endif /* STM32F446xx */
/* Enable the main PLL */ /* Enable the main PLL */
@ -737,19 +713,17 @@ void SetSysClock(void)
while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) while((PWR->CSR & PWR_CSR_ODSWRDY) == 0)
{ {
} }
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ #endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
#if defined(STM32F40_41xxx) #if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx)
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */ /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
#endif /* STM32F40_41xxx */ #endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
#if defined(STM32F401xx) #if defined(STM32F401xx) || defined(STM32F410xx) || defined(STM32F411xE)
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */ /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS;
#endif /* STM32F401xx */ #endif /* STM32F401xx || STM32F410xx || STM32F411xE*/
/* Select the main PLL as system clock source */ /* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
@ -764,113 +738,6 @@ void SetSysClock(void)
{ /* If HSE fails to start-up, the application will have wrong clock { /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */ configuration. User can add here some code to deal with this error */
} }
#elif defined(STM32F410xx) || defined(STM32F411xE)
#if defined(USE_HSE_BYPASS)
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/* Enable HSE and HSE BYPASS */
RCC->CR |= ((uint32_t)RCC_CR_HSEON | RCC_CR_HSEBYP);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Select regulator voltage output Scale 1 mode */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR |= PWR_CR_VOS;
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV2;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
(RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
#else /* HSI will be used as PLL clock source */
/* Select regulator voltage output Scale 1 mode */
RCC->APB1ENR |= RCC_APB1ENR_PWREN;
PWR->CR |= PWR_CR_VOS;
/* HCLK = SYSCLK / 1*/
RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 2*/
RCC->CFGR |= RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 4*/
RCC->CFGR |= RCC_CFGR_PPRE1_DIV2;
/* Configure the main PLL */
RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (PLL_Q << 24);
/* Enable the main PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till the main PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS;
/* Select the main PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= RCC_CFGR_SW_PLL;
/* Wait till the main PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
{
}
#endif /* USE_HSE_BYPASS */
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */
} }
/** /**

View file

@ -36,8 +36,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/serial.h" #include "drivers/serial.h"
#include "fc/config.h" #include "fc/config.h"

View file

@ -67,7 +67,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/serial.h"
#include "io/serial.h" #include "io/serial.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"

View file

@ -44,10 +44,6 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/pwm_rx.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"

View file

@ -35,13 +35,10 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h" #include "fc/rc_controls.h"
#include "drivers/serial.h"
#include "drivers/pwm_rx.h"
#include "io/serial.h" #include "io/serial.h"
#include "fc/rc_controls.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"

View file

@ -19,13 +19,6 @@
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/serial.h"
#include "drivers/bus_i2c.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/adc.h"
#include "drivers/light_led.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
@ -34,7 +27,6 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/motors.h" #include "io/motors.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gimbal.h"
#include "io/serial.h" #include "io/serial.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"