rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into feature/f4-dma-osd

This commit is contained in:
nathan 2016-07-17 22:10:44 -07:00
parent 338d548e11 0ae5d9734e
commit da602e5846
26 changed files with 1236 additions and 1067 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/blackbox/blackbox.c
View File

@ -1211,7 +1211,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
@ -1226,7 +1226,6 @@ 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("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);

27
src/main/config/config.c
View File

@ -182,12 +182,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
void resetPidProfile(pidProfile_t *pidProfile)
{
#if (defined(STM32F10X))
pidProfile->pidController = PID_CONTROLLER_INTEGER;
#else
pidProfile->pidController = PID_CONTROLLER_FLOAT;
#endif
pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT;
pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 40;
@ -220,11 +215,19 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 180;
pidProfile->yawItermIgnoreRate = 35;
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 50;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->dynamic_pid = 1;
pidProfile->vbatPidCompensation = 0;
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
// Betaflight PID controller parameters
pidProfile->toleranceBand = 15;
pidProfile->toleranceBandReduction = 35;
pidProfile->zeroCrossAllowanceCount = 3;
pidProfile->accelerationLimitPercent = 15;
pidProfile->itermThrottleGain = 0;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
@ -278,7 +281,6 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
#endif
escAndServoConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500;
escAndServoConfig->escDesyncProtection = 0;
}
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@ -311,7 +313,6 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
batteryConfig->vbatmincellvoltage = 33;
batteryConfig->vbatwarningcellvoltage = 35;
batteryConfig->vbathysteresis = 1;
batteryConfig->vbatPidCompensation = 0;
batteryConfig->currentMeterOffset = 0;
batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
batteryConfig->batteryCapacity = 0;
@ -452,15 +453,15 @@ static void resetConf(void)
masterConfig.gyro_lpf = 0; // 256HZ default
#ifdef STM32F10X
masterConfig.gyro_sync_denom = 8;
masterConfig.pid_process_denom = 1;
#else
masterConfig.gyro_sync_denom = 4;
masterConfig.pid_process_denom = 2;
#endif
masterConfig.gyro_soft_lpf_hz = 100;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5;
masterConfig.pid_process_denom = 2;
masterConfig.debug_mode = 0;
resetAccelerometerTrims(&masterConfig.accZero);

33
src/main/drivers/serial_usb_vcp.c
View File

@ -77,27 +77,25 @@ static uint8_t usbVcpRead(serialPort_t *instance)
uint8_t buf[1];
uint32_t rxed = 0;
while (rxed < 1) {
rxed += CDC_Receive_DATA((uint8_t*)buf + rxed, 1 - rxed);
while (true) {
if (CDC_Receive_DATA(buf, 1))
return buf[0];
}
return buf[0];
}
static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count)
{
UNUSED(instance);
if (!(usbIsConnected() && usbIsConfigured())) {
return;
}
uint32_t start = millis();
for (uint8_t *p = data; count > 0; ) {
uint32_t txed = CDC_Send_DATA(p, count);
uint8_t *p = data;
uint32_t txed = 0;
while (count > 0) {
txed = CDC_Send_DATA(p, count);
count -= txed;
p += txed;
@ -120,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port)
return false;
}
uint32_t txed;
uint32_t start = millis();
uint8_t *p = port->txBuf;
uint32_t txed = 0;
while (count > 0) {
txed = CDC_Send_DATA(p, count);
count -= txed;
p += txed;
do {
txed = CDC_Send_DATA(port->txBuf, count);
} while (txed != count && (millis() - start < USB_TIMEOUT));
return txed == count;
if (millis() - start > USB_TIMEOUT) {
break;
}
}
return count == 0;
}
static void usbVcpWrite(serialPort_t *instance, uint8_t c)

58
src/main/flight/mixer.c
View File

@ -65,13 +65,11 @@ static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig;
static bool syncPwm = false;
static bool syncPwmWithPidLoop = false;
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
float errorLimiter = 1.0f;
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
@ -427,7 +425,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
motorCount = 0;
servoCount = pwmOutputConfiguration->servoCount;
syncPwm = use_unsyncedPwm;
syncPwmWithPidLoop = !use_unsyncedPwm;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer
@ -528,9 +526,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
customMixers = initialCustomMixers;
}
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm)
{
UNUSED(pwmOutputConfiguration);
syncPwmWithPidLoop = !use_unsyncedPwm;
motorCount = 4;
#ifdef USE_SERVOS
servoCount = 0;
@ -644,7 +645,7 @@ void writeMotors(void)
for (i = 0; i < motorCount; i++)
pwmWriteMotor(i, motor[i]);
if (syncPwm) {
if (syncPwmWithPidLoop) {
pwmCompleteOneshotMotorUpdate(motorCount);
}
}
@ -752,13 +753,14 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void mixTable(void)
void mixTable(void *pidProfilePtr)
{
uint32_t i = 0;
fix12_t vbatCompensationFactor = 0;
static fix12_t mixReduction;
bool use_vbat_compensation = false;
if (batteryConfig && batteryConfig->vbatPidCompensation) {
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
if (batteryConfig && pidProfile->vbatPidCompensation) {
use_vbat_compensation = true;
vbatCompensationFactor = calculateVbatPidCompensation();
}
@ -827,21 +829,40 @@ void mixTable(void)
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// adjust feedback to scale PID error inputs to our limitations.
errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f);
if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100;
// Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math
float motorDtms;
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static uint32_t previousMotorTime;
uint32_t currentMotorTime = micros();
motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f;
previousMotorTime = currentMotorTime;
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
// Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS];
// acceleration limit
float delta = motor[i] - lastFilteredMotor[i];
const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f);
float maxDelta = maxDeltaPerMs * motorDtms;
if (delta > maxDelta) { // accelerating too hard
motor[i] = lastFilteredMotor[i] + maxDelta;
}
lastFilteredMotor[i] = motor[i];
}
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) {
@ -860,19 +881,6 @@ void mixTable(void)
motor[i] = escAndServoConfig->mincommand;
}
}
// Experimental Code. Anti Desync feature for ESC's
if (escAndServoConfig->escDesyncProtection) {
const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000);
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
}
}
}
// Disarmed mode

2
src/main/flight/mixer.h
View File

@ -211,7 +211,7 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
#endif
void mixerResetDisarmedMotors(void);
void mixTable(void);
void mixTable(void *pidProfilePtr);
void syncMotors(bool enabled);
void writeMotors(void);
void stopMotors(void);

129
src/main/flight/pid.c
View File

@ -49,9 +49,10 @@
extern uint8_t motorCount;
uint32_t targetPidLooptime;
extern float errorLimiter;
extern float angleRate[3], angleRateSmooth[3];
static bool pidStabilisationEnabled;
int16_t axisPID[3];
#ifdef BLACKBOX
@ -62,46 +63,31 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
uint8_t PIDweight[3];
static int32_t errorGyroI[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3];
#endif
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using
pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using
void setTargetPidLooptime(uint8_t pidProcessDenom)
{
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
{
uint16_t dynamicKi;
uint16_t resetRate;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi;
}
void pidResetErrorGyroState(void)
{
int axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
#ifndef SKIP_PID_LUXFLOAT
errorGyroIf[axis] = 0.0f;
#endif
}
}
void pidStabilisationState(pidStabilisationState_e pidControllerState)
{
pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
}
float getdT (void)
{
static float dT;
@ -115,8 +101,8 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
#ifndef SKIP_PID_LUXFLOAT
static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float RateError = 0, gyroRate = 0, RateErrorSmooth = 0;
@ -129,9 +115,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
// Scaling factors for Pids for better tunable range in configurator
static const float luxPTermScale = 1.0f / 128;
static const float luxITermScale = 1000000.0f / 0x1000000;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508;
static const float PTermScale = 0.032029f;
static const float ITermScale = 0.244381f;
static const float DTermScale = 0.000529f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -147,6 +133,25 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
}
// Yet Highly experimental and under test and development
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
static float kiThrottleGain = 1.0f;
if (pidProfile->itermThrottleGain) {
const uint16_t maxLoopCount = 20000 / targetPidLooptime;
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
static int16_t previousThrottle;
static uint16_t loopIncrement;
if (loopIncrement >= maxLoopCount) {
kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
previousThrottle = rcCommand[THROTTLE];
loopIncrement = 0;
} else {
loopIncrement++;
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
@ -170,7 +175,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
}
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion
gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
@ -178,6 +183,34 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = angleRate[axis] - gyroRate;
float dynReduction = tpaFactor;
// Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
if (pidProfile->toleranceBand) {
const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f;
static uint8_t zeroCrossCount[3];
static uint8_t currentErrorPolarity[3];
if (ABS(RateError) < pidProfile->toleranceBand) {
if (zeroCrossCount[axis]) {
if (currentErrorPolarity[axis] == POSITIVE_ERROR) {
if (RateError < 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = NEGATIVE_ERROR;
}
} else {
if (RateError > 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = POSITIVE_ERROR;
}
}
} else {
dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f);
}
} else {
zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount;
currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR;
}
}
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
@ -187,7 +220,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
// -----calculate P component
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -195,9 +228,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
// -----calculate I component.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
// Prevent strong Iterm accumulation during stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -232,12 +267,14 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -251,9 +288,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
#endif
}
}
#endif
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
int axis;
@ -330,11 +367,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
// Prevent Accumulation
uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8);
uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
int32_t rateErrorLimited = errorLimiter * RateError;
errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI;
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -377,6 +415,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
axisPID[axis] = PTerm + ITerm + DTerm;
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -396,13 +435,11 @@ void pidSetController(pidControllerType_e type)
{
switch (type) {
default:
case PID_CONTROLLER_INTEGER:
pid_controller = pidInteger;
case PID_CONTROLLER_LEGACY:
pid_controller = pidLegacy;
break;
#ifndef SKIP_PID_LUXFLOAT
case PID_CONTROLLER_FLOAT:
pid_controller = pidFloat;
#endif
case PID_CONTROLLER_BETAFLIGHT:
pid_controller = pidBetaflight;
}
}

27
src/main/flight/pid.h
View File

@ -41,8 +41,8 @@ typedef enum {
} pidIndex_e;
typedef enum {
PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets
PID_CONTROLLER_FLOAT,
PID_CONTROLLER_LEGACY = 0, // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future
PID_CONTROLLER_BETAFLIGHT, // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future
PID_COUNT
} pidControllerType_e;
@ -57,10 +57,18 @@ typedef enum {
SUPEREXPO_YAW_ALWAYS
} pidSuperExpoYaw_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef enum {
NEGATIVE_ERROR = 0,
POSITIVE_ERROR
} pidErrorPolarity_e;
typedef enum {
PID_STABILISATION_OFF = 0,
PID_STABILISATION_ON
} pidStabilisationState_e;
typedef struct pidProfile_s {
uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat)
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
@ -73,7 +81,15 @@ typedef struct pidProfile_s {
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I)
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active.
// Betaflight PID controller parameters
uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances
uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage
uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in
uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
@ -97,5 +113,6 @@ extern uint32_t targetPidLooptime;
void pidSetController(pidControllerType_e type);
void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void setTargetPidLooptime(uint8_t pidProcessDenom);

1
src/main/io/escservo.h
View File

@ -24,5 +24,4 @@ typedef struct escAndServoConfig_s {
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
uint16_t escDesyncProtection; // Value that a motor is allowed to increase or decrease in a period of 1ms
} escAndServoConfig_t;

1
src/main/io/rc_controls.h
View File

@ -50,6 +50,7 @@ typedef enum {
BOXFAILSAFE,
BOXAIRMODE,
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
CHECKBOX_ITEM_COUNT
} boxId_e;

1018
src/main/io/serial_4way.c
View File

File diff suppressed because it is too large Load Diff

131
src/main/io/serial_4way.h
View File

