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:
commit
8816678333
48 changed files with 205 additions and 371 deletions
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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];
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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 */
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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"
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue