mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge branch 'master' of https://github.com/borisbstyle/betaflight into PikoBLX_target_bf
This commit is contained in:
commit
95ae6ba515
26 changed files with 701 additions and 57 deletions
|
@ -1230,7 +1230,8 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval);
|
||||
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
|
||||
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider);
|
||||
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.use_unsyncedPwm);
|
||||
|
|
|
@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
|
|||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
controlRateConfig_t *currentControlRateProfile;
|
||||
|
||||
static const uint8_t EEPROM_CONF_VERSION = 142;
|
||||
static const uint8_t EEPROM_CONF_VERSION = 143;
|
||||
|
||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
|
@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
|
||||
pidProfile->P8[ROLL] = 45;
|
||||
pidProfile->I8[ROLL] = 40;
|
||||
pidProfile->D8[ROLL] = 25;
|
||||
pidProfile->D8[ROLL] = 20;
|
||||
pidProfile->P8[PITCH] = 60;
|
||||
pidProfile->I8[PITCH] = 60;
|
||||
pidProfile->D8[PITCH] = 30;
|
||||
pidProfile->D8[PITCH] = 25;
|
||||
pidProfile->P8[YAW] = 80;
|
||||
pidProfile->I8[YAW] = 45;
|
||||
pidProfile->D8[YAW] = 20;
|
||||
|
@ -244,9 +244,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
|
||||
// Betaflight PID controller parameters
|
||||
pidProfile->ptermSetpointWeight = 75;
|
||||
pidProfile->dtermSetpointWeight = 200;
|
||||
pidProfile->pidMaxVelocityYaw = 200;
|
||||
pidProfile->toleranceBand = 20;
|
||||
pidProfile->dtermSetpointWeight = 120;
|
||||
pidProfile->yawRateAccelLimit = 220;
|
||||
pidProfile->rateAccelLimit = 0;
|
||||
pidProfile->toleranceBand = 15;
|
||||
pidProfile->toleranceBandReduction = 40;
|
||||
pidProfile->zeroCrossAllowanceCount = 2;
|
||||
pidProfile->itermThrottleGain = 0;
|
||||
|
@ -475,14 +476,14 @@ static void resetConf(void)
|
|||
masterConfig.gyro_sync_denom = 8;
|
||||
masterConfig.pid_process_denom = 1;
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500)
|
||||
masterConfig.gyro_sync_denom = 8;
|
||||
masterConfig.gyro_sync_denom = 1;
|
||||
masterConfig.pid_process_denom = 4;
|
||||
#else
|
||||
masterConfig.gyro_sync_denom = 4;
|
||||
masterConfig.pid_process_denom = 2;
|
||||
#endif
|
||||
masterConfig.gyro_soft_type = FILTER_PT1;
|
||||
masterConfig.gyro_soft_lpf_hz = 100;
|
||||
masterConfig.gyro_soft_lpf_hz = 90;
|
||||
masterConfig.gyro_soft_notch_hz = 0;
|
||||
masterConfig.gyro_soft_notch_cutoff = 150;
|
||||
|
||||
|
@ -534,8 +535,8 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.rssi_channel = 0;
|
||||
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
|
||||
masterConfig.rxConfig.rssi_ppm_invert = 0;
|
||||
masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO;
|
||||
masterConfig.rxConfig.rcSmoothInterval = 9;
|
||||
masterConfig.rxConfig.rcInterpolation = RC_SMOOTHING_AUTO;
|
||||
masterConfig.rxConfig.rcInterpolationInterval = 19;
|
||||
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
|
||||
masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS;
|
||||
masterConfig.rxConfig.airModeActivateThreshold = 1350;
|
||||
|
|
|
@ -51,7 +51,7 @@ typedef enum {
|
|||
DEBUG_AIRMODE,
|
||||
DEBUG_PIDLOOP,
|
||||
DEBUG_NOTCH,
|
||||
DEBUG_RC_SMOOTHING,
|
||||
DEBUG_RC_INTERPOLATION,
|
||||
DEBUG_VELOCITY,
|
||||
DEBUG_DTERM_FILTER,
|
||||
DEBUG_COUNT
|
||||
|
|
|
@ -43,6 +43,7 @@
|
|||
#define JEDEC_ID_MICRON_M25P16 0x202015
|
||||
#define JEDEC_ID_MICRON_N25Q064 0x20BA17
|
||||
#define JEDEC_ID_WINBOND_W25Q64 0xEF4017
|
||||
#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016
|
||||
#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017
|
||||
#define JEDEC_ID_MICRON_N25Q128 0x20ba18
|
||||
#define JEDEC_ID_WINBOND_W25Q128 0xEF4018
|
||||
|
@ -161,6 +162,10 @@ static bool m25p16_readIdentification()
|
|||
geometry.sectors = 32;
|
||||
geometry.pagesPerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_MACRONIX_MX25L3206E:
|
||||
geometry.sectors = 64;
|
||||
geometry.pagesPerSector = 256;
|
||||
break;
|
||||
case JEDEC_ID_MICRON_N25Q064:
|
||||
case JEDEC_ID_WINBOND_W25Q64:
|
||||
case JEDEC_ID_MACRONIX_MX25L6406E:
|
||||
|
|
|
@ -133,7 +133,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
|
|||
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
|
||||
float ITerm,PTerm,DTerm;
|
||||
static float lastRateError[2];
|
||||
static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate;
|
||||
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
|
||||
float delta;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
|
@ -187,20 +187,22 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
|
|||
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
|
||||
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
|
||||
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
|
||||
yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
|
||||
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
|
||||
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
|
||||
|
||||
configP[axis] = pidProfile->P8[axis];
|
||||
configI[axis] = pidProfile->I8[axis];
|
||||
configD[axis] = pidProfile->D8[axis];
|
||||
}
|
||||
|
||||
// Limit abrupt yaw inputs
|
||||
if (axis == YAW && pidProfile->pidMaxVelocityYaw) {
|
||||
float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate;
|
||||
if (ABS(yawCurrentVelocity) > yawMaxVelocity) {
|
||||
setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity;
|
||||
// Limit abrupt yaw inputs / stops
|
||||
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
|
||||
if (maxVelocity) {
|
||||
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
|
||||
if (ABS(currentVelocity) > maxVelocity) {
|
||||
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
|
||||
}
|
||||
yawPreviousRate = setpointRate[axis];
|
||||
previousSetpoint[axis] = setpointRate[axis];
|
||||
}
|
||||
|
||||
// Yaw control is GYRO based, direct sticks control is applied to rate PID
|
||||
|
|
|
@ -100,7 +100,8 @@ typedef struct pidProfile_s {
|
|||
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
|
||||
uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking)
|
||||
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
|
||||
uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms
|
||||
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
|
||||
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
|
||||
|
||||
#ifdef GTUNE
|
||||
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
|
||||
|
|
|
@ -467,7 +467,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
|
|||
"AIRMODE",
|
||||
"PIDLOOP",
|
||||
"NOTCH",
|
||||
"RC_SMOOTHING",
|
||||
"RC_INTERPOLATION",
|
||||
"VELOCITY",
|
||||
"DFILTER",
|
||||
};
|
||||
|
@ -492,8 +492,8 @@ static const char * const lookupTableDeltaMethod[] = {
|
|||
"ERROR", "MEASUREMENT"
|
||||
};
|
||||
|
||||
static const char * const lookupTableRcSmoothing[] = {
|
||||
"OFF", "DEFAULT", "AUTO", "MANUAL"
|
||||
static const char * const lookupTableRcInterpolation[] = {
|
||||
"OFF", "PRESET", "AUTO", "MANUAL"
|
||||
};
|
||||
|
||||
static const char * const lookupTableLowpassType[] = {
|
||||
|
@ -536,7 +536,7 @@ typedef enum {
|
|||
TABLE_SUPEREXPO_YAW,
|
||||
TABLE_MOTOR_PWM_PROTOCOL,
|
||||
TABLE_DELTA_METHOD,
|
||||
TABLE_RC_SMOOTHING,
|
||||
TABLE_RC_INTERPOLATION,
|
||||
TABLE_LOWPASS_TYPE,
|
||||
#ifdef OSD
|
||||
TABLE_OSD,
|
||||
|
@ -574,7 +574,7 @@ static const lookupTableEntry_t lookupTables[] = {
|
|||
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
|
||||
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
|
||||
{ lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
|
||||
{ lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
|
||||
{ lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) },
|
||||
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
|
||||
#ifdef OSD
|
||||
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
|
||||
|
@ -635,8 +635,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
|
||||
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
|
||||
{ "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_RC_SMOOTHING } },
|
||||
{ "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 1, 50 } },
|
||||
{ "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } },
|
||||
{ "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcInterpolationInterval, .config.minmax = { 1, 50 } },
|
||||
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
|
||||
|
@ -773,8 +773,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
||||
#endif
|
||||
|
||||
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } },
|
||||
{ "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } },
|
||||
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } },
|
||||
{ "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } },
|
||||
{ "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } },
|
||||
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } },
|
||||
{ "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
|
||||
|
@ -831,8 +831,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
|
||||
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
|
||||
{ "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } },
|
||||
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } },
|
||||
{ "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } },
|
||||
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } },
|
||||
{ "yaw_rate_acceleration_limit",VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
|
||||
{ "rate_acceleration_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } },
|
||||
|
||||
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
|
|
|
@ -1041,8 +1041,8 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(masterConfig.rxConfig.spektrum_sat_bind);
|
||||
serialize16(masterConfig.rxConfig.rx_min_usec);
|
||||
serialize16(masterConfig.rxConfig.rx_max_usec);
|
||||
serialize8(masterConfig.rxConfig.rcSmoothing);
|
||||
serialize8(masterConfig.rxConfig.rcSmoothInterval);
|
||||
serialize8(masterConfig.rxConfig.rcInterpolation);
|
||||
serialize8(masterConfig.rxConfig.rcInterpolationInterval);
|
||||
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
|
||||
break;
|
||||
|
||||
|
@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(currentProfile->pidProfile.toleranceBand);
|
||||
serialize8(currentProfile->pidProfile.toleranceBandReduction);
|
||||
serialize8(currentProfile->pidProfile.itermThrottleGain);
|
||||
serialize16(currentProfile->pidProfile.pidMaxVelocityYaw);
|
||||
serialize16(1000); // Reserved for future
|
||||
serialize16(currentProfile->pidProfile.rateAccelLimit);
|
||||
serialize16(currentProfile->pidProfile.yawRateAccelLimit);
|
||||
break;
|
||||
case MSP_SENSOR_CONFIG:
|
||||
headSerialReply(3);
|
||||
|
@ -1688,8 +1688,8 @@ static bool processInCommand(void)
|
|||
masterConfig.rxConfig.rx_max_usec = read16();
|
||||
}
|
||||
if (currentPort->dataSize > 12) {
|
||||
masterConfig.rxConfig.rcSmoothing = read8();
|
||||
masterConfig.rxConfig.rcSmoothInterval = read8();
|
||||
masterConfig.rxConfig.rcInterpolation = read8();
|
||||
masterConfig.rxConfig.rcInterpolationInterval = read8();
|
||||
masterConfig.rxConfig.airModeActivateThreshold = read16();
|
||||
}
|
||||
break;
|
||||
|
@ -1858,8 +1858,8 @@ static bool processInCommand(void)
|
|||
currentProfile->pidProfile.toleranceBand = read8();
|
||||
currentProfile->pidProfile.toleranceBandReduction = read8();
|
||||
currentProfile->pidProfile.itermThrottleGain = read8();
|
||||
currentProfile->pidProfile.pidMaxVelocityYaw = read16();
|
||||
read16(); // reserved for future purposes
|
||||
currentProfile->pidProfile.rateAccelLimit = read16();
|
||||
currentProfile->pidProfile.yawRateAccelLimit = read16();
|
||||
break;
|
||||
case MSP_SET_SENSOR_CONFIG:
|
||||
masterConfig.acc_hardware = read8();
|
||||
|
|
|
@ -217,15 +217,15 @@ void processRcCommand(void)
|
|||
uint16_t rxRefreshRate;
|
||||
bool readyToCalculateRate = false;
|
||||
|
||||
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
|
||||
if (masterConfig.rxConfig.rcInterpolation || flightModeFlags) {
|
||||
if (isRXDataNew) {
|
||||
// Set RC refresh rate for sampling and channels to filter
|
||||
switch (masterConfig.rxConfig.rcSmoothing) {
|
||||
switch (masterConfig.rxConfig.rcInterpolation) {
|
||||
case(RC_SMOOTHING_AUTO):
|
||||
rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
|
||||
break;
|
||||
case(RC_SMOOTHING_MANUAL):
|
||||
rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval;
|
||||
rxRefreshRate = 1000 * masterConfig.rxConfig.rcInterpolationInterval;
|
||||
break;
|
||||
case(RC_SMOOTHING_OFF):
|
||||
case(RC_SMOOTHING_DEFAULT):
|
||||
|
@ -235,7 +235,7 @@ void processRcCommand(void)
|
|||
|
||||
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
|
||||
|
||||
if (debugMode == DEBUG_RC_SMOOTHING) {
|
||||
if (debugMode == DEBUG_RC_INTERPOLATION) {
|
||||
for (int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
|
||||
debug[3] = rxRefreshRate;
|
||||
}
|
||||
|
@ -263,9 +263,7 @@ void processRcCommand(void)
|
|||
}
|
||||
|
||||
if (readyToCalculateRate || isRXDataNew) {
|
||||
// Don't smooth yaw axis
|
||||
int axisToCalculate = (isRXDataNew) ? 3 : 2;
|
||||
for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
|
||||
for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
|
||||
|
||||
isRXDataNew = false;
|
||||
|
||||
|
|
|
@ -121,8 +121,8 @@ typedef struct rxConfig_s {
|
|||
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
|
||||
uint16_t mincheck; // minimum rc end
|
||||
uint16_t maxcheck; // maximum rc end
|
||||
uint8_t rcSmoothing;
|
||||
uint8_t rcSmoothInterval;
|
||||
uint8_t rcInterpolation;
|
||||
uint8_t rcInterpolationInterval;
|
||||
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
|
||||
uint8_t max_aux_channel;
|
||||
uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated
|
||||
|
|
|
@ -161,6 +161,9 @@
|
|||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
#define BIND_PIN PB11
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
|
|
|
@ -22,8 +22,8 @@
|
|||
#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO
|
||||
#define INVERTER_USART USART1
|
||||
|
||||
#define BEEPER PB15
|
||||
#define BEEPER_OPT PB2
|
||||
#define BEEPER PA15
|
||||
#define BEEPER_OPT PA2
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA3
|
||||
|
|
99
src/main/target/COLIBRI/config.c
Normal file
99
src/main/target/COLIBRI/config.c
Normal file
|
@ -0,0 +1,99 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "debug.h"
|
||||
|
||||
#include "build_config.h"
|
||||
|
||||
#include "blackbox/blackbox_io.h"
|
||||
|
||||
#include "common/color.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/max7456.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/escservo.h"
|
||||
#include "io/rc_controls.h"
|
||||
#include "io/rc_curves.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/vtx.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/altitudehold.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
// alternative defaults settings for Colibri/Gemini targets
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
masterConfig.mixerMode = MIXER_HEX6X;
|
||||
masterConfig.rxConfig.serialrx_provider = 2;
|
||||
featureSet(FEATURE_RX_SERIAL);
|
||||
|
||||
masterConfig.escAndServoConfig.minthrottle = 1070;
|
||||
masterConfig.escAndServoConfig.maxthrottle = 2000;
|
||||
|
||||
masterConfig.boardAlignment.pitchDegrees = 10;
|
||||
//masterConfig.rcControlsConfig.deadband = 10;
|
||||
//masterConfig.rcControlsConfig.yaw_deadband = 10;
|
||||
masterConfig.mag_hardware = 1;
|
||||
|
||||
masterConfig.profile[0].controlRateProfile[0].dynThrPID = 45;
|
||||
masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint = 1700;
|
||||
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
}
|
123
src/main/target/COLIBRI/target.c
Normal file
123
src/main/target/COLIBRI/target.c
Normal file
|
@ -0,0 +1,123 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
#include "drivers/pwm_mapping.h"
|
||||
|
||||
const uint16_t multiPPM[] = {
|
||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM7 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM8 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t multiPWM[] = {
|
||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM6 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM7 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t airPPM[] = {
|
||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM2 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM3 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM4 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM7 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM8 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t airPWM[] = {
|
||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM6 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM7 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8
|
||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
||||
PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
||||
PWM13 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM14 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM15 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
{ TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM1 }, // S1_IN
|
||||
{ TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S2_IN
|
||||
{ TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN
|
||||
{ TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN
|
||||
{ TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S5_IN
|
||||
{ TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S6_IN
|
||||
{ TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S7_IN
|
||||
{ TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S8_IN
|
||||
|
||||
|
||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT
|
||||
{ TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT
|
||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S3_OUT
|
||||
{ TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S4_OUT
|
||||
{ TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S5_OUT
|
||||
{ TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S6_OUT
|
||||
{ TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM10 }, // S7_OUT
|
||||
{ TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM11 }, // S8_OUT
|
||||
};
|
148
src/main/target/COLIBRI/target.h
Normal file
148
src/main/target/COLIBRI/target.h
Normal file
|
@ -0,0 +1,148 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define PLL_M 16
|
||||
#define PLL_N 336
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "COLI"
|
||||
|
||||
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
|
||||
|
||||
#define USBD_PRODUCT_STRING "Colibri"
|
||||
#ifdef OPBL
|
||||
#define USBD_SERIALNUMBER_STRING "0x8020000"
|
||||
#endif
|
||||
|
||||
#define LED0 PC14
|
||||
#define LED1 PC13
|
||||
#define BEEPER PC5
|
||||
#define INVERTER PB2 // PB2 used as inverter select GPIO
|
||||
#define INVERTER_USART USART2
|
||||
|
||||
#define MPU6000_CS_PIN PC4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG_FLIP
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC0
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled)
|
||||
|
||||
#define MAG
|
||||
#define USE_MAG_HMC5883
|
||||
#define MAG_HMC5883_ALIGN CW270_DEG_FLIP
|
||||
|
||||
#define USE_MAG_DATA_READY_SIGNAL
|
||||
#define ENSURE_MAG_DATA_READY_IS_HIGH
|
||||
#define MAG_INT_EXTI PC1
|
||||
|
||||
#define BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
||||
#define M25P16_CS_PIN PB12
|
||||
#define M25P16_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
|
||||
|
||||
#define USE_VCP
|
||||
#define VBUS_SENSING_PIN PA9
|
||||
|
||||
#define USE_UART1
|
||||
#define UART1_RX_PIN PB7
|
||||
#define UART1_TX_PIN PB6
|
||||
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
|
||||
|
||||
#define USE_UART2
|
||||
#define UART2_RX_PIN PA3
|
||||
#define UART2_TX_PIN PA2
|
||||
|
||||
#define USE_UART3
|
||||
#define UART3_RX_PIN PB11
|
||||
#define UART3_TX_PIN PB10
|
||||
|
||||
//#define USE_UART4
|
||||
#define UART4_RX_PIN PC11
|
||||
#define UART4_TX_PIN PC10
|
||||
|
||||
//#define USE_UART5
|
||||
#define UART5_RX_PIN PD2
|
||||
#define UART5_TX_PIN PC12
|
||||
|
||||
#define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3
|
||||
|
||||
#define USE_SPI
|
||||
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define SPI1_NSS_PIN PC4
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define SPI2_NSS_PIN PB12
|
||||
#define SPI2_SCK_PIN PB13
|
||||
#define SPI2_MISO_PIN PC2
|
||||
#define SPI2_MOSI_PIN PC3
|
||||
|
||||
#define USE_I2C
|
||||
#define I2C_DEVICE (I2CDEV_3)
|
||||
#define I2C3_SCL PA8
|
||||
#define I2C3_SDA PC9
|
||||
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
|
||||
#define LED_STRIP
|
||||
|
||||
#define WS2811_PIN PB7 // Shared UART1
|
||||
#define WS2811_TIMER TIM4
|
||||
#define WS2811_TIMER_CHANNEL TIM_Channel_2
|
||||
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST3_HANDLER
|
||||
#define WS2811_DMA_STREAM DMA1_Stream3
|
||||
#define WS2811_DMA_FLAG DMA_FLAG_TCIF3
|
||||
#define WS2811_DMA_IT DMA_IT_TCIF3
|
||||
#define WS2811_DMA_CHANNEL DMA_Channel_2
|
||||
#define WS2811_DMA_IRQ DMA1_Stream3_IRQn
|
||||
|
||||
// alternative defaults for Colibri/Gemini target
|
||||
#define TARGET_CONFIG
|
||||
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
|
||||
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL)
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD 0xffff
|
||||
|
||||
#define USABLE_TIMER_CHANNEL_COUNT 16
|
||||
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11))
|
11
src/main/target/COLIBRI/target.mk
Normal file
11
src/main/target/COLIBRI/target.mk
Normal file
|
@ -0,0 +1,11 @@
|
|||
F405_TARGETS += $(TARGET)
|
||||
FEATURES += VCP ONBOARDFLASH
|
||||
HSE_VALUE = 16000000
|
||||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro_spi_mpu6000.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
drivers/compass_hmc5883l.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/light_ws2811strip_stm32f4xx.c \
|
||||
|
|
@ -28,7 +28,7 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define MPU_INT_EXTI PA3
|
||||
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
|
|
@ -29,11 +29,6 @@ const uint16_t multiPPM[] = {
|
|||
PWM1 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
|
|
|
@ -175,6 +175,8 @@
|
|||
#define BUTTON_B_PORT GPIOB
|
||||
#define BUTTON_B_PIN Pin_0
|
||||
|
||||
#define AVOID_UART3_FOR_PWM_PPM
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3,
|
||||
#define BIND_PIN PB11
|
||||
|
|
|
@ -112,6 +112,10 @@
|
|||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3,
|
||||
#define BIND_PIN PB11
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
|
|
|
@ -92,6 +92,10 @@
|
|||
#define USE_SERVOS
|
||||
#define USE_CLI
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
#define BIND_PIN PA3
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
|
|
0
src/main/target/SPARKY2/SPARKY_OPBL.mk
Normal file
0
src/main/target/SPARKY2/SPARKY_OPBL.mk
Normal file
99
src/main/target/SPARKY2/target.c
Normal file
99
src/main/target/SPARKY2/target.c
Normal file
|
@ -0,0 +1,99 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
const uint16_t multiPPM[] = {
|
||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t multiPWM[] = {
|
||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed)
|
||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed)
|
||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3
|
||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t airPPM[] = {
|
||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||
PWM8 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM2 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM3 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM4 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const uint16_t airPWM[] = {
|
||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
||||
PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
{ TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN
|
||||
{ TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN
|
||||
{ TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3
|
||||
{ TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN
|
||||
{ TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN
|
||||
|
||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT
|
||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT
|
||||
{ TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT
|
||||
{ TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT
|
||||
{ TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3
|
||||
{ TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT
|
||||
};
|
||||
|
131
src/main/target/SPARKY2/target.h
Normal file
131
src/main/target/SPARKY2/target.h
Normal file
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#define TARGET_BOARD_IDENTIFIER "SPK2"
|
||||
|
||||
#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8)
|
||||
|
||||
#define USBD_PRODUCT_STRING "Sparky 2.0"
|
||||
#ifdef OPBL
|
||||
#define USBD_SERIALNUMBER_STRING "0x8020000"
|
||||
#endif
|
||||
|
||||
#define LED0 PB5
|
||||
#define LED1 PB4
|
||||
#define LED2 PB6
|
||||
|
||||
#define BEEPER PC9
|
||||
|
||||
#define INVERTER PC6
|
||||
#define INVERTER_USART USART6
|
||||
|
||||
// MPU9250 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC5
|
||||
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define MPU9250_CS_PIN PC4
|
||||
#define MPU9250_SPI_INSTANCE SPI1
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_SPI_MPU9250
|
||||
#define ACC_MPU9250_ALIGN CW270_DEG
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_SPI_MPU9250
|
||||
#define GYRO_MPU9250_ALIGN CW270_DEG
|
||||
|
||||
#define MAG
|
||||
//#define USE_MAG_HMC5883
|
||||
#define USE_MAG_AK8963
|
||||
|
||||
//#define MAG_HMC5883_ALIGN CW180_DEG
|
||||
#define MAG_AK8963_ALIGN CW270_DEG
|
||||
|
||||
#define BARO
|
||||
#define USE_BARO_MS5611
|
||||
//#define USE_BARO_BMP280
|
||||
|
||||
#define M25P16_CS_PIN PB3
|
||||
#define M25P16_SPI_INSTANCE SPI3
|
||||
|
||||
//#define RFM22B_CS_PIN PA15
|
||||
//#define RFM22B_SPI_INSTANCE SPI3
|
||||
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
#define USE_FLASH_TOOLS
|
||||
|
||||
#define USE_VCP
|
||||
#define VBUS_SENSING_PIN PA8
|
||||
|
||||
#define USE_USART1
|
||||
#define USART1_RX_PIN PA10
|
||||
#define USART1_TX_PIN PA9
|
||||
#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
|
||||
|
||||
#define USE_USART3
|
||||
#define USART3_RX_PIN PB11
|
||||
#define USART3_TX_PIN PB10
|
||||
|
||||
#define USE_USART6
|
||||
#define USART6_RX_PIN PC7
|
||||
#define USART6_TX_PIN PC6 //inverter
|
||||
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
|
||||
#define USE_SPI
|
||||
|
||||
#define USE_SPI_DEVICE_1 //MPU9250
|
||||
#define SPI1_NSS_PIN PC4
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_SPI_DEVICE_3 //dataflash
|
||||
#define SPI3_NSS_PIN PB3
|
||||
#define SPI3_SCK_PIN PC10
|
||||
#define SPI3_MISO_PIN PC11
|
||||
#define SPI3_MOSI_PIN PC12
|
||||
|
||||
#define USE_I2C
|
||||
#define I2C_DEVICE (I2CDEV_1)
|
||||
//#define I2C_DEVICE_EXT (I2CDEV_2)
|
||||
|
||||
#define USE_ADC
|
||||
|
||||
#define LED_STRIP
|
||||
#define LED_STRIP_TIMER TIM5
|
||||
|
||||
#define DEFAULT_FEATURES FEATURE_BLACKBOX
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
#define SERIALRX_UART SERIAL_PORT_USART3
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
|
||||
#define USABLE_TIMER_CHANNEL_COUNT 11
|
||||
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
|
||||
|
12
src/main/target/SPARKY2/target.mk
Normal file
12
src/main/target/SPARKY2/target.mk
Normal file
|
@ -0,0 +1,12 @@
|
|||
F405_TARGETS += $(TARGET)
|
||||
FEATURES += VCP ONBOARDFLASH
|
||||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro_spi_mpu9250.c \
|
||||
drivers/barometer_bmp280.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
drivers/compass_ak8963.c \
|
||||
drivers/compass_hmc5883l.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/light_ws2811strip_stm32f4xx.c
|
||||
|
|
@ -371,7 +371,11 @@ uint32_t hse_value = HSE_VALUE;
|
|||
/************************* PLL Parameters *************************************/
|
||||
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx)
|
||||
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
|
||||
#if defined(COLIBRI)
|
||||
#define PLL_M 16
|
||||
#else
|
||||
#define PLL_M 8
|
||||
#endif
|
||||
#elif defined (STM32F446xx)
|
||||
#define PLL_M 8
|
||||
#elif defined (STM32F410xx) || defined (STM32F411xE)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue