void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float horizonLevelStrength = 0.0f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// (convert 0-100 range to 0.0-1.0 range)
horizonLevelStrength = (float)calcHorizonLevelStrength(rxConfig->midrc, pidProfile->horizon_tilt_effect,
pidProfile->horizon_tilt_mode, pidProfile->D8[PIDLEVEL]) / 100.0f;
}
// ----------PID controller----------
for (int axis = 0; axis < 3; axis++) {
const uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
float angleRate;
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;
} else {
// control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
// multiplication of rcCommand corresponds to changing the sticks scaling here
#ifdef GPS
const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#else
const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode
angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode
// mix in errorAngle to desired angleRate to add a little auto-level feel.
// horizonLevelStrength has been scaled to the stick input
angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
}
// --------low-level gyro-based PID. ----------
const float gyroRate = luxGyroScale * gyroADCf[axis] * gyro.scale;
axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);
//axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
}
}
void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
pidFilterIsSetCheck(pidProfile);
float horizonLevelStrength;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the most deflected stick position
const int32_t stickPosAil = ABS(getRcStickDeflection(ROLL, rxConfig->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(PITCH, rxConfig->midrc));
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
}
}
// ----------PID controller----------
for (int axis = 0; axis < 3; axis++) {
const uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
float angleRate;
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;
} else {
// control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
// multiplication of rcCommand corresponds to changing the sticks scaling here
#ifdef GPS
const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#else
const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
- attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode
angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode
// mix in errorAngle to desired angleRate to add a little auto-level feel.
// horizonLevelStrength has been scaled to the stick input
angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
}
// --------low-level gyro-based PID. ----------
const float gyroRate = luxGyroScale * gyroADC[axis] * gyro.scale;
axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);
//axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
}
}
