1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00

Merge branch 'betaflight' into development

# Conflicts:
#	src/main/flight/pid.c
#	src/main/main.c
#	src/main/target/IRCFUSIONF3/target.h
This commit is contained in:
blckmn 2016-08-27 08:34:23 +10:00
commit 9bdb4a9b25
23 changed files with 333 additions and 57 deletions

View file

@ -47,6 +47,8 @@ env:
# - TARGET=VRRACE # - TARGET=VRRACE
# - TARGET=X_RACERSPI # - TARGET=X_RACERSPI
# - TARGET=ZCOREF3 # - TARGET=ZCOREF3
# - TARGET=RCEXPLORERF3
# use new docker environment # use new docker environment
sudo: false sudo: false

View file

@ -21,8 +21,8 @@ targets=("PUBLISHMETA=True" \
"TARGET=DOGE" \ "TARGET=DOGE" \
"TARGET=SINGULARITY" \ "TARGET=SINGULARITY" \
"TARGET=SIRINFPV" \ "TARGET=SIRINFPV" \
"TARGET=X_RACERSPI") "TARGET=X_RACERSPI" \
"TARGET=RCEXPLORERF3")
#fake a travis build environment #fake a travis build environment

View file

@ -56,5 +56,6 @@ typedef enum {
DEBUG_RC_INTERPOLATION, DEBUG_RC_INTERPOLATION,
DEBUG_VELOCITY, DEBUG_VELOCITY,
DEBUG_DTERM_FILTER, DEBUG_DTERM_FILTER,
DEBUG_ANGLERATE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

View file

@ -127,7 +127,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
static void resetPidProfile(pidProfile_t *pidProfile) static void resetPidProfile(pidProfile_t *pidProfile)
{ {
#if defined(SKIP_PID_FLOAT)
pidProfile->pidController = PID_CONTROLLER_LEGACY;
#else
pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT;
#endif
pidProfile->P8[ROLL] = 45; pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 40; pidProfile->I8[ROLL] = 40;
@ -171,8 +175,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
// Betaflight PID controller parameters // Betaflight PID controller parameters
pidProfile->ptermSetpointWeight = 75; pidProfile->ptermSetpointWeight = 80;
pidProfile->dtermSetpointWeight = 120; pidProfile->dtermSetpointWeight = 150;
pidProfile->yawRateAccelLimit = 220; pidProfile->yawRateAccelLimit = 220;
pidProfile->rateAccelLimit = 0; pidProfile->rateAccelLimit = 0;
pidProfile->toleranceBand = 0; pidProfile->toleranceBand = 0;
@ -378,7 +382,7 @@ void createDefaultConfig(master_t *config)
memset(config, 0, sizeof(master_t)); memset(config, 0, sizeof(master_t));
intFeatureClearAll(config); intFeatureClearAll(config);
intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config);
#ifdef DEFAULT_FEATURES #ifdef DEFAULT_FEATURES
intFeatureSet(DEFAULT_FEATURES, config); intFeatureSet(DEFAULT_FEATURES, config);
#endif #endif

View file

@ -49,7 +49,7 @@ typedef enum {
FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_CHANNEL_FORWARDING = 1 << 20,
FEATURE_TRANSPONDER = 1 << 21, FEATURE_TRANSPONDER = 1 << 21,
FEATURE_AIRMODE = 1 << 22, FEATURE_AIRMODE = 1 << 22,
FEATURE_SUPEREXPO_RATES = 1 << 23, //FEATURE_SUPEREXPO_RATES = 1 << 23,
FEATURE_VTX = 1 << 24, FEATURE_VTX = 1 << 24,
FEATURE_RX_NRF24 = 1 << 25, FEATURE_RX_NRF24 = 1 << 25,
FEATURE_SOFTSPI = 1 << 26, FEATURE_SOFTSPI = 1 << 26,

View file

@ -235,6 +235,13 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
type = MAP_TO_SERVO_OUTPUT; type = MAP_TO_SERVO_OUTPUT;
#endif #endif
#if defined(RCEXPLORERF3)
if (timerIndex == PWM2)
{
type = MAP_TO_SERVO_OUTPUT;
}
#endif
#if (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) #if (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3))
// remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer
if (init->useSoftSerial) { if (init->useSoftSerial) {

View file

@ -174,22 +174,44 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
} }
#define RC_RATE_INCREMENTAL 14.54f
float calculateSetpointRate(int axis, int16_t rc) { float calculateSetpointRate(int axis, int16_t rc) {
float angleRate; float angleRate, rcRate, rcSuperfactor, rcCommandf;
uint8_t rcExpo;
if (isSuperExpoActive()) { if (axis != YAW) {
rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); rcExpo = currentControlRateProfile->rcExpo8;
float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); rcRate = currentControlRateProfile->rcRate8 / 100.0f;
angleRate = rcFactor * ((27 * rc) / 16.0f);
} else { } else {
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; rcExpo = currentControlRateProfile->rcYawExpo8;
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
}
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcInput[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof));
}
angleRate = 200.0f * rcRate * rcCommandf;
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate *= rcSuperfactor;
}
if (debugMode == DEBUG_ANGLERATE) {
debug[axis] = angleRate;
} }
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
else else
return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
} }
void scaleRcCommandToFpvCamAngle(void) { void scaleRcCommandToFpvCamAngle(void) {
@ -299,14 +321,14 @@ static void updateRcCommands(void)
} else { } else {
tmp = 0; tmp = 0;
} }
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8); rcCommand[axis] = tmp;
} else if (axis == YAW) { } else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband; tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else { } else {
tmp = 0; tmp = 0;
} }
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;; rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
} }
if (rcData[axis] < masterConfig.rxConfig.midrc) { if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis]; rcCommand[axis] = -rcCommand[axis];
@ -814,28 +836,16 @@ uint8_t setPidUpdateCountDown(void) {
// Function for loop trigger // Function for loop trigger
void taskMainPidLoopCheck(void) void taskMainPidLoopCheck(void)
{ {
static uint32_t previousTime;
static bool runTaskMainSubprocesses; static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
cycleTime = micros() - previousTime; cycleTime = getTaskDeltaTime(TASK_SELF);
previousTime = micros();
if (debugMode == DEBUG_CYCLETIME) { if (debugMode == DEBUG_CYCLETIME) {
debug[0] = cycleTime; debug[0] = cycleTime;
debug[1] = averageSystemLoadPercent; debug[1] = averageSystemLoadPercent;
} }
const uint32_t startTime = micros();
while (true) {
if (gyroSyncCheckUpdate(&gyro)) {
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
break;
}
}
static uint8_t pidUpdateCountdown;
if (runTaskMainSubprocesses) { if (runTaskMainSubprocesses) {
subTaskMainSubprocesses(); subTaskMainSubprocesses();
runTaskMainSubprocesses = false; runTaskMainSubprocesses = false;

View file

@ -76,10 +76,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
} }
bool isSuperExpoActive(void) {
return (feature(FEATURE_SUPEREXPO_RATES));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX #ifndef BLACKBOX
UNUSED(adjustmentFunction); UNUSED(adjustmentFunction);

View file

@ -254,7 +254,6 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15 #define MAX_ADJUSTMENT_RANGE_COUNT 15
bool isAirmodeActive(void); bool isAirmodeActive(void);
bool isSuperExpoActive(void);
void resetAdjustmentStates(void); void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig);

View file

@ -50,12 +50,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
} }
} }
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate)
{
float tmpf = tmp / 100.0f;
return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f );
}
int16_t rcLookupThrottle(int32_t tmp) int16_t rcLookupThrottle(int32_t tmp)
{ {
const int32_t tmp2 = tmp / 100; const int32_t tmp2 = tmp / 100;

View file

@ -21,6 +21,5 @@ struct controlRateConfig_s;
struct escAndServoConfig_s; struct escAndServoConfig_s;
void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig);
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate);
int16_t rcLookupThrottle(int32_t tmp); int16_t rcLookupThrottle(int32_t tmp);