@ -15,127 +15,36 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* Author: 4712
*/
#pragma once
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "serial_4way_impl.h"
#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
#define USE_SERIAL_4WAY_SK_BOOTLOADER
typedef enum {
imC2 = 0,
imSIL_BLB = 1,
imATM_BLB = 2,
imSK = 3,
} esc4wayInterfaceMode_e;
#define imC2 0
#define imSIL_BLB 1
#define imATM_BLB 2
#define imSK 3
typedef enum {
// Test Interface still present
cmd_InterfaceTestAlive = 0x30, // '0' alive
// RETURN: ACK
extern uint8_t selected_esc;
// get Protocol Version Number 01..255
cmd_ProtocolGetVersion = 0x31, // '1' version
// RETURN: uint8_t VersionNumber + ACK
extern ioMem_t ioMem;
// get Version String
cmd_InterfaceGetName = 0x32, // '2' name
// RETURN: String + ACK
typedef union __attribute__ ((packed)) {
uint8_t bytes[2];
uint16_t word;
} uint8_16_u;
//get Version Number 01..255
cmd_InterfaceGetVersion = 0x33, // '3' version
// RETURN: uint16_t VersionNumber + ACK
typedef union __attribute__ ((packed)) {
uint8_t bytes[4];
uint16_t words[2];
uint32_t dword;
} uint8_32_u;
// Exit / Restart Interface - can be used to switch to Box Mode
cmd_InterfaceExit = 0x34, // '4' exit
// RETURN: ACK
//extern uint8_32_u DeviceInfo;
// Reset the Device connected to the Interface
cmd_DeviceReset = 0x35, // '5' reset
// PARAM: uint8_t escId
// RETURN: ACK
// Get the Device ID connected
// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106
// RETURN: uint8_t DeviceID + ACK
// Initialize Flash Access for Device connected
// Autodetects interface protocol; retruns device signature and protocol
cmd_DeviceInitFlash = 0x37, // '7' init flash access
// PARAM: uint8_t escId
// RETURN: uint8_t deviceInfo[4] + ACK
// Erase the whole Device Memory of connected Device
cmd_DeviceEraseAll = 0x38, // '8' erase all
// RETURN: ACK
// Erase one Page of Device Memory of connected Device
cmd_DevicePageErase = 0x39, // '9' page erase
// PARAM: uint8_t PageNumber (512B pages)
// RETURN: APageNumber ACK
// Read to Buffer from FLASH Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceRead = 0x3A, // ':' read Device
// PARAM: [ADRESS] uint8_t BuffLen
// RETURN: [ADRESS, len] Buffer[0..256] ACK
// Write Buffer to FLASH Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceWrite = 0x3B, // ';' write
// PARAM: [ADRESS + BuffLen] Buffer[1..256]
// RETURN: ACK
// Set C2CK low infinite - permanent Reset state (unimplemented)
cmd_DeviceC2CK_LOW = 0x3C, // '<'
// RETURN: ACK
// Read to Buffer from EEPROM Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceReadEEprom = 0x3D, // '=' read Device
// PARAM: [ADRESS] uint8_t BuffLen
// RETURN: [ADRESS + BuffLen] + Buffer[1..256] ACK
// Write Buffer to EEPROM Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceWriteEEprom = 0x3E, // '>' write
// PARAM: [ADRESS + BuffLen] Buffer[1..256]
// RETURN: ACK
// Set Interface Mode
cmd_InterfaceSetMode = 0x3F, // '?'
// PARAM: uint8_t Mode (interfaceMode_e)
// RETURN: ACK
} esc4wayCmd_e;
// responses
typedef enum {
esc4wayAck_OK = 0x00,
// esc4wayAck_I_UNKNOWN_ERROR = 0x01,
esc4wayAck_I_INVALID_CMD = 0x02,
esc4wayAck_I_INVALID_CRC = 0x03,
esc4wayAck_I_VERIFY_ERROR = 0x04,
// esc4wayAck_D_INVALID_COMMAND = 0x05,
// esc4wayAck_D_COMMAND_FAILED = 0x06,
// esc4wayAck_D_UNKNOWN_ERROR = 0x07,
esc4wayAck_I_INVALID_CHANNEL = 0x08,
esc4wayAck_I_INVALID_PARAM = 0x09,
esc4wayAck_D_GENERAL_ERROR = 0x0f,
} esc4wayAck_e;
typedef struct escDeviceInfo_s {
uint16_t signature; // lower 16 bit of signature
uint8_t signature2; // top 8 bit of signature for SK / BootMsg last char from BL
uint8_t interfaceMode;
} escDeviceInfo_t;
extern bool esc4wayExitRequested; // flag that exit was requested. Set by esc4wayProcessCmd, used internally by esc4wayProcess
int esc4wayInit(void);
void esc4wayStart(void);
bool isMcuConnected(void);
uint8_t esc4wayInit(void);
void esc4wayProcess(serialPort_t *mspPort);
void esc4wayRelease(void);
void esc4wayProcess(serialPort_t *serial);
esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen);
#endif

329
src/main/io/serial_4way_avrootloader.c
View File

