diff --git a/baseflight.uvproj b/baseflight.uvproj index 51e4d102c5..a104661ca7 100755 --- a/baseflight.uvproj +++ b/baseflight.uvproj @@ -197,7 +197,7 @@ 1 1 1 - 0 + 1 0 "Cortex-M3" @@ -990,7 +990,7 @@ 1 1 0 - 0 + 1 1 0 0 @@ -1003,7 +1003,7 @@ 1 1 1 - 0 + 1 0 "Cortex-M3" diff --git a/src/board.h b/src/board.h index 4bc386af4d..82b99dc783 100755 --- a/src/board.h +++ b/src/board.h @@ -75,6 +75,7 @@ typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and a typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data +typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype typedef struct sensor_t { diff --git a/src/cli.c b/src/cli.c index e95efeb2f1..22237ab828 100644 --- a/src/cli.c +++ b/src/cli.c @@ -133,6 +133,7 @@ const clivalue_t valueTable[] = { { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, { "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 }, { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 }, + { "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 }, { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 }, @@ -1013,7 +1014,7 @@ void cliProcess(void) else uartPrint("ERR: Unknown command, try 'help'"); - *cliBuffer = '\0'; + memset(cliBuffer, 0, sizeof(cliBuffer)); bufferIndex = 0; // 'exit' will reset this flag, so we don't need to print prompt again diff --git a/src/config.c b/src/config.c index 9632f69c6e..e580b2816c 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static uint8_t EEPROM_CONF_VERSION = 47; +static uint8_t EEPROM_CONF_VERSION = 48; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -84,6 +84,7 @@ void readEEPROM(void) } cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR + setPIDController(cfg.pidController); } void writeEEPROM(uint8_t b, uint8_t updateProfile) @@ -202,6 +203,7 @@ static void resetConf(void) mcfg.serial_baudrate = 115200; mcfg.looptime = 3500; + cfg.pidController = 0; cfg.P8[ROLL] = 40; cfg.I8[ROLL] = 30; cfg.D8[ROLL] = 23; diff --git a/src/mw.c b/src/mw.c index 6de1e0aed0..38ad20d7c5 100755 --- a/src/mw.c +++ b/src/mw.c @@ -1,7 +1,7 @@ #include "board.h" #include "mw.h" -// October 2012 V2.1-dev +// June 2013 V2.2-dev flags_t f; int16_t debug[4]; @@ -24,6 +24,10 @@ int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE uint16_t rssi; // range: [0;1023] rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) +static void pidMultiWii(void); +static void pidRewrite(void); +pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii + uint8_t dynP8[3], dynI8[3], dynD8[3]; uint8_t rcOptions[CHECKBOXITEMS]; @@ -132,6 +136,7 @@ void annexCode(void) prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500; } dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100; + dynI8[axis] = (uint16_t) cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100; if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; @@ -273,25 +278,160 @@ static void mwVario(void) } +static int32_t errorGyroI[3] = { 0, 0, 0 }; +static int32_t errorAngleI[2] = { 0, 0 }; + +static void pidMultiWii(void) +{ + int axis, prop; + int16_t error, errorAngle; + int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; + static int16_t lastGyro[3] = { 0, 0, 0 }; + static int16_t delta1[3], delta2[3]; + int16_t deltaSum; + int16_t delta; + + // **** PITCH & ROLL & YAW PID **** + prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500] + for (axis = 0; axis < 3; axis++) { + if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC + // 50 degrees max inclination + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; + PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result + PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); + + errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp + ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; + } + if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis + error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; + error -= gyroData[axis]; + + PTermGYRO = rcCommand[axis]; + + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp + if (abs(gyroData[axis]) > 640) + errorGyroI[axis] = 0; + ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; + } + if (f.HORIZON_MODE && axis < 2) { + PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500; + ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500; + } else { + if (f.ANGLE_MODE && axis < 2) { + PTerm = PTermACC; + ITerm = ITermACC; + } else { + PTerm = PTermGYRO; + ITerm = ITermGYRO; + } + } + + PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastGyro[axis] = gyroData[axis]; + deltaSum = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation + axisPID[axis] = PTerm + ITerm - DTerm; + } +} + +#define GYRO_I_MAX 256 + +static void pidRewrite(void) +{ + int16_t errorAngle; + int axis; + int16_t delta, deltaSum; + static int16_t delta1[3], delta2[3]; + int16_t PTerm, ITerm, DTerm; + static int16_t lastError[3] = { 0, 0, 0 }; + int16_t AngleRateTmp, RateError; + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + // -----Get the desired angle rate depending on flight mode + if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC + // calculate error and limit the angle to 50 degrees max inclination + errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here + } + if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) + AngleRateTmp = (((int32_t) (cfg.yawRate + 27) * rcCommand[2]) >> 5); + } else { + if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID + AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4; + if (f.HORIZON_MODE) { + // mix up angle error to desired AngleRateTmp to add a little auto-level feel + AngleRateTmp += ((int32_t) errorAngle * cfg.I8[PIDLEVEL]) >> 8; + } + } else { // it's the ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = ((int32_t) errorAngle * cfg.P8[PIDLEVEL]) >> 4; + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + RateError = AngleRateTmp - gyroData[axis]; + + // -----calculate P component + PTerm = ((int32_t)RateError * cfg.P8[axis]) >> 7; + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // 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. + errorGyroI[axis] = errorGyroI[axis] + (((int32_t)RateError * cycleTime) >> 11) * cfg.I8[axis]; + + // 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 + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13); + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastError[axis] = RateError; + + // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6; + // add moving average here to reduce noise + deltaSum = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + DTerm = (deltaSum * cfg.D8[axis]) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } +} + +void setPIDController(int type) +{ + switch (type) { + case 0: + default: + pid_controller = pidMultiWii; + break; + case 1: + pid_controller = pidRewrite; + break; + } +} + void loop(void) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos uint8_t stTmp = 0; - uint8_t axis, i; - int16_t error, errorAngle; - int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; - static int16_t errorGyroI[3] = { 0, 0, 0 }; - static int16_t errorAngleI[2] = { 0, 0 }; - int16_t delta; - static int16_t lastGyro[3] = { 0, 0, 0 }; - static int16_t delta1[3], delta2[3]; - int16_t deltaSum; + int i; static uint32_t rcTime = 0; static int16_t initialThrottleHold; static uint32_t loopTime; uint16_t auxState = 0; - int16_t prop; static uint8_t GPSNavReset = 1; // this will return false if spektrum is disabled. shrug. @@ -703,51 +843,8 @@ void loop(void) } } - // **** PITCH & ROLL & YAW PID **** - prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500] - for (axis = 0; axis < 3; axis++) { - if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC - // 50 degrees max inclination - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; - PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result - PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); - - errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp - ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; - } - if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis - error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; - error -= gyroData[axis]; - - PTermGYRO = rcCommand[axis]; - - errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp - if (abs(gyroData[axis]) > 640) - errorGyroI[axis] = 0; - ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; - } - if (f.HORIZON_MODE && axis < 2) { - PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500; - ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500; - } else { - if (f.ANGLE_MODE && axis < 2) { - PTerm = PTermACC; - ITerm = ITermACC; - } else { - PTerm = PTermGYRO; - ITerm = ITermGYRO; - } - } - - PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation - delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyro[axis] = gyroData[axis]; - deltaSum = delta1[axis] + delta2[axis] + delta; - delta2[axis] = delta1[axis]; - delta1[axis] = delta; - DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation - axisPID[axis] = PTerm + ITerm - DTerm; - } + // PID - note this is function pointer set by setPIDController() + pid_controller(); mixTable(); writeServos(); diff --git a/src/mw.h b/src/mw.h index 551548f1d3..0b6623159d 100755 --- a/src/mw.h +++ b/src/mw.h @@ -145,6 +145,7 @@ enum { }; typedef struct config_t { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 uint8_t P8[PIDITEMS]; uint8_t I8[PIDITEMS]; uint8_t D8[PIDITEMS]; @@ -371,6 +372,7 @@ extern sensor_t gyro; extern baro_t baro; // main +void setPIDController(int type); void loop(void); // IMU