View file

@ -125,6 +125,25 @@ void initFilters(const pidProfile_t *pidProfile)
} }
} }
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm);
DTerm = 0.0f; // needed for blackbox
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0; // needed for blackbox
void pidSetController(pidControllerType_e type) void pidSetController(pidControllerType_e type)
{ {
switch (type) { switch (type) {

View file

@ -189,10 +189,10 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void)
STATIC_UNIT_TESTED void determineOrientationLimits(void) STATIC_UNIT_TESTED void determineOrientationLimits(void)
{ {
highestYValueForNorth = (ledGridHeight / 2) - 1; highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0);
lowestYValueForSouth = ((ledGridHeight + 1) / 2); lowestYValueForSouth = (ledGridHeight + 1) / 2;
highestXValueForWest = (ledGridWidth / 2) - 1; highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0);
lowestXValueForEast = ((ledGridWidth + 1) / 2); lowestXValueForEast = (ledGridWidth + 1) / 2;
} }
STATIC_UNIT_TESTED void updateLedCount(void) STATIC_UNIT_TESTED void updateLedCount(void)
@ -463,7 +463,11 @@ static void applyLedFixedLayers()
case LED_FUNCTION_FLIGHT_MODE: case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) {
color = *directionalColor;
}
break; // stop on first match break; // stop on first match
} }
break; break;

View file