@ -24,271 +24,289 @@
#include <stdlib.h>
#include "platform.h"
#include "common/utils.h"
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/buf_writer.h"
#include "drivers/pwm_mapping.h"
#include "drivers/gpio.h"
#include "io/serial.h"
#include "io/serial_msp.h"
#include "io/serial_4way.h"
#include "io/serial_4way_impl.h"
#include "io/serial_4way_avrootloader.h"
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
// Bootloader commands
// RunCmd
#define RestartBootloader 0
#define ExitBootloader 1
#define ExitBootloader 1
#define CMD_RUN 0x00
#define CMD_PROG_FLASH 0x01
#define CMD_ERASE_FLASH 0x02
#define CMD_RUN 0x00
#define CMD_PROG_FLASH 0x01
#define CMD_ERASE_FLASH 0x02
#define CMD_READ_FLASH_SIL 0x03
#define CMD_VERIFY_FLASH 0x03
#define CMD_READ_EEPROM 0x04
#define CMD_PROG_EEPROM 0x05
#define CMD_READ_SRAM 0x06
#define CMD_VERIFY_FLASH 0x03
#define CMD_READ_EEPROM 0x04
#define CMD_PROG_EEPROM 0x05
#define CMD_READ_SRAM 0x06
#define CMD_READ_FLASH_ATM 0x07
#define CMD_KEEP_ALIVE 0xFD
#define CMD_SET_ADDRESS 0xFF
#define CMD_SET_BUFFER 0xFE
#define CMD_KEEP_ALIVE 0xFD
#define CMD_SET_ADDRESS 0xFF
#define CMD_SET_BUFFER 0xFE
#define CMD_BOOTINIT 0x07
#define CMD_BOOTSIGN 0x08
#define CMD_BOOTINIT 0x07
#define CMD_BOOTSIGN 0x08
// Bootloader result codes
#define BR_SUCCESS 0x30
#define BR_ERRORCOMMAND 0xC1
#define BR_ERRORCRC 0xC2
#define BR_NONE 0xFF
#define brSUCCESS 0x30
#define brERRORCOMMAND 0xC1
#define brERRORCRC 0xC2
#define brNONE 0xFF
#define START_BIT_TIMEOUT 2000 // 2ms
#define BIT_TIME 52 // 52uS
#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS
#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS
#define START_BIT_TIME (BIT_TIME_3_4)
#define START_BIT_TIMEOUT_MS 2
static int suart_getc(void)
#define BIT_TIME (52) //52uS
#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS
#define START_BIT_TIME (BIT_TIME_HALVE + 1)
//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE)
static uint8_t suart_getc_(uint8_t *bt)
{
uint32_t btime;
uint32_t start_time;
uint32_t wait_time = micros() + START_BIT_TIMEOUT;
uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS;
while (ESC_IS_HI) {
// check for startbit begin
if (micros() >= wait_time) {
return -1;
if (millis() >= wait_time) {
return 0;
}
}
// start bit
start_time = micros();
btime = start_time + START_BIT_TIME;
uint16_t bitmask = 0;
for(int bit = 0; bit < 10; bit++) {
while (cmp32(micros(), btime) < 0);
uint8_t bit = 0;
while (micros() < btime);
while(1) {
if (ESC_IS_HI)
{
bitmask |= (1 << bit);
}
btime = btime + BIT_TIME;
bit++;
if (bit == 10) break;
while (micros() < btime);
}
// check start bit and stop bit
if ((bitmask & (1 << 0)) || (!(bitmask & (1 << 9)))) {
return -1;
if ((bitmask & 1) || (!(bitmask & (1 << 9)))) {
return 0;
}
return bitmask >> 1;
*bt = bitmask >> 1;
return 1;
}
static void suart_putc(uint8_t byte)
static void suart_putc_(uint8_t *tx_b)
{
// send one idle bit first (stopbit from previous byte)
uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10);
// shift out stopbit first
uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10);
uint32_t btime = micros();
while(1) {
if(bitmask & 1)
if(bitmask & 1) {
ESC_SET_HI; // 1
else
}
else {
ESC_SET_LO; // 0
}
btime = btime + BIT_TIME;
bitmask >>= 1;
if (bitmask == 0)
break; // stopbit shifted out - but don't wait
while (cmp32(micros(), btime) < 0);
bitmask = (bitmask >> 1);
if (bitmask == 0) break; // stopbit shifted out - but don't wait
while (micros() < btime);
}
}
static uint16_t crc16Byte(uint16_t from, uint8_t byte)
static uint8_16_u CRC_16;
static uint8_16_u LastCRC_16;
static void ByteCrc(uint8_t *bt)
{
uint16_t crc16 = from;
for (int i = 0; i < 8; i++) {
if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) {
crc16 >>= 1;
crc16 ^= 0xA001;
uint8_t xb = *bt;
for (uint8_t i = 0; i < 8; i++)
{
if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) {
CRC_16.word = CRC_16.word >> 1;
CRC_16.word = CRC_16.word ^ 0xA001;
} else {
crc16 >>= 1;
CRC_16.word = CRC_16.word >> 1;
}
byte >>= 1;
xb = xb >> 1;
}
return crc16;
}
static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc)
static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len)
{
int crc = 0;
int c;
// len 0 means 256
CRC_16.word = 0;
LastCRC_16.word = 0;
uint8_t LastACK = brNONE;
do {
if(!suart_getc_(pstring)) goto timeout;
ByteCrc(pstring);
pstring++;
len--;
} while(len > 0);
uint8_t lastACK = BR_NONE;
for(int i = 0; i < len; i++) {
int c;
if ((c = suart_getc()) < 0) goto timeout;
crc = crc16Byte(crc, c);
pstring[i] = c;
}
if(checkCrc) {
// With CRC read 3 more
for(int i = 0; i < 2; i++) { // checksum 2 CRC bytes
if ((c = suart_getc()) < 0) goto timeout;
crc = crc16Byte(crc, c);
if(isMcuConnected()) {
//With CRC read 3 more
if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout;
if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout;
if(!suart_getc_(&LastACK)) goto timeout;
if (CRC_16.word != LastCRC_16.word) {
LastACK = brERRORCRC;
}
if((c = suart_getc()) < 0) goto timeout;
lastACK = c;
if (crc != 0) // CRC of correct message is 0
lastACK = BR_ERRORCRC;
} else {
if((c = suart_getc()) < 0) goto timeout;
lastACK = c;
if(!suart_getc_(&LastACK)) goto timeout;
}
timeout:
return (lastACK == BR_SUCCESS);
return (LastACK == brSUCCESS);
}
static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc)
static void BL_SendBuf(uint8_t *pstring, uint8_t len)
{
ESC_OUTPUT;
uint16_t crc = 0;
for(int i = 0; i < len; i++) {
suart_putc(pstring[i]);
crc = crc16Byte(crc, pstring[i]);
}
if (appendCrc) {
suart_putc(crc & 0xff);
suart_putc(crc >> 8);
CRC_16.word=0;
do {
suart_putc_(pstring);
ByteCrc(pstring);
pstring++;
len--;
} while (len > 0);
if (isMcuConnected()) {
suart_putc_(&CRC_16.bytes[0]);
suart_putc_(&CRC_16.bytes[1]);
}
ESC_INPUT;
}
uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo)
uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo)
{
#define BOOT_MSG_LEN 4
#define DevSignHi (BOOT_MSG_LEN)
#define DevSignLo (BOOT_MSG_LEN + 1)
#define BootMsgLen 4
#define DevSignHi (BootMsgLen)
#define DevSignLo (BootMsgLen+1)
memset(pDeviceInfo, 0, sizeof(*pDeviceInfo));
uint8_t bootInfo[BOOT_MSG_LEN + 4];
static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471";
//DeviceInfo.dword=0; is set before
uint8_t BootInfo[9];
uint8_t BootMsg[BootMsgLen-1] = "471";
// x * 0 + 9
#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
// SK message was sent during autodetection, use longer preamble
uint8_t bootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
BL_SendBuf(BootInit, 21);
#else
uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
BL_SendBuf(BootInit, 17);
#endif
BL_SendBuf(bootInit, sizeof(bootInit), false);
if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false))
if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) {
return 0;
}
// BootInfo has no CRC (ACK byte already analyzed... )
// Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK)
if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK
return 0;
for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK
if (BootInfo[i] != BootMsg[i]) {
return (0);
}
}
pDeviceInfo->signature2 = bootInfo[BOOT_MSG_LEN - 1]; // taken from bootloaderMsg part, ascii 'c' now
pDeviceInfo->signature = (bootInfo[DevSignHi] << 8) | bootInfo[DevSignLo]; // SIGNATURE_001, SIGNATURE_002
return 1;
//only 2 bytes used $1E9307 -> 0x9307
pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1];
pDeviceInfo->bytes[1] = BootInfo[DevSignHi];
pDeviceInfo->bytes[0] = BootInfo[DevSignLo];
return (1);
}
static uint8_t BL_GetACK(int timeout)
static uint8_t BL_GetACK(uint32_t Timeout)
{
int c;
while ((c = suart_getc()) < 0)
if(--timeout < 0) // timeout=1 -> 1 retry
return BR_NONE;
return c;
uint8_t LastACK = brNONE;
while (!(suart_getc_(&LastACK)) && (Timeout)) {
Timeout--;
} ;
return (LastACK);
}
uint8_t BL_SendCMDKeepAlive(void)
uint8_t BL_SendCMDKeepAlive(void)
{
uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
BL_SendBuf(sCMD, sizeof(sCMD), true);
if (BL_GetACK(1) != BR_ERRORCOMMAND)
BL_SendBuf(sCMD, 2);
if (BL_GetACK(1) != brERRORCOMMAND) {
return 0;
}
return 1;
}
void BL_SendCMDRunRestartBootloader(void)
void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo)
{
uint8_t sCMD[] = {RestartBootloader, 0};
BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00)
pDeviceInfo->bytes[0] = 1;
BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00)
return;
}
static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr
{
// skip if adr == 0xFFFF
if(pMem->addr == 0xffff)
return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff };
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(2) == BR_SUCCESS;
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L };
BL_SendBuf(sCMD, 4);
return (BL_GetACK(2) == brSUCCESS);
}
static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem)
{
uint16_t len = pMem->len;
uint8_t sCMD[] = {CMD_SET_BUFFER, 0, len >> 8, len & 0xff};
BL_SendBuf(sCMD, sizeof(sCMD), true);
if (BL_GetACK(2) != BR_NONE)
return 0;
BL_SendBuf(pMem->data, len, true);
return BL_GetACK(40) == BR_SUCCESS;
uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES};
if (pMem->D_NUM_BYTES == 0) {
// set high byte
sCMD[2] = 1;
}
BL_SendBuf(sCMD, 4);
if (BL_GetACK(2) != brNONE) return 0;
BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES);
return (BL_GetACK(40) == brSUCCESS);
}
static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
unsigned len = pMem->len;
uint8_t sCMD[] = {cmd, len & 0xff}; // 0x100 is sent a 0x00 here
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_ReadBuf(pMem->data, len, true);
if (BL_SendCMDSetAddress(pMem)) {
uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES};
BL_SendBuf(sCMD, 2);
return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES ));
}
return 0;
}
static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
if (!BL_SendCMDSetBuffer(pMem))
return 0;
uint8_t sCMD[] = {cmd, 0x01};
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(timeout) == BR_SUCCESS;
if (BL_SendCMDSetAddress(pMem)) {
if (!BL_SendCMDSetBuffer(pMem)) return 0;
uint8_t sCMD[] = {cmd, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK(timeout) == brSUCCESS);
}
return 0;
}
uint8_t BL_ReadFlashATM(ioMem_t *pMem)
uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
if(interface_mode == imATM_BLB) {
return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
} else {
return BL_ReadA(CMD_READ_FLASH_SIL, pMem);
}
}
uint8_t BL_ReadFlashSIL(ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_FLASH_SIL, pMem);
}
uint8_t BL_ReadEEprom(ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_EEPROM, pMem);
@ -296,22 +314,23 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem)
uint8_t BL_PageErase(ioMem_t *pMem)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(40 * 1000 / START_BIT_TIMEOUT) == BR_SUCCESS;
if (BL_SendCMDSetAddress(pMem)) {
uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
}
return 0;
}
uint8_t BL_WriteEEprom(ioMem_t *pMem)
{
return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT);
return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS));
}
uint8_t BL_WriteFlash(ioMem_t *pMem)
{
return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT);
return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS));
}
#endif
#endif

15
src/main/io/serial_4way_avrootloader.h
View File

@ -20,17 +20,12 @@
#pragma once
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
void BL_SendBootInit(void);
uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo);
uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo);
uint8_t BL_SendCMDKeepAlive(void);
uint8_t BL_WriteEEprom(ioMem_t *pMem);
uint8_t BL_ReadEEprom(ioMem_t *pMem);
uint8_t BL_PageErase(ioMem_t *pMem);
uint8_t BL_ReadEEprom(ioMem_t *pMem);
uint8_t BL_WriteEEprom(ioMem_t *pMem);
uint8_t BL_WriteFlash(ioMem_t *pMem);
uint8_t BL_ReadFlashATM(ioMem_t *pMem);
uint8_t BL_ReadFlashSIL(ioMem_t *pMem);
void BL_SendCMDRunRestartBootloader(void);
#endif
uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem);
void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo);

25
src/main/io/serial_4way_impl.h
View File

@ -15,12 +15,15 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* Author: 4712
*/
#pragma once
#include "drivers/io.h"
typedef struct {
IO_t io;
} escHardware_t;
extern uint8_t escSelected;
extern uint8_t selected_esc;
bool isEscHi(uint8_t selEsc);
bool isEscLo(uint8_t selEsc);
@ -29,15 +32,17 @@ void setEscLo(uint8_t selEsc);
void setEscInput(uint8_t selEsc);
void setEscOutput(uint8_t selEsc);
#define ESC_IS_HI isEscHi(escSelected)
#define ESC_IS_LO isEscLo(escSelected)
#define ESC_SET_HI setEscHi(escSelected)
#define ESC_SET_LO setEscLo(escSelected)
#define ESC_INPUT setEscInput(escSelected)
#define ESC_OUTPUT setEscOutput(escSelected)
#define ESC_IS_HI isEscHi(selected_esc)
#define ESC_IS_LO isEscLo(selected_esc)
#define ESC_SET_HI setEscHi(selected_esc)
#define ESC_SET_LO setEscLo(selected_esc)
#define ESC_INPUT setEscInput(selected_esc)
#define ESC_OUTPUT setEscOutput(selected_esc)
typedef struct ioMem_s {
uint16_t len;
uint16_t addr;
uint8_t *data;
uint8_t D_NUM_BYTES;
uint8_t D_FLASH_ADDR_H;
uint8_t D_FLASH_ADDR_L;
uint8_t *D_PTR_I;
} ioMem_t;

311
src/main/io/serial_4way_stk500v2.c
View File

