From 312655183263eae0ed32a73b62ed038f0a43c6d9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 17 Jul 2016 07:32:53 +1000 Subject: [PATCH 01/19] Fixed use_unsyncedpwm inversion issue --- src/main/flight/mixer.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8db1443d0..5b0ae97aa 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -65,7 +65,7 @@ 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]; @@ -427,7 +427,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 +528,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 +647,7 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - if (syncPwm) { + if (syncPwmWithPidLoop) { pwmCompleteOneshotMotorUpdate(motorCount); } } From c16bf1400f38a0a3395291dd70f447bb87211b14 Mon Sep 17 00:00:00 2001 From: gaelj Date: Sun, 17 Jul 2016 01:04:03 +0200 Subject: [PATCH 02/19] Fix wrong header size in MSP_STATUS_EX --- src/main/io/serial_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c21680f77..01b9a9930 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -737,7 +737,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(12); + headSerialReply(13); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); From 50da1cfa1defba9bc08520a3a03ec05c2efdec2f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:19:16 +0200 Subject: [PATCH 03/19] KISSFC Move PPM to AUX2 // Board identifier set to 4 chars --- src/main/target/KISSFC/target.c | 4 ++-- src/main/target/KISSFC/target.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index e270422ad..5045599da 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -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), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 3c5f4cd5d..7224db6b6 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "KISSFC" +#define TARGET_BOARD_IDENTIFIER "KISS" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE From f80fe9ba5e8d8b9440ca325848d5bac067b20ecd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:22:20 +0200 Subject: [PATCH 04/19] Remove Auto Looptime Config --- src/main/io/serial_msp.c | 73 +--------------------------------------- 1 file changed, 1 insertion(+), 72 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c21680f77..a2cf85cb6 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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. @@ -1311,7 +1243,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 +1289,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); 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++) { From 34fd8f3c887efae56322a608229a2cd1452af2db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:41:17 +0200 Subject: [PATCH 05/19] Improve Accuracy of roll_yaw_cam_mix feature --- src/main/mw.c | 46 +++++++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 5865a5118..0f999d81c 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -188,6 +188,24 @@ float calculateRate(int axis, int16_t rc) { 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) { static int16_t lastCommand[4] = { 0, 0, 0, 0 }; @@ -208,6 +226,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 && !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 +253,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 +315,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) From 6582a958aa5c93a84f97e91abfb851df319860d3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:46:04 +0200 Subject: [PATCH 06/19] PID float scaling in deg/sec fix --- src/main/flight/pid.c | 14 +++++++------- src/main/mw.c | 6 ++++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 25ca7c40a..f90f1f48b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -129,9 +129,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 @@ -170,7 +170,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 @@ -187,7 +187,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] * tpaFactor; // 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) { @@ -197,7 +197,7 @@ 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]; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * kI, -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,7 +232,7 @@ 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] * tpaFactor, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); diff --git a/src/main/mw.c b/src/main/mw.c index 0f999d81c..66555404d 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,8 +184,10 @@ float calculateRate(int axis, int16_t rc) { angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; } - - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_INTEGER) + 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) } void scaleRcCommandToFpvCamAngle(void) { From 5e32ced94db8196ad618c701ede887f824caa199 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:00:57 +0200 Subject: [PATCH 07/19] Add FPV ANGLE MIX as a mode ident --- src/main/io/rc_controls.h | 1 + src/main/io/serial_msp.c | 3 +++ src/main/mw.c | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 75cbe4c8e..be5e7311d 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -50,6 +50,7 @@ typedef enum { BOXFAILSAFE, BOXAIRMODE, BOX3DDISABLESWITCH, + BOXFPVANGLEMIX, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index dd05efcb4..9a5b96ff5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -146,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 } }; @@ -474,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; } diff --git a/src/main/mw.c b/src/main/mw.c index 66555404d..05c8d1a08 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -229,7 +229,7 @@ void processRcCommand(void) 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 && !FLIGHT_MODE(HEADFREE_MODE)) { + if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { scaleRcCommandToFpvCamAngle(); } From b854ae7faf4d0a28b0a11d206a97f8c07b8d315b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:45:47 +0200 Subject: [PATCH 08/19] Fix fast PWM motor update --- src/main/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 0daf40276..7e6e6f182 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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; // 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; From 9cac1c9a8ac0e3c8ce0e1047d45cef1bd8e94a01 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:59:37 +0200 Subject: [PATCH 09/19] Add BRUSHED to unsynced to prevent changing motor_pwm_rate --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 7e6e6f182..944faf0fa 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -310,7 +310,7 @@ void init(void) featureClear(FEATURE_ONESHOT125); } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL; + 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); From 19a814afbb7034b0f295a605879d0c8402e7d4fb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 03:16:04 +0200 Subject: [PATCH 10/19] Seperate float and int accumulation logic --- src/main/config/config.c | 4 ++-- src/main/flight/pid.c | 29 +++++++++-------------------- 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0caa5c62e..a9723ecaa 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,8 +220,8 @@ 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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f90f1f48b..8de667012 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,20 +76,6 @@ 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; @@ -195,9 +181,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] + ITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -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 @@ -330,11 +318,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((int32_t)angleRate) << 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] + ((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 From e40942366e555acdbb0c6b23b3c47f192728e072 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 13:32:54 +1000 Subject: [PATCH 11/19] Reverting serial_4way to 2.9 code --- src/main/io/serial_4way.c | 1019 ++++++++++++++---------- src/main/io/serial_4way.h | 131 +-- src/main/io/serial_4way_avrootloader.c | 329 ++++---- src/main/io/serial_4way_avrootloader.h | 15 +- src/main/io/serial_4way_impl.h | 25 +- src/main/io/serial_4way_stk500v2.c | 311 ++++---- src/main/io/serial_4way_stk500v2.h | 5 +- 7 files changed, 999 insertions(+), 836 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 7b783422f..b065dfd91 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -25,169 +25,247 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" +#include "drivers/buf_writer.h" #include "drivers/gpio.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" +#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" +#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) #include "io/serial_4way_stk500v2.h" +#endif #define USE_TXRX_LED -#if defined(USE_TXRX_LED) && defined(LED0) -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON #ifdef LED1 -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON #else -#define TX_LED_OFF LED0_OFF -#define TX_LED_ON LED0_ON +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON #endif #else -# define RX_LED_OFF do {} while(0) -# define RX_LED_ON do {} while(0) -# define TX_LED_OFF do {} while(0) -# define TX_LED_ON do {} while(0) +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON #endif #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" -#define SERIAL_4WAY_VER_MAIN 14 -#define SERIAL_4WAY_VER_SUB_1 4 -#define SERIAL_4WAY_VER_SUB_2 4 +// *** change to adapt Revision +#define SERIAL_4WAY_VER_MAIN 14 +#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 + #define SERIAL_4WAY_PROTOCOL_VER 106 +// *** end -#if SERIAL_4WAY_VER_MAIN > 24 -# error "SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1 must fit in uint8_t" -#endif -#if SERIAL_4WAY_VER_SUB_1 >= 10 -# warning "SERIAL_4WAY_VER_SUB_1 should be 0-9" +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" #endif -#if SERIAL_4WAY_VER_SUB_2 >= 100 -# warning "SERIAL_4WAY_VER_SUB_2 should be <= 99 (9.9)" -#endif +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) -#define SERIAL_4WAY_VERSION_HI (uint8_t)(SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1) -#define SERIAL_4WAY_VERSION_LO (uint8_t)(SERIAL_4WAY_VER_SUB_2) +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) static uint8_t escCount; -uint8_t escSelected; + escHardware_t escHardware[MAX_PWM_MOTORS]; -bool esc4wayExitRequested = false; +uint8_t selected_esc; -static escDeviceInfo_t deviceInfo; +uint8_32_u DeviceInfo; -static bool isMcuConnected(void) +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) { - return deviceInfo.signature != 0; + return (DeviceInfo.bytes[0] > 0); } -static void setDisconnected(void) -{ - deviceInfo.signature = 0; -} - -bool isEscHi(uint8_t selEsc) +inline bool isEscHi(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) != Bit_RESET); } - -bool isEscLo(uint8_t selEsc) +inline bool isEscLo(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) == Bit_RESET); } -void setEscHi(uint8_t selEsc) +inline void setEscHi(uint8_t selEsc) { IOHi(escHardware[selEsc].io); } -void setEscLo(uint8_t selEsc) +inline void setEscLo(uint8_t selEsc) { IOLo(escHardware[selEsc].io); } -void setEscInput(uint8_t selEsc) +inline void setEscInput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } -void setEscOutput(uint8_t selEsc) +inline void setEscOutput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } -// Initialize 4way ESC interface -// initializes internal structures -// returns number of ESCs available -int esc4wayInit(void) +uint8_t esc4wayInit(void) { // StopPwmAllMotors(); + pwmDisableMotors(); + escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - int escIdx = 0; for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escIdx].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); - escIdx++; + escHardware[escCount].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } } - escCount = escIdx; return escCount; } -// stat BLHeli 4way interface -// sets all ESC lines as output + hi -void esc4wayStart(void) -{ - pwmDisableMotors(); // prevent updating PWM registers - for (int i = 0; i < escCount; i++) { - setEscInput(i); - setEscHi(i); - } -} - -// stops BLHeli 4way interface -// returns all claimed pins back to PWM drivers, reenables PWM void esc4wayRelease(void) { - for(int i = 0; i < escCount; i++) { - IOConfigGPIO(escHardware[i].io, IOCFG_AF_PP); // see pwmOutConfig() - setEscOutput(i); - setEscLo(i); + while (escCount > 0) { + escCount--; + IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP); + setEscLo(escCount); } - escCount = 0; pwmEnableMotors(); } -// BLHeliSuite packet framing -// for reference, see 'Manuals/BLHeliSuite 4w-if protocol.pdf' from BLHeliSuite + +#define SET_DISCONNECTED DeviceInfo.words[0] = 0 + +#define INTF_MODE_IDX 3 // index for DeviceInfostate + +// Interface related only +// establish and test connection to the Interface + // Send Structure -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) CRC16_Hi CRC16_Lo +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo // Return -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo -// esc4wayCmd_e in public header +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' -typedef enum { - // not commands, but keep naming consistent with BLHeli suite - cmd_Remote_Escape = 0x2E, // '.' - cmd_Local_Escape = 0x2F, // '/' -} syn_4way_e; +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK +// get Protocol Version Number 01..255 +#define cmd_ProtocolGetVersion 0x31 // '1' version +// RETURN: uint8_t VersionNumber + ACK + +// get Version String +#define cmd_InterfaceGetName 0x32 // '2' name +// RETURN: String + ACK + +//get Version Number 01..255 +#define cmd_InterfaceGetVersion 0x33 // '3' version +// RETURN: uint8_t AVersionNumber + ACK + + +// Exit / Restart Interface - can be used to switch to Box Mode +#define cmd_InterfaceExit 0x34 // '4' exit +// RETURN: ACK + +// Reset the Device connected to the Interface +#define cmd_DeviceReset 0x35 // '5' reset +// RETURN: ACK + +// Get the Device ID connected +// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 +// RETURN: uint8_t DeviceID + ACK + +// Initialize Flash Access for Device connected +#define cmd_DeviceInitFlash 0x37 // '7' init flash access +// RETURN: ACK + +// Erase the whole Device Memory of connected Device +#define cmd_DeviceEraseAll 0x38 // '8' erase all +// RETURN: ACK + +// Erase one Page of Device Memory of connected Device +#define cmd_DevicePageErase 0x39 // '9' page erase +// PARAM: uint8_t APageNumber +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceRead 0x3A // ':' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWrite 0x3B // ';' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set C2CK low infinite ) permanent Reset state +#define cmd_DeviceC2CK_LOW 0x3C // '<' +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceReadEEprom 0x3D // '=' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWriteEEprom 0x3E // '>' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set Interface Mode +#define cmd_InterfaceSetMode 0x3F // '?' +// #define imC2 0 +// #define imSIL_BLB 1 +// #define imATM_BLB 2 +// #define imSK 3 +// PARAM: uint8_t Mode +// RETURN: ACK or ACK_I_INVALID_CHANNEL + +// responses +#define ACK_OK 0x00 +// #define ACK_I_UNKNOWN_ERROR 0x01 +#define ACK_I_INVALID_CMD 0x02 +#define ACK_I_INVALID_CRC 0x03 +#define ACK_I_VERIFY_ERROR 0x04 +// #define ACK_D_INVALID_COMMAND 0x05 +// #define ACK_D_COMMAND_FAILED 0x06 +// #define ACK_D_UNKNOWN_ERROR 0x07 + +#define ACK_I_INVALID_CHANNEL 0x08 +#define ACK_I_INVALID_PARAM 0x09 +#define ACK_D_GENERAL_ERROR 0x0F /* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz Copyright (c) 2005, 2007 Joerg Wunsch @@ -221,12 +299,11 @@ typedef enum { CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) -{ +uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { int i; crc = crc ^ ((uint16_t)data << 8); - for (i = 0; i < 8; i++){ + for (i=0; i < 8; i++){ if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -236,388 +313,514 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) } // * End copyright -static uint16_t signaturesAtmel[] = {0x9307, 0x930A, 0x930F, 0x940B, 0}; -static uint16_t signaturesSilabs[] = {0xF310, 0xF330, 0xF410, 0xF390, 0xF850, 0xE8B1, 0xE8B2, 0}; -static bool signatureMatch(uint16_t signature, uint16_t *list) +#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ + (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) + +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ + (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ + (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ + (pDeviceInfo->words[0] == 0xE8B2)) + +static uint8_t CurrentInterfaceMode; + +static uint8_t Connect(uint8_32_u *pDeviceInfo) { - for(; *list; list++) - if(signature == *list) - return true; - return false; -} - -static uint8_t currentInterfaceMode; - -// Try connecting to device -// 3 attempts are made, trying both STK and BL protocols. -static uint8_t connect(escDeviceInfo_t *pDeviceInfo) -{ - for (int try = 0; try < 3; try++) { -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (Stk_ConnectEx(pDeviceInfo) && signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imSK; + for (uint8_t I = 0; I < 3; ++I) { + #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { + CurrentInterfaceMode = imSK; return 1; + } else { + if (BL_ConnectEx(pDeviceInfo)) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } + } } -#endif -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if (BL_ConnectEx(pDeviceInfo)) { - if(signatureMatch(pDeviceInfo->signature, signaturesSilabs)) { - currentInterfaceMode = imSIL_BLB; + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; return 1; - } - if(signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imATM_BLB; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; return 1; } } -#endif + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo)) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } + #endif } return 0; } static serialPort_t *port; -static uint16_t crcIn, crcOut; -static uint8_t readByte(void) +static uint8_t ReadByte(void) { // need timeout? while (!serialRxBytesWaiting(port)); return serialRead(port); } -static uint8_t readByteCrc(void) +static uint8_16_u CRC_in; +static uint8_t ReadByteCrc(void) { - uint8_t b = readByte(); - crcIn = _crc_xmodem_update(crcIn, b); + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); return b; } -static void writeByte(uint8_t b) +static void WriteByte(uint8_t b) { serialWrite(port, b); } -static void writeByteCrc(uint8_t b) +static uint8_16_u CRCout; +static void WriteByteCrc(uint8_t b) { - writeByte(b); - crcOut = _crc_xmodem_update(crcOut, b); + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); } -// handle 4way interface on serial port -// esc4wayStart / esc4wayRelease in called internally -// 256 bytes buffer is allocated on stack -void esc4wayProcess(serialPort_t *serial) +void esc4wayProcess(serialPort_t *mspPort) { - uint8_t command; - uint16_t addr; - int inLen; - int outLen; - uint8_t paramBuf[256]; - uint8_t replyAck; - esc4wayStart(); + uint8_t ParamBuf[256]; + uint8_t ESC; + uint8_t I_PARAM_LEN; + uint8_t CMD; + uint8_t ACK_OUT; + uint8_16_u CRC_check; + uint8_16_u Dummy; + uint8_t O_PARAM_LEN; + uint8_t *O_PARAM; + uint8_t *InBuff; + ioMem_t ioMem; - port = serial; + port = mspPort; -#ifdef BEEPER + // Start here with UART Main loop + #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); -#endif + #endif + bool isExitScheduled = false; - esc4wayExitRequested = false; - while(!esc4wayExitRequested) { + while(1) { // restart looking for new sequence from host - crcIn = 0; - - uint8_t esc = readByteCrc(); - if(esc != cmd_Local_Escape) - continue; // wait for sync character + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); RX_LED_ON; - command = readByteCrc(); - addr = readByteCrc() << 8; - addr |= readByteCrc(); + Dummy.word = 0; + O_PARAM = &Dummy.bytes[0]; + O_PARAM_LEN = 1; + CMD = ReadByteCrc(); + ioMem.D_FLASH_ADDR_H = ReadByteCrc(); + ioMem.D_FLASH_ADDR_L = ReadByteCrc(); + I_PARAM_LEN = ReadByteCrc(); - inLen = readByteCrc(); - if(inLen == 0) - inLen = 0x100; // len ==0 -> param is 256B + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); - for(int i = 0; i < inLen; i++) - paramBuf[i] = readByteCrc(); + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); - readByteCrc(); readByteCrc(); // update input CRC - - outLen = 0; // output handling code will send single zero byte if necessary - replyAck = esc4wayAck_OK; - - if(crcIn != 0) // CRC of correct message == 0 - replyAck = esc4wayAck_I_INVALID_CRC; - - TX_LED_ON; - if (replyAck == esc4wayAck_OK) - replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); RX_LED_OFF; - // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) - if(!outLen) { - paramBuf[0] = 0; - outLen = 1; + if(CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; } - crcOut = 0; + if (ACK_OUT == ACK_OK) + { + // wtf.D_FLASH_ADDR_H=Adress_H; + // wtf.D_FLASH_ADDR_L=Adress_L; + ioMem.D_PTR_I = ParamBuf; + + switch(CMD) { + // ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + { + if (isMcuConnected()){ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + { + if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_SignOn()) { // SetStateDisconnected(); + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + } + if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; + } + break; + } + case cmd_ProtocolGetVersion: + { + // Only interface itself, no matter what Device + Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; + break; + } + + case cmd_InterfaceGetName: + { + // Only interface itself, no matter what Device + // O_PARAM_LEN=16; + O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; + break; + } + + case cmd_InterfaceGetVersion: + { + // Only interface itself, no matter what Device + // Dummy = iUart_res_InterfVersion; + O_PARAM_LEN = 2; + Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; + Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; + break; + } + case cmd_InterfaceExit: + { + isExitScheduled = true; + break; + } + case cmd_InterfaceSetMode: + { + #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (ParamBuf[0] == imSK) { + #endif + CurrentInterfaceMode = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_PARAM; + } + break; + } + + case cmd_DeviceReset: + { + if (ParamBuf[0] < escCount) { + // Channel may change here + selected_esc = ParamBuf[0]; + } + else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + BL_SendCMDRunRestartBootloader(&DeviceInfo); + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + break; + } + #endif + } + SET_DISCONNECTED; + break; + } + case cmd_DeviceInitFlash: + { + SET_DISCONNECTED; + if (ParamBuf[0] < escCount) { + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + selected_esc = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + O_PARAM_LEN = DeviceInfoSize; //4 + O_PARAM = (uint8_t *)&DeviceInfo; + if(Connect(&DeviceInfo)) { + DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; + } else { + SET_DISCONNECTED; + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + { + switch(CurrentInterfaceMode) + { + case imSK: + { + if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + { + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + { + Dummy.bytes[0] = ParamBuf[0]; + //Address = Page * 512 + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + ioMem.D_FLASH_ADDR_L = 0; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + //*** Device Memory Read Ops *** + case cmd_DeviceRead: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if(!Stk_ReadFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + case cmd_DeviceReadEEprom: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H = Adress_H; + wtf.D_FLASH_ADDR_L = Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + if (!BL_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if(ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + //*** Device Memory Write Ops *** + case cmd_DeviceWrite: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if (!BL_WriteFlash(&ioMem)) { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_WriteFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + } + break; + } + + case cmd_DeviceWriteEEprom: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + ACK_OUT = ACK_D_GENERAL_ERROR; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + case imATM_BLB: + { + if (BL_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (Stk_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + } + break; + } + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + } + } + } + + CRCout.word = 0; + + TX_LED_ON; serialBeginWrite(port); - writeByteCrc(cmd_Remote_Escape); - writeByteCrc(command); - writeByteCrc(addr >> 8); - writeByteCrc(addr & 0xff); - writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B - for(int i = 0; i < outLen % 0x100; i++) - writeByteCrc(paramBuf[i]); - writeByteCrc(replyAck); - writeByte(crcOut >> 8); - writeByte(crcOut & 0xff); + WriteByteCrc(cmd_Remote_Escape); + WriteByteCrc(CMD); + WriteByteCrc(ioMem.D_FLASH_ADDR_H); + WriteByteCrc(ioMem.D_FLASH_ADDR_L); + WriteByteCrc(O_PARAM_LEN); + + i=O_PARAM_LEN; + do { + WriteByteCrc(*O_PARAM); + O_PARAM++; + i--; + } while (i > 0); + + WriteByteCrc(ACK_OUT); + WriteByte(CRCout.bytes[1]); + WriteByte(CRCout.bytes[0]); serialEndWrite(port); - -#ifdef STM32F4 - delay(50); -#endif TX_LED_OFF; - } - - esc4wayRelease(); + if (isExitScheduled) { + esc4wayRelease(); + return; + } + }; } -// handle 4Way interface command -// command - received command, will be sent back in reply -// addr - from received header -// data - buffer used both for received parameters and returned data. -// Should be 256B long ; TODO - implement limited buffer size -// inLen - received input length -// outLen - size of data to return, max 256, initialized to zero -// single '\0' byte will be send if outLen is zero (protocol limitation) -esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen) -{ - ioMem_t ioMem; - ioMem.addr = addr; // default flash operation address - ioMem.data = data; // command data buffer is used for read and write commands - switch(command) { -// ******* Interface related stuff ******* - case cmd_InterfaceTestAlive: - if (!isMcuConnected()) - return esc4wayAck_OK; - - switch(currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - case imSIL_BLB: - if (BL_SendCMDKeepAlive()) - return esc4wayAck_OK; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - if (Stk_SignOn()) - return esc4wayAck_OK; - break; -#endif - } - setDisconnected(); - return esc4wayAck_D_GENERAL_ERROR; - - case cmd_ProtocolGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_PROTOCOL_VER; - *outLen = 1; - return esc4wayAck_OK; - - case cmd_InterfaceGetName: - // Only interface itself, no matter what Device - // outLen=16; - memcpy(data, SERIAL_4WAY_INTERFACE_NAME_STR, strlen(SERIAL_4WAY_INTERFACE_NAME_STR)); - *outLen = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); - return esc4wayAck_OK; - - case cmd_InterfaceGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_VERSION_HI; - data[1] = SERIAL_4WAY_VERSION_LO; - *outLen = 2; - return esc4wayAck_OK; - - case cmd_InterfaceExit: - esc4wayExitRequested = true; - return esc4wayAck_OK; - - case cmd_InterfaceSetMode: - switch(data[0]) { -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - case imSIL_BLB: - case imATM_BLB: -#endif -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - case imSK: -#endif - currentInterfaceMode = data[0]; - break; - default: - return esc4wayAck_I_INVALID_PARAM; - } - return esc4wayAck_OK; - - case cmd_DeviceReset: - if(data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - // Channel may change here - escSelected = data[0]; - switch (currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - case imATM_BLB: - BL_SendCMDRunRestartBootloader(); - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - break; -#endif - } - setDisconnected(); - return esc4wayAck_OK; - - case cmd_DeviceInitFlash: { - setDisconnected(); - if (data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - //Channel may change here - //ESC_LO or ESC_HI; Halt state for prev channel - int replyAck = esc4wayAck_OK; - escSelected = data[0]; - if(!connect(&deviceInfo)) { - setDisconnected(); - replyAck = esc4wayAck_D_GENERAL_ERROR; - } - deviceInfo.interfaceMode = currentInterfaceMode; - memcpy(data, &deviceInfo, sizeof(deviceInfo)); - *outLen = sizeof(deviceInfo); - return replyAck; - } - -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case cmd_DeviceEraseAll: - switch(currentInterfaceMode) { - case imSK: - if (!Stk_Chip_Erase()) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case cmd_DevicePageErase: - switch (currentInterfaceMode) { - case imSIL_BLB: - *outLen = 1; // return block number (from incoming packet) - // Address = Page * 512 - ioMem.addr = data[0] << 9; - if (!BL_PageErase(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -//*** Device Memory Read Ops *** - -// macros to mix interface with (single bit) memory type for switch statement -#define M_FLASH 0 -#define M_EEPROM 1 -#define INTFMEM(interface, memory) (((interface) << 1) | (memory)) - - case cmd_DeviceReadEEprom: - case cmd_DeviceRead: { - int len = data[0]; - if(len == 0) - len = 0x100; - ioMem.len = len; - switch(INTFMEM(currentInterfaceMode, (command == cmd_DeviceRead) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - if(!BL_ReadFlashSIL(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_FLASH): - if(!BL_ReadFlashATM(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if(!BL_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - // INTFMEM(imSIL_BLB, M_EEPROM): no eeprom on Silabs -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if(!Stk_ReadFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - *outLen = ioMem.len; - return esc4wayAck_OK; - } - -//*** Device Memory Write Ops *** - case cmd_DeviceWrite: - case cmd_DeviceWriteEEprom: - ioMem.len = inLen; - switch (INTFMEM(currentInterfaceMode, (command == cmd_DeviceWrite) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - case INTFMEM(imATM_BLB, M_FLASH): - if (!BL_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if (!BL_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if (!Stk_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#undef M_FLASH -#undef M_EEPROM -#undef INTFMEM - default: - return esc4wayAck_I_INVALID_CMD; - } - // should not get here -} #endif diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index 6c86b44c8..b1bba5ff6 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -15,127 +15,36 @@ * along with Cleanflight. If not, see . * 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 diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 01a598e8b..2a10dcdb4 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -24,271 +24,289 @@ #include #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 diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 9ed08db61..39cfaaa3d 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -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); diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index ca1012b28..a8f83bfae 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -15,12 +15,15 @@ * along with Cleanflight. If not, see . * 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; + diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 1c32e2c6b..342a0c8b0 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,38 +23,33 @@ #include #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 diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index 8f2fddcf4..80ca89826 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -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 From 669b8e72395b26ce28e87d4503eda50cc61c5e4f Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 21:09:32 +1000 Subject: [PATCH 12/19] Small fix for VCP port buffer send incomplete --- src/main/drivers/serial_usb_vcp.c | 7 +++---- src/main/vcpf4/usbd_cdc_vcp.c | 13 ++++++------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index f225dd62b..61b397996 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -90,7 +90,6 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) { UNUSED(instance); - if (!(usbIsConnected() && usbIsConfigured())) { return; } @@ -120,12 +119,12 @@ static bool usbVcpFlush(vcpPort_t *port) return false; } - uint32_t txed; + uint8_t txed = 0; uint32_t start = millis(); do { - txed = CDC_Send_DATA(port->txBuf, count); - } while (txed != count && (millis() - start < USB_TIMEOUT)); + txed += CDC_Send_DATA(&port->txBuf[txed], count-txed); + } while (txed < count && (millis() - start < USB_TIMEOUT)); return txed == count; } diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index f312e4a34..879b2edc6 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -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 From 187c41cfd13c7c50dbff6ea3b881fee02e99c786 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 21:12:07 +1000 Subject: [PATCH 13/19] Removed unnecessary include --- src/main/io/serial_4way.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index b065dfd91..9dd777b46 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -32,7 +32,6 @@ #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" -#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" From e7eb534401bb021efaae366dc78abcc83a53418e Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 17 Jul 2016 07:56:55 +1000 Subject: [PATCH 14/19] minor cleanup and a little extra safety --- src/main/drivers/serial_usb_vcp.c | 32 +++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 61b397996..8360b0068 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -77,13 +77,10 @@ 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) @@ -95,8 +92,10 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) } 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; @@ -119,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port) return false; } - uint8_t txed = 0; 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[txed], count-txed); - } 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) From 221f617799ba73841e56b37e99b662dbd7f9806c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:10:00 +0200 Subject: [PATCH 15/19] Fix for missing I gain (float) --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8de667012..e4702b30d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -185,7 +185,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat 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] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler, -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 @@ -320,7 +320,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin // is normalized to cycle time = 2048. // Prevent Accumulation uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS((int32_t)angleRate) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; From 2d6e0da773ee0a7bebecc600dcbe00e49dff643e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:12:46 +0200 Subject: [PATCH 16/19] Minor cleanup Fix for integer PIDC --- src/main/flight/mixer.c | 4 ---- src/main/flight/pid.c | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5b0ae97aa..a35e051e6 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -836,10 +836,6 @@ void mixTable(void) 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; - // 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++) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e4702b30d..0c08f64ce 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -323,7 +323,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; + 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 From 9c50ed8769c84d1a9770e198c2ad3e0a142e96c1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:35:04 +0200 Subject: [PATCH 17/19] anti desync depricated // Replaced by motor_accel_limit --- src/main/config/config.c | 2 +- src/main/flight/mixer.c | 38 ++++++++++++++++++++++++-------------- src/main/io/escservo.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index a9723ecaa..5479b5820 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -278,7 +278,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; - escAndServoConfig->escDesyncProtection = 0; + escAndServoConfig->accelerationLimitPercent = 15; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a35e051e6..e4043d4d0 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -830,17 +830,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); } + // Keep track for motor update timing + float motorDtms; + if (escAndServoConfig->accelerationLimitPercent) { + 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. + if (escAndServoConfig->accelerationLimitPercent) { + static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS]; + + // acceleration limit + float delta = motor[i] - lastFilteredMotor[i]; + const float maxDeltaPerMs = throttleRange * ((float)escAndServoConfig->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)) { @@ -859,19 +882,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 diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index b73f9920f..d97f41df4 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,5 +24,5 @@ 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 + uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 490476425..a888065fa 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -601,7 +601,7 @@ 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 } }, + { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.accelerationLimitPercent, .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 diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9a5b96ff5..952bc73b5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1226,7 +1226,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->rcYawRate8); serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(masterConfig.escAndServoConfig.escDesyncProtection); + serialize16(masterConfig.escAndServoConfig.accelerationLimitPercent); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1801,7 +1801,7 @@ static bool processInCommand(void) currentControlRateProfile->rcYawRate8 = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.rcSmoothInterval = read8(); - masterConfig.escAndServoConfig.escDesyncProtection = read16(); + masterConfig.escAndServoConfig.accelerationLimitPercent = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); From 37fd2e5adc4677b257d2a550ac8b3b59bc9cb543 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 23:35:56 +0200 Subject: [PATCH 18/19] Initial PID controller separation // Betaflight pidc for future development --- src/main/blackbox/blackbox.c | 3 +- src/main/config/config.c | 22 +++++----- src/main/flight/mixer.c | 15 +++---- src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 79 ++++++++++++++++++++++++++--------- src/main/flight/pid.h | 20 ++++++--- src/main/io/escservo.h | 1 - src/main/io/serial_cli.c | 14 ++++--- src/main/io/serial_msp.c | 10 ++--- src/main/mw.c | 4 +- src/main/sensors/battery.c | 2 +- src/main/sensors/battery.h | 1 - src/main/target/CC3D/target.h | 1 - 13 files changed, 113 insertions(+), 61 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9ab7e8a0b..96281b05f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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); diff --git a/src/main/config/config.c b/src/main/config/config.c index 5479b5820..be9d615ed 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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; @@ -224,7 +219,14 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 50; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; - pidProfile->dynamic_pid = 1; + pidProfile->vbatPidCompensation = 0; + + // 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 +280,6 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; - escAndServoConfig->accelerationLimitPercent = 15; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) @@ -311,7 +312,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 +452,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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e4043d4d0..aa9f0fc90 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -755,13 +755,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(); } @@ -836,9 +837,9 @@ void mixTable(void) throttleMax = throttleMax - (rollPitchYawMixRange / 2); } - // Keep track for motor update timing + // 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 (escAndServoConfig->accelerationLimitPercent) { + if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { static uint32_t previousMotorTime; uint32_t currentMotorTime = micros(); motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f; @@ -850,13 +851,13 @@ void mixTable(void) 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. - if (escAndServoConfig->accelerationLimitPercent) { + // 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)escAndServoConfig->accelerationLimitPercent / 100.0f); + const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f); float maxDelta = maxDeltaPerMs * motorDtms; if (delta > maxDelta) { // accelerating too hard motor[i] = lastFilteredMotor[i] + maxDelta; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4f4869b20..b76aafa1b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0c08f64ce..3907e3955 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -62,14 +62,12 @@ 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) { @@ -82,9 +80,7 @@ void pidResetErrorGyroState(void) for (axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; -#ifndef SKIP_PID_LUXFLOAT errorGyroIf[axis] = 0.0f; -#endif } } @@ -101,8 +97,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; @@ -133,6 +129,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++) { @@ -164,6 +179,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) @@ -173,7 +216,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate P component - PTerm = PTermScale * 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) { @@ -185,7 +228,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat 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] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler, -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 @@ -220,7 +263,7 @@ 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(DTermScale * 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); @@ -239,9 +282,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; @@ -385,13 +428,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; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a407aeb80..56767f871 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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,13 @@ 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 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 +76,14 @@ 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 + + // 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 diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index d97f41df4..66cd7db59 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -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 accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a888065fa..ec06f6fcc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 } }, - { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.accelerationLimitPercent, .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,14 @@ 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 } }, + { "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 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 952bc73b5..054eb4940 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1219,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.accelerationLimitPercent); + serialize16(currentProfile->pidProfile.accelerationLimitPercent); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1295,7 +1295,7 @@ static bool processInCommand(void) read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); + currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: @@ -1795,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.accelerationLimitPercent = read16(); + currentProfile->pidProfile.accelerationLimitPercent = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 05c8d1a08..91165131d 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,7 +184,7 @@ float calculateRate(int axis, int16_t rc) { angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; } - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_INTEGER) + 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) @@ -765,7 +765,7 @@ void subTaskMotorUpdate(void) previousMotorUpdateTime = startTime; } - mixTable(); + mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS filterServos(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 53e709648..70129156b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -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 { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 21197e07f..e52541db7 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -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 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index cee25b272..ab711c151 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -121,7 +121,6 @@ #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP -//#define SKIP_PID_LUXFLOAT #undef DISPLAY #undef SONAR #endif From 0ae5d9734e41bb26a459cd3e4eab30b04dc3dde8 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 18 Jul 2016 02:18:04 +0200 Subject: [PATCH 19/19] Add option to fully disable PID controller on zero throttle --- src/main/config/config.c | 1 + src/main/flight/mixer.c | 2 -- src/main/flight/pid.c | 15 +++++++++++---- src/main/flight/pid.h | 7 +++++++ src/main/io/serial_cli.c | 1 + src/main/mw.c | 6 ++++++ 6 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index be9d615ed..1b7c71479 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,6 +220,7 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; + pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->toleranceBand = 15; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aa9f0fc90..f8ac94b22 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -70,8 +70,6 @@ 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]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3907e3955..d23db67a3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 @@ -76,14 +77,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) void pidResetErrorGyroState(void) { - int axis; - - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; errorGyroIf[axis] = 0.0f; } } +void pidStabilisationState(pidStabilisationState_e pidControllerState) +{ + pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; +} + float getdT (void) { static float dT; @@ -269,6 +273,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc 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); @@ -409,6 +415,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina axisPID[axis] = PTerm + ITerm + DTerm; } + if (!pidStabilisationEnabled) axisPID[axis] = 0; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56767f871..64182f825 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -62,6 +62,11 @@ typedef enum { POSITIVE_ERROR } pidErrorPolarity_e; +typedef enum { + PID_STABILISATION_OFF = 0, + PID_STABILISATION_ON +} pidStabilisationState_e; + typedef struct pidProfile_s { uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) @@ -77,6 +82,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm 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 @@ -107,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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ec06f6fcc..768489a8d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -766,6 +766,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "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 } }, diff --git a/src/main/mw.c b/src/main/mw.c index 91165131d..2848abd40 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -518,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