void Initial_Math(void)
{
RollRate.Kp=RollRate.Kp/100.0f;//all reduce 100;
RollRate.Ki=RollRate.Ki/1000.0f;
RollRate.Kd=RollRate.Kd/100.0f;
PitchRate.Kp=PitchRate.Kp/100.0f;
PitchRate.Ki=PitchRate.Ki/1000.0f;
PitchRate.Kd=PitchRate.Kd/100.0f;
YawRate.Kp=YawRate.Kp/100.0f;
YawRate.Ki=YawRate.Ki/1000.0f;
YawRate.Kd=YawRate.Kd/100.0f;
Stabilize_Roll.Kp=Stabilize_Roll.Kp/100.0f;
Stabilize_Roll.Ki=0.0f;
Stabilize_Roll.Kd=0.0f;
Stabilize_Pitch.Kp=Stabilize_Pitch.Kp/100.0f;
Stabilize_Pitch.Ki=0.0f;
Stabilize_Pitch.Kd=0.0f;
Stabilize_Yaw.Kp=Stabilize_Yaw.Kp/100.0f;
Stabilize_Yaw.Ki=0.0f;
Stabilize_Yaw.Kd=0.0f;
pidSetLowPassEnable(&RollRate);
pidSetLowPassEnable(&PitchRate);
pidSetLowPassEnable(&YawRate);
pidSetLowPassEnable(&Climb);
pidSetLowPassEnable(&AutoHigh_THR);
pidSetIntegralLimit(&Climb , 100.0f);
pidSetIntegralLimit(&AutoHigh_THR , 10.0f);
pidSetIntegralLimit(&RollRate,100.0f);
pidSetIntegralLimit(&PitchRate,100.0f);
pidSetIntegralLimit(&YawRate,100.0f);
/*
RollRate.Kp=0.15f;//all reduce 100;
RollRate.Ki=0.002f;
RollRate.Kd=3;
PitchRate.Kp=0.15f;
PitchRate.Ki=0.002f;
PitchRate.Kd=3;
YawRate.Kp=1.8f;
YawRate.Ki=0.004f;
YawRate.Kd=0.0f;
Stabilize_Roll.Kp=2.08f;
Stabilize_Roll.Ki=0.0f;
Stabilize_Roll.Kd=0.0f;
Stabilize_Pitch.Kp=2.08f;
Stabilize_Pitch.Ki=0.0f;
Stabilize_Pitch.Kd=0.0f;
Stabilize_Yaw.Kp=1.3f;
Stabilize_Yaw.Ki=0.0f;
Stabilize_Yaw.Kd=0.0f;
*/
pid_flag=1;
}
void controllerInit(void)
{
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
}
void attitudeControllerInit(const float updateDt)
{
if(isInit)
return;
//TODO: get parameters from configuration manager instead
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD,
updateDt, ATTITUDE_RATE, ATTITUDE_RATE_LPF_CUTOFF_FREQ, ATTITUDE_RATE_LPF_ENABLE);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, updateDt,
ATTITUDE_RATE, ATTITUDE_LPF_CUTOFF_FREQ, ATTITUDE_LPF_ENABLE);
pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT);
isInit = true;
}
void controllerInit()
{
if(isInit)
return;
//TODO: get parameters from configuration manager instead
pidInit(&pidRollRate, 0, PID_ROLL_RATE_KP, PID_ROLL_RATE_KI, PID_ROLL_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidPitchRate, 0, PID_PITCH_RATE_KP, PID_PITCH_RATE_KI, PID_PITCH_RATE_KD, IMU_UPDATE_DT);
pidInit(&pidYawRate, 0, PID_YAW_RATE_KP, PID_YAW_RATE_KI, PID_YAW_RATE_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRollRate, PID_ROLL_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitchRate, PID_PITCH_RATE_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYawRate, PID_YAW_RATE_INTEGRATION_LIMIT);
pidInit(&pidRoll, 0, PID_ROLL_KP, PID_ROLL_KI, PID_ROLL_KD, IMU_UPDATE_DT);
pidInit(&pidPitch, 0, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, IMU_UPDATE_DT);
pidInit(&pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, IMU_UPDATE_DT);
pidSetIntegralLimit(&pidRoll, PID_ROLL_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT);
pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT);
isInit = true;
}
static void stabilizerAltHoldUpdate() {
// Get the time
float altitudeError = 0;
static float altitudeError_i = 0;
float instAcceleration = 0;
float deltaVertSpeed = 0;
static uint32_t timeStart = 0;
static uint32_t timeCurrent = 0;
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
// Compute the altitude
altitudeError = aslRaw - estimatedAltitude;
altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError));
instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi;
deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError;
estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError;
vSpeedComp += deltaVertSpeed;
aslLong = estimatedAltitude; // Override aslLong
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit);
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging()) {
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold) {
// Set to current altitude
altHoldTarget = estimatedAltitude;
// Set the start time
timeStart = xTaskGetTickCount();
timeCurrent = 0;
// Reset PID controller
pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT);
// Cache last integral term for reuse after pid init
// const float pre_integral = hoverPID.integ;
// Reset the PID controller for the hover controller. We want zero vertical velocity
pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT);
pidSetIntegralLimit(&hoverPID, 3);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
// hoverPID.integ = pre_integral;
}
// In altitude hold mode
if (altHold) {
// Get current time
timeCurrent = xTaskGetTickCount();
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false);
// Get the PID value for the hover
float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true);
float thrustValFloat;
// Use different weights depending on time into altHold mode
if (timeCurrent > 150) {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
} else {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal;
}
// float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac);
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));
// i part should compensate for voltage drop
} else {
altHoldTarget = 0.0;
altHoldErr = 0.0;
}
}
