int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return result;
}
// Altitude P-Controller
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
} else {
setVel = setVelocity;
}
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
result += errorVelocityI / 8192; // I in range +/-200
// D
result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
return result;
}
int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
uint32_t newBaroPID = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return newBaroPID;
}
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
errorAltitudeI += (pidProfile->I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
newBaroPID += errorAltitudeI / 1024; // I in range +/-200
// D
newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
return newBaroPID;
}
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return result;
}
// Altitude P-Controller
if(!ARMING_FLAG(ARMED))//Drona alt
{AltHold= EstAlt + 100;
//initialThrottleHold=1500;//Drona pras
}//default alt = 100cm;
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
//led2_op(false);
} else {
//led2_op(true);
setVel = setVelocity;
}
/*if((setVelocity>50))
{led1_op(true);}
else
{led1_op(false);}*/
/*if(setVel>50)
{
led0_op(true);
}
else
{
led0_op(false);
}
*/
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
if(ARMING_FLAG(ARMED))/*//Drona alt*/
{
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
}
else
{
errorVelocityI = 0;
}/*//Drona alt*/
errorVelocityI = constrain(errorVelocityI, -(8192 * 300), (8192 * 300));
result += errorVelocityI / 8192; // I in range +/-200
// D
result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
return result;
}