@ -489,6 +489,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"RC_INTERPOLATION", "RC_INTERPOLATION",
"VELOCITY", "VELOCITY",
"DFILTER", "DFILTER",
"ANGLERATE",
}; };
#ifdef OSD #ifdef OSD
@ -803,9 +804,9 @@ const clivalue_t valueTable[] = {
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .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 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
{ "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } },
{ "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },
@ -855,7 +856,7 @@ const clivalue_t valueTable[] = {
{ "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "iterm_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 } }, { "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, 200 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
{ "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
{ "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } },
@ -864,7 +865,9 @@ const clivalue_t valueTable[] = {
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
#ifndef SKIP_PID_FLOAT
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
#endif
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } },

View file

@ -1360,8 +1360,10 @@ static bool processInCommand(void)
read16(); read16();
break; break;
case MSP_SET_PID_CONTROLLER: case MSP_SET_PID_CONTROLLER:
#ifndef SKIP_PID_FLOAT
currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); currentProfile->pidProfile.pidController = constrain(read8(), 0, 1);
pidSetController(currentProfile->pidProfile.pidController); pidSetController(currentProfile->pidProfile.pidController);
#endif
break; break;
case MSP_SET_PID: case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) { for (i = 0; i < PID_ITEM_COUNT; i++) {

View file

@ -604,7 +604,6 @@ void init(void)
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX #ifdef BLACKBOX
initBlackbox(); initBlackbox();
#endif #endif

View file

@ -118,6 +118,7 @@
#undef SONAR #undef SONAR
#undef USE_SOFTSERIAL1 #undef USE_SOFTSERIAL1
#undef LED_STRIP #undef LED_STRIP
#define SKIP_PID_FLOAT
#endif #endif
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_RX_FEATURE FEATURE_RX_PPM

View file

@ -23,6 +23,9 @@
#define LED0 PB3 #define LED0 PB3
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL

View file

@ -88,8 +88,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB8 { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB8
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB9 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB9
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA2 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA3
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA3 { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA2
// UART3 RX/TX // UART3 RX/TX
{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7)

View file

@ -0,0 +1,71 @@
/*
* 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/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
const uint16_t multiPPM[] = {
PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
0xFFFF
};
const uint16_t multiPWM[] = {
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
0xFFFF
};
const uint16_t airPPM[] = {
PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
0xFFFF
};
const uint16_t airPWM[] = {
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3
0xFFFF
};
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - PA4
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - PA7
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM3 - PA8
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - PB0
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PB1
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - PPM
};

View file

@ -0,0 +1,145 @@
/*
* 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 "REF3"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
#define LED0 PB4
#define LED1 PB5
#define BEEPER PA0
#define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 6
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define MPU_INT_EXTI PA15
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define GYRO
#define USE_GYRO_SPI_MPU6000
#define USE_ACC_SPI_MPU6000
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_INSTANCE SPI2
#define ACC
#define ACC_MPU6000_ALIGN CW180_DEG
#define GYRO_MPU6000_ALIGN CW180_DEG
#define BARO
#define USE_BARO_MS5611
#define MAG
#define USE_MPU9250_MAG // Enables bypass configuration
#define USE_MAG_AK8975
#define USE_MAG_HMC5883 // External
#define MAG_AK8975_ALIGN CW180_DEG
#define SONAR
#define SONAR_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
#define SONAR_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
#define USB_IO
#define USE_VCP
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define SERIAL_PORT_COUNT 4
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define UART3_TX_PIN PB10 // PB10 (AF7)
#define UART3_RX_PIN PB11 // PB11 (AF7)
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
#define I2C2_SCL PA9
#define I2C2_SDA PA10
#define USE_SPI
#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG)
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA5
#define CURRENT_METER_ADC_PIN PB2
#define RSSI_ADC_PIN PA6
#define LED_STRIP // LED strip configuration using PWM motor output pin 5.
#define USE_LED_STRIP_ON_DMA1_CHANNEL3
#define WS2811_PIN PB8 // TIM16_CH1
#define WS2811_TIMER TIM16
#define WS2811_DMA_CHANNEL DMA1_Channel3
#define WS2811_IRQ DMA1_Channel3_IRQn
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER
#define DEFAULT_FEATURES FEATURE_VBAT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
#define NAV
#define NAV_AUTO_MAG_DECLINATION
#define NAV_GPS_GLITCH_DETECTION
#define NAV_MAX_WAYPOINTS 60
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define USE_SERVOS
#define USE_CLI
#define SPEKTRUM_BIND
// USART3,
#define BIND_PIN PA3
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#define USABLE_TIMER_CHANNEL_COUNT 6
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(17))

View file

@ -0,0 +1,15 @@
F3_TARGETS += $(TARGET)
FEATURES = VCP
TARGET_SRC = \
drivers/accgyro_mpu.c \
drivers/accgyro_spi_mpu6000.c \
drivers/barometer_ms5611.c \
drivers/compass_hmc5883l.c \
drivers/compass_ak8975.c \
drivers/display_ug2864hsweg01.c \
drivers/serial_usb_vcp.c \
drivers/flash_m25p16.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stm32f30x.c \
drivers/sonar_hcsr04.c

View file

@ -26,11 +26,13 @@
#define BEEPER PC15 #define BEEPER PC15
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_MAG_DATA_READY_SIGNAL //#define USE_MAG_DATA_READY_SIGNAL // XXX Do RMDO has onboard mag???
#define ENSURE_MAG_DATA_READY_IS_HIGH //#define ENSURE_MAG_DATA_READY_IS_HIGH
#define GYRO #define GYRO
#define USE_GYRO_MPU6050 #define USE_GYRO_MPU6050