From 8565dfbcc3c415b20775101d7a8452b2cc7991d7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 17:52:53 -0500 Subject: [PATCH] Factor the relay tuning out of the main stabilization.c file into it's own tool. --- .../Modules/Stabilization/inc/relay_tuning.h | 40 ++++ flight/Modules/Stabilization/relay_tuning.c | 179 ++++++++++++++++ flight/Modules/Stabilization/stabilization.c | 192 ++---------------- 3 files changed, 239 insertions(+), 172 deletions(-) create mode 100644 flight/Modules/Stabilization/inc/relay_tuning.h create mode 100644 flight/Modules/Stabilization/relay_tuning.c diff --git a/flight/Modules/Stabilization/inc/relay_tuning.h b/flight/Modules/Stabilization/inc/relay_tuning.h new file mode 100644 index 000000000..d788ae62e --- /dev/null +++ b/flight/Modules/Stabilization/inc/relay_tuning.h @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Relay tuning controller + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * @{ + * + * @file relay_tuning.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Attitude stabilization module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef RELAY_TUNING_H +#define RELAY_TUNING_H + +int stabilization_relay_init(); +int stabilization_relay_rate(float err, float *output, int axis, bool reinit); + +#endif \ No newline at end of file diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c new file mode 100644 index 000000000..ad4c7e064 --- /dev/null +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -0,0 +1,179 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Relay tuning controller + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" + * @{ + * + * @file stabilization.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Attitude stabilization module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "stabilization.h" +#include "stabilizationsettings.h" +#include "actuatordesired.h" +#include "ratedesired.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" +#include "stabilizationdesired.h" +#include "attitudeactual.h" +#include "gyros.h" +#include "flightstatus.h" +#include "manualcontrol.h" // Just to get a macro +#include "CoordinateConversions.h" + +//! Private variables +static float *sin_lookup; // TODO: Move this to flash +static const int SIN_RESOLUTION = 180; + +//! Private methods +static float sin_l(int angle); + +#define MAX_AXES 3 + +int stabilization_relay_init() +{ + sin_lookup = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); + if (sin_lookup == NULL) + return -1; + + for(uint32_t i = 0; i < 180; i++) + sin_lookup[i] = sinf((float)i * 2 * M_PI / 360.0f); + + return 0; +} + +/** + * Apply a step function for the stabilization controller and monitor the + * result + * + * Used to Replace the rate PID with a relay to measure the critical properties of this axis + * i.e. period and gain + */ +int stabilization_relay_rate(float error, float *output, int axis, bool reinit) +{ + RelayTuningData relay; + RelayTuningGet(&relay); + + static bool high = false; + static portTickType lastHighTime; + static portTickType lastLowTime; + + 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; + + portTickType thisTime = xTaskGetTickCount(); + + static bool rateRelayRunning[MAX_AXES]; + + // On first run initialize estimates to something reasonable + if(reinit) { + rateRelayRunning[axis] = false; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + + accum_sin = 0; + accum_cos = 0; + accumulated = 0; + + // These should get reinitialized anyway + high = true; + lastHighTime = thisTime; + lastLowTime = thisTime; + RelayTuningSet(&relay); + } + + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + // Compute output, simple threshold on error + *output = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; + + /**** The code below here is to estimate the properties of the oscillation ****/ + + // Make sure the period can't go below limit + if (relay.Period[axis] < DEGLITCH_TIME) + relay.Period[axis] = DEGLITCH_TIME; + + // Project the error onto a sine and cosine of the same frequency + // to accumulate the average amplitude + int dT = thisTime - lastHighTime; + uint32_t phase = 360 * dT / relay.Period[axis]; + if(phase >= 360) + phase = 1; + accum_sin += sin_l(phase) * error; + accum_cos += sin_l(phase + 90) * error; + accumulated ++; + + // Make sure 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[axis] == false) { + rateRelayRunning[axis] = true; + relay.Period[axis] = 200; + relay.Gain[axis] = 0; + } else { + // Low pass filter each amplitude and period + relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } + lastHighTime = thisTime; + high = true; + RelayTuningSet(&relay); + } else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */ + lastLowTime = thisTime; + high = false; + } + + return 0; +} + + +/** + * Uses the lookup table to calculate sine (angle is in degrees) + * @param[in] angle in degrees + * @returns sin(angle) + */ +static float sin_l(int angle) { + angle = angle % 360; + if (angle > 180) + return - sin_lookup[angle-180]; + else + return sin_lookup[angle]; +} + diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index a37b1aca5..4ffe9b944 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -45,6 +45,9 @@ #include "manualcontrol.h" // Just to get a macro #include "CoordinateConversions.h" +// Includes for various stabilization algorithms +#include "relay_tuning.h" + // Private constants #define MAX_QUEUE_SIZE 1 @@ -91,9 +94,6 @@ pid_type pids[PID_MAX]; int8_t vbar_gyros_suppress; bool vbar_piro_comp = false; -// TODO: Move this to flash -static float sin_lookup[180]; - // Private functions static void stabilizationTask(void* parameters); static float ApplyPid(pid_type * pid, const float err, float dT); @@ -101,15 +101,6 @@ static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); -//! Uses the lookup table to calculate sine (angle is in degrees) -static float sin_l(int angle) { - angle = angle % 360; - if (angle > 180) - return - sin_lookup[angle-180]; - else - return sin_lookup[angle]; -} - /** * Module initialization */ @@ -119,8 +110,8 @@ int32_t StabilizationStart() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - for(uint32_t i = 0; i < 180; i++) - sin_lookup[i] = sinf((float)i * 2 * M_PI / 360.0f); + // This prepares this optional algorithm + stabilization_relay_init(); // Listen for updates. // AttitudeActualConnectQueue(queue); @@ -366,171 +357,28 @@ static void stabilizationTask(void* parameters) break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: + // Store to rate desired variable for storing to UAVO + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); + + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: - { - RelayTuningData relay; - RelayTuningGet(&relay); - - static bool rateRelayRunning[MAX_AXES]; - - // On first run initialize estimates to something reasonable - if(reinit) { + 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 + // Compute the outer loop like attitude mode 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]); + // Run the relay controller which also estimates the oscillation parameters + stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0); - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); - float error = rateDesiredAxis[i] - gyro_filtered[i]; - float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; - actuatorDesiredAxis[i] = bound(command,1.0f); - - 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_l(phase) * error; - accum_cos += sin_l(phase + 90) * 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_RELAYRATE: - { - 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 - - // 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_l(phase) * error; - accum_cos += sin_l(phase + 90) * 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: