void ControlClass::update()
{
prevError = error;
error.phi = PICInterface.rx.pitch - AHRS.orientation.phi;
error.psi = PICInterface.rx.roll - AHRS.orientation.psi;
error.theta = PICInterface.rx.yaw - AHRS.calibratedData.r; //Yaw uses rate control instead of angle
differential.phi = (error.phi - prevError.phi) / Timer.dt;
differential.psi = (error.psi - prevError.psi) / Timer.dt;
differential.theta = (error.theta - prevError.theta) / Timer.dt;
integral.phi += error.phi * Timer.dt;
integral.psi += error.psi * Timer.dt;
integral.theta += error.theta * Timer.dt;
constrain_ (&integral.phi, 0.5);
constrain_ (&integral.psi, 0.5);
constrain_ (&integral.theta, 0.5);
pid.phi = pidconstants.kp * error.phi + pidconstants.kd * differential.phi + pidconstants.ki * integral.phi;
pid.psi = pidconstants.kp * error.psi + pidconstants.kd * differential.psi + pidconstants.ki * integral.psi;
pid.theta = yawPIDConstants.kp * error.theta;// + yawPIDConstants.kd * differential.theta;
PICInterface.pwmwidths.frontleft = ((PICInterface.rx.throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - pid.phi - pid.psi + pid.theta;
PICInterface.pwmwidths.frontright = ((PICInterface.rx.throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + pid.phi - pid.psi - pid.theta;
PICInterface.pwmwidths.rearright = ((PICInterface.rx.throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + pid.phi + pid.psi + pid.theta;
PICInterface.pwmwidths.rearleft = ((PICInterface.rx.throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - pid.phi + pid.psi - pid.theta;
PICInterface.setPWM();
}
void ControlClass::altitudeControl_() {
altitudePID.prevError = altitudePID.error;
altitudePID.error = targetAltitude_ - AHRS.calibratedData.altitude;
altitudePID.differential = (altitudePID.error - altitudePID.prevError) / Timer.dt;
altitudePID.output = altitudePID.constants.kp * altitudePID.error + altitudePID.differential * altitudePID.error;
constrain_(&altitudePID.output, 500);
}
