From 44e72d0a700bda4fe5f5d0cd2a064d83ca9e8900 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 30 Jul 2012 20:55:43 -0500 Subject: [PATCH] A big refactoring of stabilization.c to get rid of the two separate loops and move them into one big structure. This makes it easier to implement other modes. --- flight/Modules/Stabilization/stabilization.c | 454 +++++++++++-------- 1 file changed, 273 insertions(+), 181 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index f47aefe9f..87bee87cd 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -97,7 +97,7 @@ static float sin_lookup[360]; // Private functions static void stabilizationTask(void* parameters); static float ApplyPid(pid_type * pid, const float err, float dT); -static float bound(float val); +static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); @@ -116,10 +116,10 @@ int32_t StabilizationStart() // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue); - + StabilizationSettingsConnectCallback(SettingsUpdatedCb); SettingsUpdatedCb(StabilizationSettingsHandle()); - + // Start main task xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle); @@ -140,7 +140,7 @@ int32_t StabilizationInitialize() #if defined(DIAGNOSTICS) RateDesiredInitialize(); #endif - + return 0; } @@ -152,73 +152,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart) static void stabilizationTask(void* parameters) { UAVObjEvent ev; - + uint32_t timeval = PIOS_DELAY_GetRaw(); - + ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; GyrosData gyrosData; FlightStatusData flightStatus; - + SettingsUpdatedCb((UAVObjEvent *) NULL); - + // Main task loop ZeroPids(); while(1) { float dT; - + PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); - + // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } - + dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; timeval = PIOS_DELAY_GetRaw(); - + FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); - + #if defined(DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif - + #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; float q_desired[4]; float q_error[4]; float local_error[3]; - + // Essentially zero errors for anything in rate or none if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[0] = stabDesired.Roll; else rpy_desired[0] = attitudeActual.Roll; - + if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[1] = stabDesired.Pitch; else rpy_desired[1] = attitudeActual.Pitch; - + if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[2] = stabDesired.Yaw; else rpy_desired[2] = attitudeActual.Yaw; - + RPY2Quaternion(rpy_desired, q_desired); quat_inverse(q_desired); quat_mult(q_desired, &attitudeActual.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); - + #else // Simpler algorithm for CC, less memory float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, @@ -227,7 +227,6 @@ static void stabilizationTask(void* parameters) local_error[2] = fmodf(local_error[2] + 180, 360) - 180; #endif - gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); @@ -236,50 +235,98 @@ static void stabilizationTask(void* parameters) float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; - //Calculate desired rate + ActuatorDesiredGet(&actuatorDesired); + + // A flag to track which stabilization mode each axis is in + static uint8_t previous_mode[MAX_AXES] = {255,255,255}; + bool error = false; + + //Run the selected stabilization algorithm on each axis: for(uint8_t i=0; i< MAX_AXES; i++) { + // Check whether this axis mode needs to be reinitialized + bool reinit = (stabDesired.StabilizationMode[i] == previous_mode[i]); + previous_mode[i] = stabDesired.StabilizationMode[i]; + + // Apply the selected control law switch(stabDesired.StabilizationMode[i]) { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: + if(reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + + // Compute the inner loop + actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + + break; + + case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: + if(reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } + + // Compute the outer loop + rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Compute the inner loop + actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: + { + if(reinit) + vbar_integral[i] = 0; + rateDesiredAxis[i] = attitudeDesiredAxis[i]; - // Zero attitude and axis lock accumulators - pids[PID_ROLL + i].iAccumulator = 0; - axis_lock_accum[i] = 0; - break; + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT; + vbar_integral[i] = bound(vbar_integral[i], settings.VbarMaxAngle); - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: - { - float weak_leveling = local_error[i] * weak_leveling_kp; + // Command signal can indicate how much to disregard the gyro feedback (fast flips) + float gyro_gain = 1.0f; + if (vbar_gyros_suppress > 0) { + gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } - if(weak_leveling > weak_leveling_max) - weak_leveling = weak_leveling_max; - if(weak_leveling < -weak_leveling_max) - weak_leveling = -weak_leveling_max; + // Command signal is composed of stick input added to the gyro and virtual flybar + actuatorDesiredAxis[i] = rateDesiredAxis[i] * vbar_sensitivity[i] - + gyro_gain * (vbar_integral[i] * pids[PID_VBAR_ROLL + i].i + + gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p); - rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); - // Zero attitude and axis lock accumulators - pids[PID_ROLL + i].iAccumulator = 0; - axis_lock_accum[i] = 0; break; } - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); - - if(rateDesiredAxis[i] > settings.MaximumRate[i]) - rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) - rateDesiredAxis[i] = -settings.MaximumRate[i]; - - - axis_lock_accum[i] = 0; + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + { + if (reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + + float weak_leveling = local_error[i] * weak_leveling_kp; + weak_leveling = bound(weak_leveling, weak_leveling_max); + + // Compute desired rate as input biased towards leveling + rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + break; + } case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: + if (reinit) + pids[PID_RATE_ROLL + i].iAccumulator = 0; + if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = attitudeDesiredAxis[i]; @@ -287,59 +334,46 @@ static void stabilizationTask(void* parameters) } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; - if(axis_lock_accum[i] > max_axis_lock) - axis_lock_accum[i] = max_axis_lock; - else if(axis_lock_accum[i] < -max_axis_lock) - axis_lock_accum[i] = -max_axis_lock; - + axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - - if(rateDesiredAxis[i] > settings.MaximumRate[i]) - rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) - rateDesiredAxis[i] = -settings.MaximumRate[i]; + + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; - } - } - // Piro compensation rotates the virtual flybar by yaw step to keep it - // rotated to external world - if (vbar_piro_comp) { - const float F_PI = 3.14159f; - float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT); - float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT); - - float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; - float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; - - vbar_integral[1] = vbar_pitch; - vbar_integral[0] = vbar_roll; - } - - uint8_t shouldUpdate = 1; -#if defined(DIAGNOSTICS) - RateDesiredSet(&rateDesired); -#endif - - static bool rateRelayRunning[3] = {false, false, false}; - ActuatorDesiredGet(&actuatorDesired); - //Calculate desired command - for(uint32_t i = 0; i < MAX_AXES; i++) - { - switch(stabDesired.StabilizationMode[i]) - { - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: { + RelayTuningData relay; + RelayTuningGet(&relay); + + static bool rateRelayRunning[MAX_AXES]; + + // On first run initialize estimates to something reasonable + if(reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + rateRelayRunning[i] = false; + relay.Period[i] = 200; + relay.Gain[i] = 0; + } + // Replace the rate PID with a relay to measure the critical properties of this axis + // i.e. period and gain + + // Compute the outer loop + rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + RelayTuningSettingsData relaySettings; RelayTuningSettingsGet(&relaySettings); float error = rateDesiredAxis[i] - gyro_filtered[i]; float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; - actuatorDesiredAxis[i] = bound(command); - - RelayTuningData relay; - RelayTuningGet(&relay); + actuatorDesiredAxis[i] = bound(command,1.0f); static bool high = false; static portTickType lastHighTime; @@ -371,6 +405,8 @@ static void stabilizationTask(void* parameters) bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */ float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; + float this_gain = this_amplitude / relaySettings.Amplitude; + accumulated = 0; accum_sin = 0; accum_cos = 0; @@ -378,10 +414,10 @@ static void stabilizationTask(void* parameters) if(rateRelayRunning[i] == false) { rateRelayRunning[i] = true; relay.Period[i] = 200; - relay.Amplitude[i] = 0; + relay.Gain[i] = 0; } else { // Low pass filter each amplitude and period - relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (1 - AMPLITUDE_ALPHA); + relay.Gain[i] = relay.Gain[i] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); } lastHighTime = thisTime; @@ -394,110 +430,165 @@ static void stabilizationTask(void* parameters) break; } - case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: - case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: - case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: { - rateRelayRunning[i] = false; - float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); - actuatorDesiredAxis[i] = bound(command); - break; - } - case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - { - rateRelayRunning[i] = false; - // Track the angle of the virtual flybar which includes a slow decay - vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT; - if (vbar_integral[i] > settings.VbarMaxAngle) - vbar_integral[i] = settings.VbarMaxAngle; - if (-vbar_integral[i] < -settings.VbarMaxAngle) - vbar_integral[i] = -settings.VbarMaxAngle; + RelayTuningData relay; + RelayTuningGet(&relay); - // Command signal is composed of stick input added to the gyro and virtual flybar - float gyro_gain = 1.0f; - if (vbar_gyros_suppress > 0) { - gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); - gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + static bool rateRelayRunning[MAX_AXES]; + + // On first run initialize estimates to something reasonable + if(reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + rateRelayRunning[i] = false; + relay.Period[i] = 200; + relay.Gain[i] = 0; } - float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * ( - vbar_integral[i] * pids[PID_VBAR_ROLL + i].i + - gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p); - actuatorDesiredAxis[i] = bound(command); - break; + // Replace the rate PID with a relay to measure the critical properties of this axis + // i.e. period and gain + + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + float error = rateDesiredAxis[i] - gyro_filtered[i]; + float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; + actuatorDesiredAxis[i] = bound(command,1.0); + + static bool high = false; + static portTickType lastHighTime; + static portTickType lastLowTime; + portTickType thisTime = xTaskGetTickCount(); + + static float accum_sin, accum_cos; + static uint32_t accumulated = 0; + + const uint16_t DEGLITCH_TIME = 20; // ms + const float AMPLITUDE_ALPHA = 0.95; + const float PERIOD_ALPHA = 0.95; + + // Make sure the period can't go below limit + if (relay.Period[i] < DEGLITCH_TIME) + relay.Period[i] = DEGLITCH_TIME; + + // Project the error onto a sine and cosine of the same frequency + // to accumulate the average amplitude + float dT = thisTime - lastHighTime; + uint32_t phase = 360 * dT / relay.Period[i]; + if(phase >= 360) + phase = 1; + accum_sin += sin_lookup[phase] * error; + accum_cos += sin_lookup[(phase + 90) % 360] * error; + accumulated ++; + + // Make susre we've had enough time since last transition then check for a change in the output + bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; + if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */ + float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; + float this_gain = this_amplitude / relaySettings.Amplitude; + + accumulated = 0; + accum_sin = 0; + accum_cos = 0; + + if(rateRelayRunning[i] == false) { + rateRelayRunning[i] = true; + relay.Period[i] = 200; + relay.Gain[i] = 0; + } else { + // Low pass filter each amplitude and period + relay.Gain[i] = relay.Gain[i] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } + lastHighTime = thisTime; + high = true; + RelayTuningSet(&relay); + } else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */ + lastLowTime = thisTime; + high = false; + } } + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - rateRelayRunning[i] = false; - switch (i) - { - case ROLL: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_ROLL].iAccumulator = 0; - pids[PID_ROLL].iAccumulator = 0; - break; - case PITCH: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_PITCH].iAccumulator = 0; - pids[PID_PITCH].iAccumulator = 0; - break; - case YAW: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]); - shouldUpdate = 1; - pids[PID_RATE_YAW].iAccumulator = 0; - pids[PID_YAW].iAccumulator = 0; - break; - } + actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f); + break; + default: + error = true; break; - } } + // Piro compensation rotates the virtual flybar by yaw step to keep it + // rotated to external world + if (vbar_piro_comp) { + const float F_PI = 3.14159f; + float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT); + float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT); + + float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0]; + float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0]; + + vbar_integral[1] = vbar_pitch; + vbar_integral[0] = vbar_roll; + } + +#if defined(DIAGNOSTICS) + RateDesiredSet(&rateDesired); +#endif + // Save dT actuatorDesired.UpdateTime = dT * 1000; + actuatorDesired.Throttle = stabDesired.Throttle; - if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) - shouldUpdate = 0; - - if(shouldUpdate) - { - actuatorDesired.Throttle = stabDesired.Throttle; - if(dT > 15) - actuatorDesired.NumLongUpdates++; + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); + } else { + // Force all axes to reinitialize when engaged + for(uint8_t i=0; i< MAX_AXES; i++) + previous_mode[i] = 255; } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && stabDesired.Throttle < 0) || - !shouldUpdate) + (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { - ZeroPids(); + // Force all axes to reinitialize when engaged + for(uint8_t i=0; i< MAX_AXES; i++) + previous_mode[i] = 255; } - - // Clear alarms - AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + // Clear or set alarms. Done like this to prevent toggline each cycle + // and hammering system alarms + if (error) + AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); + else + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } } +/** + * Update one of the PID structures with the input error and timestep + * @param pid Pointer to the PID structure + * @param[in] err The error on for this controller + * @param[in] dT The time step since the last update + */ float ApplyPid(pid_type * pid, const float err, float dT) { float diff = (err - pid->lastErr); pid->lastErr = err; - + // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); - if(pid->iAccumulator > (pid->iLim * 1000.0f)) { - pid->iAccumulator = pid->iLim * 1000.0f; - } else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) { - pid->iAccumulator = -pid->iLim * 1000.0f; - } + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); } +/** + * Clear the accumulators and derivatives for all the axes + */ static void ZeroPids(void) { for(int8_t ct = 0; ct < PID_MAX; ct++) { @@ -512,12 +603,12 @@ static void ZeroPids(void) /** * Bound input value between limits */ -static float bound(float val) +static float bound(float val, float range) { - if(val < -1.0f) { - val = -1.0f; - } else if(val > 1.0f) { - val = 1.0f; + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; } return val; } @@ -525,40 +616,40 @@ static float bound(float val) static void SettingsUpdatedCb(UAVObjEvent * ev) { StabilizationSettingsGet(&settings); - + // Set the roll rate PID constants pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; - + // Set the pitch rate PID constants pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; - + // Set the yaw rate PID constants pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; - + // Set the roll attitude PI constants pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; - + // Set the pitch attitude PI constants pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; - + // Set the yaw attitude PI constants pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; - + // Set the roll attitude PI constants pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; @@ -570,23 +661,23 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) // Set the yaw attitude PI constants pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP]; pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI]; - + // Need to store the vbar sensitivity vbar_sensitivity[0] = settings.VbarSensitivity[0]; vbar_sensitivity[1] = settings.VbarSensitivity[1]; vbar_sensitivity[2] = settings.VbarSensitivity[2]; - + // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; max_axislock_rate = settings.MaxAxisLockRate; - + // Settings for weak leveling weak_leveling_kp = settings.WeakLevelingKp; weak_leveling_max = settings.MaxWeakLevelingRate; - + // Whether to zero the PID integrals while throttle is low lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; - + // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant // based on a time (in ms) rather than a fixed multiplier. The error between @@ -597,7 +688,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) gyro_alpha = 0; // not trusting this to resolve to 0 else gyro_alpha = expf(-fakeDt / settings.GyroTau); - + // Compute time constant for vbar decay term based on a tau vbar_decay = expf(-fakeDt / settings.VbarTau); vbar_gyros_suppress = settings.VbarGyroSuppress; @@ -609,3 +700,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) * @} * @} */ +