@ -23,38 +23,33 @@
#include <stdlib.h>
#include "platform.h"
#include "common/utils.h"
#include "drivers/gpio.h"
#include "drivers/buf_writer.h"
#include "drivers/pwm_mapping.h"
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "drivers/serial.h"
#include "drivers/system.h"
#include "config/config.h"
#include "io/serial.h"
#include "io/serial_msp.h"
#include "io/serial_4way.h"
#include "io/serial_4way_impl.h"
#include "io/serial_4way_stk500v2.h"
#include "drivers/system.h"
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
#define BIT_LO_US (32) //32uS
#define BIT_HI_US (2*BIT_LO_US)
static uint8_t StkInBuf[16];
#define BIT_LO_US 32 //32uS
#define BIT_HI_US (2 * BIT_LO_US)
static uint8_t stkInBuf[16];
#define STK_BIT_TIMEOUT 250 // micro seconds
#define STK_BIT_TIMEOUT 250 // micro seconds
#define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms
#define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms
#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5ms
#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s
#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms
#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s
#define WaitPinLo while (ESC_IS_HI) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; }
#define WaitPinHi while (ESC_IS_LO) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; }
#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;}
#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;}
static uint32_t lastBitTime;
static uint32_t hiLoTsh;
static uint32_t LastBitTime;
static uint32_t HiLoTsh;
static uint8_t SeqNumber;
static uint8_t StkCmd;
@ -77,10 +72,10 @@ static uint8_t ckSumOut;
#define STATUS_CMD_OK 0x00
#define CmdFlashEepromRead 0xA0
#define EnterIspCmd1 0xAC
#define EnterIspCmd2 0x53
#define SPI_SIGNATURE_READ 0x30
#define CmdFlashEepromRead 0xA0
#define EnterIspCmd1 0xAC
#define EnterIspCmd2 0x53
#define signature_r 0x30
#define delay_us(x) delayMicroseconds(x)
#define IRQ_OFF // dummy
@ -89,7 +84,7 @@ static uint8_t ckSumOut;
static void StkSendByte(uint8_t dat)
{
ckSumOut ^= dat;
for (uint8_t i = 0; i < 8; i++) {
for (uint8_t i = 0; i < 8; i++) {
if (dat & 0x01) {
// 1-bits are encoded as 64.0us high, 72.8us low (135.8us total).
ESC_SET_HI;
@ -111,7 +106,7 @@ static void StkSendByte(uint8_t dat)
}
}
static void StkSendPacketHeader(uint16_t len)
static void StkSendPacketHeader(void)
{
IRQ_OFF;
ESC_OUTPUT;
@ -121,9 +116,6 @@ static void StkSendPacketHeader(uint16_t len)
ckSumOut = 0;
StkSendByte(MESSAGE_START);
StkSendByte(++SeqNumber);
StkSendByte(len >> 8);
StkSendByte(len & 0xff);
StkSendByte(TOKEN);
}
static void StkSendPacketFooter(void)
@ -135,14 +127,16 @@ static void StkSendPacketFooter(void)
IRQ_ON;
}
static int8_t ReadBit(void)
{
uint32_t btimer = micros();
uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT;
WaitPinLo;
WaitPinHi;
lastBitTime = micros() - btimer;
if (lastBitTime <= hiLoTsh) {
LastBitTime = micros() - btimer;
if (LastBitTime <= HiLoTsh) {
timeout_timer = timeout_timer + STK_BIT_TIMEOUT;
WaitPinLo;
WaitPinHi;
@ -155,27 +149,30 @@ timeout:
return -1;
}
static int ReadByte(void)
static uint8_t ReadByte(uint8_t *bt)
{
uint8_t byte = 0;
for (int i = 0; i < 8; i++) {
*bt = 0;
for (uint8_t i = 0; i < 8; i++) {
int8_t bit = ReadBit();
if (bit < 0)
return -1; // timeout
byte |= bit << i;
if (bit == -1) goto timeout;
if (bit == 1) {
*bt |= (1 << i);
}
}
ckSumIn ^= byte;
return byte;
ckSumIn ^=*bt;
return 1;
timeout:
return 0;
}
static uint8_t StkReadLeader(void)
{
// Reset learned timing
hiLoTsh = BIT_HI_US + BIT_LO_US;
HiLoTsh = BIT_HI_US + BIT_LO_US;
// Wait for the first bit
int waitcycl; //250uS each
uint32_t waitcycl; //250uS each
if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) {
waitcycl = STK_WAITCYLCES_EXT;
@ -184,54 +181,57 @@ static uint8_t StkReadLeader(void)
} else {
waitcycl= STK_WAITCYLCES;
}
while(ReadBit() < 0 && --waitcycl > 0);
if (waitcycl <= 0)
goto timeout;
// Skip the first bits
for (int i = 0; i < 10; i++) {
if (ReadBit() < 0)
goto timeout;
for ( ; waitcycl >0 ; waitcycl--) {
//check is not timeout
if (ReadBit() >- 1) break;
}
// learn timing (0.75 * lastBitTime)
hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2);
//Skip the first bits
if (waitcycl == 0){
goto timeout;
}
for (uint8_t i = 0; i < 10; i++) {
if (ReadBit() == -1) goto timeout;
}
// learn timing
HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2);
// Read until we get a 0 bit
int bit;
int8_t bit;
do {
bit = ReadBit();
if (bit < 0)
goto timeout;
if (bit == -1) goto timeout;
} while (bit > 0);
return 1;
timeout:
return 0;
}
static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen)
static uint8_t StkRcvPacket(uint8_t *pstring)
{
int byte;
int len;
uint8_t bt = 0;
uint8_16_u Len;
IRQ_OFF;
if (!StkReadLeader()) goto Err;
ckSumIn=0;
if ((byte = ReadByte()) < 0 || (byte != MESSAGE_START)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != SeqNumber)) goto Err;
len = ReadByte() << 8;
len |= ReadByte();
if(len < 1 || len >= 256 + 4) // will catch timeout too; limit length to max expected size
goto Err;
if ((byte = ReadByte()) < 0 || (byte != TOKEN)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != StkCmd)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != STATUS_CMD_OK)) goto Err;
for (int i = 0; i < len - 2; i++) {
if ((byte = ReadByte()) < 0) goto Err;
if(i < maxLen) // limit saved length (buffer is only 256B, but memory read reply contains additional status + 1 unknown byte)
pstring[i] = byte;
if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err;
if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err;
ReadByte(&Len.bytes[1]);
if (Len.bytes[1] > 1) goto Err;
ReadByte(&Len.bytes[0]);
if (Len.bytes[0] < 1) goto Err;
if (!ReadByte(&bt) || (bt != TOKEN)) goto Err;
if (!ReadByte(&bt) || (bt != StkCmd)) goto Err;
if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err;
for (uint16_t i = 0; i < (Len.word - 2); i++)
{
if (!ReadByte(pstring)) goto Err;
pstring++;
}
ReadByte(); // read checksum
ReadByte(&bt);
if (ckSumIn != 0) goto Err;
IRQ_ON;
return 1;
@ -240,26 +240,27 @@ Err:
return 0;
}
static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr)
static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo)
{
StkCmd = CMD_SPI_MULTI;
StkSendPacketHeader(8);
StkCmd= CMD_SPI_MULTI;
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(8); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_SPI_MULTI);
StkSendByte(4); // NumTX
StkSendByte(4); // NumRX
StkSendByte(0); // RxStartAdr
StkSendByte(subcmd); // {TxData} Cmd
StkSendByte(addr >> 8); // {TxData} AdrHi
StkSendByte(addr & 0xff); // {TxData} AdrLow
StkSendByte(0); // {TxData} 0
StkSendByte(4); // NumTX
StkSendByte(4); // NumRX
StkSendByte(0); // RxStartAdr
StkSendByte(Cmd); // {TxData} Cmd
StkSendByte(AdrHi); // {TxData} AdrHi
StkSendByte(AdrLo); // {TxData} AdrLoch
StkSendByte(0); // {TxData} 0
StkSendPacketFooter();
if (StkRcvPacket(stkInBuf, sizeof(stkInBuf))) { // NumRX + 3
if ((stkInBuf[0] == 0x00)
&& ((stkInBuf[1] == subcmd) || (stkInBuf[1] == 0x00 /* ignore zero returns */))
&& (stkInBuf[2] == 0x00)) {
*resByte = stkInBuf[3];
}
return 1;
if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3
if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) {
*ResByte = StkInBuf[3];
}
return 1;
}
return 0;
}
@ -267,37 +268,61 @@ static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t add
static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem)
{
// ignore 0xFFFF
// assume address is set before and we read or write the immediately following memory
if((pMem->addr == 0xffff))
return 1;
// assume address is set before and we read or write the immediately following package
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
StkCmd = CMD_LOAD_ADDRESS;
StkSendPacketHeader(5);
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(5); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_LOAD_ADDRESS);
StkSendByte(0);
StkSendByte(0);
StkSendByte(pMem->addr >> 8);
StkSendByte(pMem->addr & 0xff);
StkSendByte(pMem->D_FLASH_ADDR_H);
StkSendByte(pMem->D_FLASH_ADDR_L);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket((void *)StkInBuf));
}
static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem)
{
StkSendPacketHeader(4);
uint8_t LenHi;
if (pMem->D_NUM_BYTES>0) {
LenHi=0;
} else {
LenHi=1;
}
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(4); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(StkCmd);
StkSendByte(pMem->len >> 8);
StkSendByte(pMem->len & 0xff);
StkSendByte(LenHi);
StkSendByte(pMem->D_NUM_BYTES);
StkSendByte(CmdFlashEepromRead);
StkSendPacketFooter();
return StkRcvPacket(pMem->data, pMem->len);
return (StkRcvPacket(pMem->D_PTR_I));
}
static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem)
{
StkSendPacketHeader(pMem->len + 10);
uint8_16_u Len;
uint8_t LenLo = pMem->D_NUM_BYTES;
uint8_t LenHi;
if (LenLo) {
LenHi = 0;
Len.word = LenLo + 10;
} else {
LenHi = 1;
Len.word = 256 + 10;
}
StkSendPacketHeader();
StkSendByte(Len.bytes[1]); // high byte Msg len
StkSendByte(Len.bytes[0]); // low byte Msg len
StkSendByte(TOKEN);
StkSendByte(StkCmd);
StkSendByte(pMem->len >> 8);
StkSendByte(pMem->len & 0xff);
StkSendByte(LenHi);
StkSendByte(LenLo);
StkSendByte(0); // mode
StkSendByte(0); // delay
StkSendByte(0); // cmd1
@ -305,82 +330,92 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem)
StkSendByte(0); // cmd3
StkSendByte(0); // poll1
StkSendByte(0); // poll2
for(int i = 0; i < pMem->len; i++)
StkSendByte(pMem->data[i]);
do {
StkSendByte(*pMem->D_PTR_I);
pMem->D_PTR_I++;
LenLo--;
} while (LenLo);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return StkRcvPacket((void *)StkInBuf);
}
uint8_t Stk_SignOn(void)
{
StkCmd = CMD_SIGN_ON;
StkSendPacketHeader(1);
StkCmd=CMD_SIGN_ON;
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(1); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_SIGN_ON);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket((void *) StkInBuf));
}
uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo)
uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo)
{
if (!Stk_SignOn())
return 0;
uint8_t signature[3]; // device signature, MSB first
for(unsigned i = 0; i < sizeof(signature); i++) {
if (!_CMD_SPI_MULTI_EX(&signature[i], SPI_SIGNATURE_READ, i))
return 0;
if (Stk_SignOn()) {
if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) {
if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) {
return 1;
}
}
}
// convert signature to little endian
pDeviceInfo->signature = (signature[1] << 8) | signature[2];
pDeviceInfo->signature2 = signature[0];
return 1;
return 0;
}
uint8_t Stk_Chip_Erase(void)
{
StkCmd = CMD_CHIP_ERASE_ISP;
StkSendPacketHeader(7);
StkSendByte(StkCmd);
StkSendPacketHeader();
StkSendByte(0); // high byte Msg len
StkSendByte(7); // low byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_CHIP_ERASE_ISP);
StkSendByte(20); // ChipErase_eraseDelay atmega8
StkSendByte(0); // ChipErase_pollMethod atmega8
StkSendByte(0); // ChipErase_pollMethod atmega8
StkSendByte(0xAC);
StkSendByte(0x88);
StkSendByte(0x13);
StkSendByte(0x76);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket(StkInBuf));
}
uint8_t Stk_ReadFlash(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_READ_FLASH_ISP;
return _CMD_READ_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_READ_FLASH_ISP;
return (_CMD_READ_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_ReadEEprom(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_READ_EEPROM_ISP;
return _CMD_READ_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_READ_EEPROM_ISP;
return (_CMD_READ_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_WriteFlash(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_PROGRAM_FLASH_ISP;
return _CMD_PROGRAM_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_PROGRAM_FLASH_ISP;
return (_CMD_PROGRAM_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_WriteEEprom(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_PROGRAM_EEPROM_ISP;
return _CMD_PROGRAM_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_PROGRAM_EEPROM_ISP;
return (_CMD_PROGRAM_MEM_ISP(pMem));
}
return 0;
}
#endif
#endif

5
src/main/io/serial_4way_stk500v2.h
View File

@ -17,14 +17,11 @@
*/
#pragma once
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
uint8_t Stk_SignOn(void);
uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo);
uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo);
uint8_t Stk_ReadEEprom(ioMem_t *pMem);
uint8_t Stk_WriteEEprom(ioMem_t *pMem);
uint8_t Stk_ReadFlash(ioMem_t *pMem);
uint8_t Stk_WriteFlash(ioMem_t *pMem);
uint8_t Stk_Chip_Erase(void);
#endif

15
src/main/io/serial_cli.c
View File

@ -389,7 +389,7 @@ static const char * const lookupTableBlackboxDevice[] = {
static const char * const lookupTablePidController[] = {
"UNUSED", "INT", "FLOAT"
"LEGACY", "BETAFLIGHT"
};
static const char * const lookupTableSerialRX[] = {
@ -601,7 +601,6 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
@ -680,7 +679,6 @@ const clivalue_t valueTable[] = {
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } },
{ "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
{ "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
@ -767,8 +765,15 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } },
{ "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } },
{ "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },
{ "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } },
{ "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },

88
src/main/io/serial_msp.c
View File

@ -104,74 +104,6 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c
extern void resetPidProfile(pidProfile_t *pidProfile);
void setGyroSamplingSpeed(uint16_t looptime) {
uint16_t gyroSampleRate = 1000;
uint8_t maxDivider = 1;
if (looptime != gyro.targetLooptime || looptime == 0) {
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
#ifdef STM32F303xC
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
gyroSampleRate = 125;
maxDivider = 8;
masterConfig.pid_process_denom = 1;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
if (looptime < 250) {
masterConfig.acc_hardware = ACC_NONE;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
masterConfig.pid_process_denom = 2;
} else if (looptime < 375) {
masterConfig.acc_hardware = CONFIG_FASTLOOP_PREFERRED_ACC;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
masterConfig.pid_process_denom = 2;
}
masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
} else {
masterConfig.gyro_lpf = 0;
masterConfig.gyro_sync_denom = 8;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
}
#else
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
masterConfig.acc_hardware = ACC_NONE;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
gyroSampleRate = 125;
maxDivider = 8;
masterConfig.pid_process_denom = 1;
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
masterConfig.pid_process_denom = 2;
}
if (looptime < 250) {
masterConfig.pid_process_denom = 4;
} else if (looptime < 375) {
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
masterConfig.pid_process_denom = 3;
} else {
masterConfig.pid_process_denom = 2;
}
}
masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
} else {
masterConfig.gyro_lpf = 0;
masterConfig.gyro_sync_denom = 8;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
masterConfig.pid_process_denom = 1;
}
#endif
}
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
@ -214,6 +146,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -542,6 +475,8 @@ void mspInit(serialConfig_t *serialConfig)
if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;
}
@ -737,7 +672,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_STATUS_EX:
headSerialReply(12);
headSerialReply(13);
serialize16(cycleTime);
#ifdef USE_I2C
serialize16(i2cGetErrorCounter());
@ -1284,14 +1219,14 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
serialize16(currentProfile->pidProfile.yaw_p_limit);
serialize8(currentProfile->pidProfile.deltaMethod);
serialize8(masterConfig.batteryConfig.vbatPidCompensation);
serialize8(currentProfile->pidProfile.vbatPidCompensation);
break;
case MSP_SPECIAL_PARAMETERS:
headSerialReply(1 + 2 + 1 + 2);
serialize8(currentControlRateProfile->rcYawRate8);
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
serialize8(masterConfig.rxConfig.rcSmoothInterval);
serialize16(masterConfig.escAndServoConfig.escDesyncProtection);
serialize16(currentProfile->pidProfile.accelerationLimitPercent);
break;
case MSP_SENSOR_CONFIG:
headSerialReply(3);
@ -1311,7 +1246,6 @@ static bool processInCommand(void)
uint32_t i;
uint16_t tmp;
uint8_t rate;
uint8_t oldPid;
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
@ -1358,13 +1292,11 @@ static bool processInCommand(void)
masterConfig.disarm_kill_switch = read8();
break;
case MSP_SET_LOOP_TIME:
setGyroSamplingSpeed(read16());
read16();
break;
case MSP_SET_PID_CONTROLLER:
oldPid = currentProfile->pidProfile.pidController;
currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);
currentProfile->pidProfile.pidController = constrain(read8(), 0, 1);
pidSetController(currentProfile->pidProfile.pidController);
if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
break;
case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) {
@ -1863,13 +1795,13 @@ static bool processInCommand(void)
currentProfile->pidProfile.yawItermIgnoreRate = read16();
currentProfile->pidProfile.yaw_p_limit = read16();
currentProfile->pidProfile.deltaMethod = read8();
masterConfig.batteryConfig.vbatPidCompensation = read8();
currentProfile->pidProfile.vbatPidCompensation = read8();
break;
case MSP_SET_SPECIAL_PARAMETERS:
currentControlRateProfile->rcYawRate8 = read8();
masterConfig.rxConfig.airModeActivateThreshold = read16();
masterConfig.rxConfig.rcSmoothInterval = read8();
masterConfig.escAndServoConfig.escDesyncProtection = read16();
currentProfile->pidProfile.accelerationLimitPercent = read16();
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8();

4
src/main/main.c
View File

@ -310,12 +310,12 @@ void init(void)
featureClear(FEATURE_ONESHOT125);
}
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm;
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED;
// Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;

58
src/main/mw.c
View File

@ -184,8 +184,28 @@ float calculateRate(int axis, int16_t rc) {
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
}
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
else
return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec)
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = angleRate[ROLL];
int16_t yaw = angleRate[YAW];
angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
void processRcCommand(void)
@ -208,6 +228,11 @@ void processRcCommand(void)
if (isRXDataNew) {
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
@ -230,24 +255,6 @@ void processRcCommand(void)
}
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
int16_t yaw = rcCommand[YAW];
rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
static void updateRcCommands(void)
{
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
@ -310,11 +317,6 @@ static void updateRcCommands(void)
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
// experimental scaling of RC command to FPV cam angle
if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
}
static void updateLEDs(void)
@ -516,6 +518,12 @@ void processRx(void)
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) {
pidResetErrorGyroState();
if (currentProfile->pidProfile.zeroThrottleStabilisation)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
if (currentProfile->pidProfile.zeroThrottleStabilisation || wasAirmodeIsActivated) pidStabilisationState(PID_STABILISATION_ON);
}
// When armed and motors aren't spinning, do beeps and then disarm
@ -763,7 +771,7 @@ void subTaskMotorUpdate(void)
previousMotorUpdateTime = startTime;
}
mixTable();
mixTable(&currentProfile->pidProfile);
#ifdef USE_SERVOS
filterServos();

2
src/main/sensors/battery.c
View File

@ -212,7 +212,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
fix12_t calculateVbatPidCompensation(void) {
fix12_t batteryScaler;
if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) {
if (feature(FEATURE_VBAT) && batteryCellCount > 1) {
uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount;
batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage));
} else {

1
src/main/sensors/battery.h
View File

@ -43,7 +43,6 @@ typedef struct batteryConfig_s {
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps

1
src/main/target/CC3D/target.h
View File

@ -121,7 +121,6 @@
#ifdef CC3D_OPBL
#define SKIP_CLI_COMMAND_HELP
//#define SKIP_PID_LUXFLOAT
#undef DISPLAY
#undef SONAR
#endif

4
src/main/target/KISSFC/target.c
View File

@ -22,7 +22,7 @@
#include "drivers/pwm_mapping.h"
const uint16_t multiPPM[] = {
PWM9 | (MAP_TO_PPM_INPUT << 8),
PWM12 | (MAP_TO_PPM_INPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
@ -32,7 +32,6 @@ const uint16_t multiPPM[] = {
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
@ -44,6 +43,7 @@ const uint16_t multiPWM[] = {
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM7 | (MAP_TO_PWM_INPUT << 8),
PWM8 | (MAP_TO_PWM_INPUT << 8),
PWM9 | (MAP_TO_PWM_INPUT << 8),
PWM10 | (MAP_TO_PWM_INPUT << 8),

2
src/main/target/KISSFC/target.h
View File

@ -17,7 +17,7 @@
#pragma once
#define TARGET_BOARD_IDENTIFIER "KISSFC"
#define TARGET_BOARD_IDENTIFIER "KISS"
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE

13
src/main/vcpf4/usbd_cdc_vcp.c
View File

@ -166,7 +166,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength)
* this function.
* @param Buf: Buffer of data to be sent
* @param Len: Number of data to be sent (in bytes)
* @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL
* @retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL
*/
static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len)
{
@ -191,18 +191,17 @@ uint8_t usbAvailable(void)
*******************************************************************************/
uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
{
uint32_t ch = 0;
uint32_t count = 0;
while (usbAvailable() && ch < len) {
recvBuf[ch] = usbData.buffer[usbData.bufferOutPosition];
while (usbAvailable() && count < len) {
recvBuf[count] = usbData.buffer[usbData.bufferOutPosition];
usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE;
ch++;
count++;
receiveLength--;
}
return ch;
return count;
}
/**
* @brief VCP_DataRx
* Data received over USB OUT endpoint are sent over CDC interface