From 9ce7d9b4ac35c8f9ae19d5329c5c478a39f13cde Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 29 Jul 2012 15:10:55 -0500 Subject: [PATCH 01/43] Add UAVOs for relay tuning --- flight/Revolution/UAVObjects.inc | 2 ++ .../src/plugins/uavobjects/uavobjects.pro | 4 ++++ shared/uavobjectdefinition/relaytuning.xml | 11 +++++++++++ shared/uavobjectdefinition/relaytuningsettings.xml | 10 ++++++++++ 4 files changed, 27 insertions(+) create mode 100644 shared/uavobjectdefinition/relaytuning.xml create mode 100644 shared/uavobjectdefinition/relaytuningsettings.xml diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 462acfc34..bd0612412 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -62,6 +62,8 @@ UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += positiondesired UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += relaytuning +UAVOBJSRCFILENAMES += relaytuningsettings UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += sonaraltitude UAVOBJSRCFILENAMES += stabilizationdesired diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 720d36ebd..d252c654a 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -63,6 +63,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/velocityactual.h \ $$UAVOBJECT_SYNTHETICS/guidancesettings.h \ $$UAVOBJECT_SYNTHETICS/positiondesired.h \ + $$UAVOBJECT_SYNTHETICS/relaytuning.h \ + $$UAVOBJECT_SYNTHETICS/relaytuningsettings.h \ $$UAVOBJECT_SYNTHETICS/ratedesired.h \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ @@ -125,6 +127,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \ $$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \ $$UAVOBJECT_SYNTHETICS/positiondesired.cpp \ + $$UAVOBJECT_SYNTHETICS/relaytuningsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/relaytuning.cpp \ $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \ $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml new file mode 100644 index 000000000..f13bfc9f2 --- /dev/null +++ b/shared/uavobjectdefinition/relaytuning.xml @@ -0,0 +1,11 @@ + + + The input to the relay tuning. + + + + + + + + diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml new file mode 100644 index 000000000..46a5bc124 --- /dev/null +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -0,0 +1,10 @@ + + + Setting to run a relay tuning algorithm + + + + + + + From 17a0d3ebb41b0a8b5cd0819b5fb10da5ee0336e5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 29 Jul 2012 15:21:14 -0500 Subject: [PATCH 02/43] Add a relay stabilization mode --- flight/Modules/ManualControl/manualcontrol.c | 3 +++ flight/Modules/Stabilization/stabilization.c | 12 ++++++++++++ shared/uavobjectdefinition/manualcontrolsettings.xml | 6 +++--- shared/uavobjectdefinition/stabilizationdesired.xml | 2 +- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index cad5d065d..3dc4c53bc 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -589,6 +589,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -597,6 +598,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -605,6 +607,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 43bb9b0d5..c49f5c103 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -36,6 +36,7 @@ #include "stabilizationsettings.h" #include "actuatordesired.h" #include "ratedesired.h" +#include "relaytuningsettings.h" #include "stabilizationdesired.h" #include "attitudeactual.h" #include "gyros.h" @@ -231,6 +232,7 @@ static void stabilizationTask(void* parameters) { switch(stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: rateDesiredAxis[i] = attitudeDesiredAxis[i]; @@ -317,6 +319,16 @@ static void stabilizationTask(void* parameters) { switch(stabDesired.StabilizationMode[i]) { + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: + { + RelayTuningSettingsData relay; + RelayTuningSettingsGet(&relay); + float error = rateDesiredAxis[i] - gyro_filtered[i]; + float command = error > 0 ? relay.Amplitude : -relay.Amplitude; + actuatorDesiredAxis[i] = bound(command); + break; + break; + } case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 09d4ca618..8a52b3b23 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -18,9 +18,9 @@ - - - + + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 02c63cd90..f24dd9eeb 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + From 4ac8df6aa8ca194f31263ef9c6d0a634dfa5c55c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 29 Jul 2012 21:08:46 -0500 Subject: [PATCH 03/43] Make the system perform an online estimate of the period and amplitude of the oscillation during relay tuning --- flight/Modules/Stabilization/stabilization.c | 57 ++++++++++++++++++-- shared/uavobjectdefinition/relaytuning.xml | 3 +- 2 files changed, 55 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index c49f5c103..3eb2c9c05 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -36,6 +36,7 @@ #include "stabilizationsettings.h" #include "actuatordesired.h" #include "ratedesired.h" +#include "relaytuning.h" #include "relaytuningsettings.h" #include "stabilizationdesired.h" #include "attitudeactual.h" @@ -128,6 +129,8 @@ int32_t StabilizationInitialize() // Initialize variables StabilizationSettingsInitialize(); ActuatorDesiredInitialize(); + RelayTuningSettingsInitialize(); + RelayTuningInitialize(); #if defined(DIAGNOSTICS) RateDesiredInitialize(); #endif @@ -321,12 +324,58 @@ static void stabilizationTask(void* parameters) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: { - RelayTuningSettingsData relay; - RelayTuningSettingsGet(&relay); + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); float error = rateDesiredAxis[i] - gyro_filtered[i]; - float command = error > 0 ? relay.Amplitude : -relay.Amplitude; + float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; actuatorDesiredAxis[i] = bound(command); - break; + + RelayTuningData relay; + RelayTuningGet(&relay); + + static bool high = false; + static portTickType lastHighTime; + static portTickType lastLowTime; + portTickType thisTime = xTaskGetTickCount(); + + static float accum_cos, accum_sin; + 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; + float phase = dT * 2 * M_PI / relay.Period[i]; + accum_cos += cosf(phase) * error; + accum_sin += sinf(phase) * 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_cos*accum_cos + accum_sin*accum_sin) / accumulated; + accumulated = 0; + accum_cos = 0; + accum_sin = 0; + + // Low pass filter each amplitude and period + relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (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_RATE: diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml index f13bfc9f2..ef54f8b7f 100644 --- a/shared/uavobjectdefinition/relaytuning.xml +++ b/shared/uavobjectdefinition/relaytuning.xml @@ -2,7 +2,8 @@ The input to the relay tuning. - + + From e01c5d5f871a49d94ddf8b6e04df74a47322c087 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 30 Jul 2012 15:20:18 -0500 Subject: [PATCH 04/43] Get online estimation of period and amplitude working --- flight/Modules/Stabilization/stabilization.c | 37 +++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 3eb2c9c05..f47aefe9f 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -91,6 +91,9 @@ pid_type pids[PID_MAX]; int8_t vbar_gyros_suppress; bool vbar_piro_comp = false; +// TODO: Move this to flash +static float sin_lookup[360]; + // Private functions static void stabilizationTask(void* parameters); static float ApplyPid(pid_type * pid, const float err, float dT); @@ -107,6 +110,9 @@ int32_t StabilizationStart() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + for(uint32_t i = 0; i < 360; i++) + sin_lookup[i] = sinf((float)i * 2 * M_PI / 360.0f); + // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue); @@ -316,6 +322,8 @@ static void stabilizationTask(void* parameters) #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++) @@ -338,7 +346,7 @@ static void stabilizationTask(void* parameters) static portTickType lastLowTime; portTickType thisTime = xTaskGetTickCount(); - static float accum_cos, accum_sin; + static float accum_sin, accum_cos; static uint32_t accumulated = 0; const uint16_t DEGLITCH_TIME = 20; // ms @@ -352,22 +360,30 @@ static void stabilizationTask(void* parameters) // Project the error onto a sine and cosine of the same frequency // to accumulate the average amplitude float dT = thisTime - lastHighTime; - float phase = dT * 2 * M_PI / relay.Period[i]; - accum_cos += cosf(phase) * error; - accum_sin += sinf(phase) * error; + 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_cos*accum_cos + accum_sin*accum_sin) / accumulated; + float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; accumulated = 0; - accum_cos = 0; accum_sin = 0; + accum_cos = 0; - // Low pass filter each amplitude and period - relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (1 - AMPLITUDE_ALPHA); - relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + if(rateRelayRunning[i] == false) { + rateRelayRunning[i] = true; + relay.Period[i] = 200; + relay.Amplitude[i] = 0; + } else { + // Low pass filter each amplitude and period + relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (1 - AMPLITUDE_ALPHA); + relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + } lastHighTime = thisTime; high = true; RelayTuningSet(&relay); @@ -383,12 +399,14 @@ static void stabilizationTask(void* parameters) case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { + 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) @@ -410,6 +428,7 @@ static void stabilizationTask(void* parameters) break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: + rateRelayRunning[i] = false; switch (i) { case ROLL: From 44e72d0a700bda4fe5f5d0cd2a064d83ca9e8900 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 30 Jul 2012 20:55:43 -0500 Subject: [PATCH 05/43] 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) * @} * @} */ + From a9af53b4f364fcad455e61570eca7be0f0de21f3 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 02:04:36 -0500 Subject: [PATCH 06/43] Create new autotuning module which rotates through the axes for autotuning and then computes new stabilization settings. --- flight/Modules/Autotune/autotune.c | 291 ++++++++++++++++++ flight/Modules/Autotune/inc/autotune.h | 37 +++ shared/uavobjectdefinition/flightstatus.xml | 2 +- .../manualcontrolsettings.xml | 2 +- 4 files changed, 330 insertions(+), 2 deletions(-) create mode 100644 flight/Modules/Autotune/autotune.c create mode 100644 flight/Modules/Autotune/inc/autotune.h diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c new file mode 100644 index 000000000..3fc9246e4 --- /dev/null +++ b/flight/Modules/Autotune/autotune.c @@ -0,0 +1,291 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Autotuning module + * @brief Reads from @ref ManualControlCommand and fakes a rate mode while + * toggling the channels to relay mode + * @{ + * + * @file autotune.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Module to handle all comms to the AHRS on a periodic basis. + * + * @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 + */ + +/** + * Input objects: None, takes sensor data via pios + * Output objects: @ref AttitudeRaw @ref AttitudeActual + * + * This module computes an attitude estimate from the sensor data + * + * The module executes in its own thread. + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "pios.h" +#include "flightstatus.h" +#include "manualcontrolcommand.h" +#include "manualcontrolsettings.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" +#include "stabilizationdesired.h" +#include "stabilizationsettings.h" +#include + +// Private constants +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY+2) + +// Private types +enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; + +// Private variables +static xTaskHandle taskHandle; + +// Private functions +static void AutotuneTask(void *parameters); +static void update_stabilization_settings(); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneInitialize(void) +{ + // Create a queue, connect to manual control command and flightstatus + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneStart(void) +{ + + // Start main task + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + + //TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle); + //PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); + + return 0; +} + +MODULE_INITCALL(AutotuneInitialize, AutotuneStart) + +/** + * Module thread, should not return. + */ +static void AutotuneTask(void *parameters) +{ + //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + + enum AUTOTUNE_STATE state = AT_INIT; + + portTickType lastUpdateTime = xTaskGetTickCount(); + + while(1) { + // TODO: + // 1. get from queue + // 2. based on whether it is flightstatus or manualcontrol + + portTickType diffTime; + + const uint32_t PREPARE_TIME = 2000; + const uint32_t MEAURE_TIME = 10000; + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + // Only allow this module to run when autotuning + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + state = AT_INIT; + vTaskDelay(50); + continue; + } + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + ManualControlSettingsData manualSettings; + ManualControlSettingsGet(&manualSettings); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Throttle = manualControl.Throttle; + + switch(state) { + case AT_INIT: + + lastUpdateTime = xTaskGetTickCount(); + + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) + state = AT_START; + break; + + case AT_START: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Spend the first block of time in normal rate mode to get airborne + if (diffTime > PREPARE_TIME) { + state = AT_ROLL; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_ROLL: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the roll axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_PITCH; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_PITCH: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the pitch axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_FINISHED; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_FINISHED: + + // Wait until disarmed and landed before updating the settings + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) + state = AT_SET; + + break; + + case AT_SET: + update_stabilization_settings(); + state = AT_INIT; + break; + + default: + // Set an alarm or some shit like that + break; + } + + StabilizationDesiredSet(&stabDesired); + + vTaskDelay(10); + } +} + +/** + * Called after measuring roll and pitch to update the + * stabilization settings + * + * takes in @ref RelayTuning and outputs @ref StabilizationSettings + */ +static void update_stabilization_settings() +{ + RelayTuningData relayTuning; + RelayTuningGet(&relayTuning); + + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + // Eventually get these settings from RelayTuningSettings + const float gain_ratio_r = 1.0f / 3.0f; + const float zero_ratio_r = 1.0f / 3.0f; + const float gain_ratio_p = 1.0f / 5.0f; + const float zero_ratio_p = 1.0f / 5.0f; + + float input = relaySettings.Amplitude * 4.0f / M_PI; // amplitude of input (fundamental component of fourier series for the square wave) + + // For now just run over roll and pitch + for (uint i = 0; i < 2; i++) { + float output = relayTuning.Amplitude[i]; // amplitude of sinusoidal oscillation in output + float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) + + float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + float zc = wc * zero_ratio_r; // controller zero location (rad/s) + float kpu = input / output; // ultimate gain, i.e. the proportional gain for instablity + float kp = kpu * gain_ratio_r; // proportional gain + float ki = zc * kp; // integral gain + + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + float wc2 = wc * gain_ratio_p; // crossover of the attitude loop + float kp2 = wc2; // kp of attitude + float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch(i) { + case 0: // roll + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; + stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; + stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; + break; + default: + // Oh shit oh shit oh shit + break; + } + } + StabilizationSettingsSet(&stabSettings); +} + +/** + * @} + * @} + */ diff --git a/flight/Modules/Autotune/inc/autotune.h b/flight/Modules/Autotune/inc/autotune.h new file mode 100644 index 000000000..3f90e56a6 --- /dev/null +++ b/flight/Modules/Autotune/inc/autotune.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Attitude Attitude Module + * @{ + * + * @file attitude.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Acquires sensor data and fuses it into attitude estimate for CC + * + * @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 ATTITUDE_H +#define ATTITUDE_H + +#include "openpilot.h" + +int32_t AttitudeInitialize(void); + +#endif // ATTITUDE_H diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 22c4f9360..ba24d9bff 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 8a52b3b23..b9babcc4d 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -24,7 +24,7 @@ - + From 48362f56f6fb7b7d7816d16d303a0cdc9f1dbb35 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 02:35:33 -0500 Subject: [PATCH 07/43] Enable autotune as an optional CC module --- flight/CopterControl/Makefile | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 7c5498806..721ec1896 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -68,6 +68,7 @@ USE_GPS ?= YES USE_TXPID ?= YES USE_I2C ?= NO USE_ALTITUDE ?= NO +USE_AUTOTUNE ?= NO TEST_FAULTS ?= NO # List of optional modules to include @@ -94,6 +95,9 @@ endif ifeq ($(TEST_FAULTS), YES) OPTMODULES += Fault endif +ifeq ($(USE_AUTOTUNE), YES) +OPTMODULES += Autotune +endif # List of mandatory modules to include MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP @@ -204,6 +208,8 @@ SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c +SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c +SRC += $(OPUAVSYNTHDIR)/relaytuning.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c From 17878b32f30ebe6dce7db72bae3a4ca6df111be1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 09:49:17 -0500 Subject: [PATCH 08/43] Make autotune run within an attitude loop instead of direct rate mode. Easier for beginners. --- flight/Modules/Autotune/autotune.c | 19 ++++++++++++++----- flight/Modules/Stabilization/stabilization.c | 12 ++++++++++++ 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 3fc9246e4..754136b83 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -144,12 +144,21 @@ static void AutotuneTask(void *parameters) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + if (0) { // rate mode + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; - stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + + stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; + stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; + } + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; stabDesired.Throttle = manualControl.Throttle; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 87bee87cd..0b15b60cd 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -322,6 +322,18 @@ static void stabilizationTask(void* parameters) break; } + case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: + 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; + break; case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) From c365a9c7ffb86613d2a3c886d0e9fbd9081f7f3d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 17:38:21 -0500 Subject: [PATCH 09/43] Add stub for the autotune configuration widget --- .../src/plugins/config/autotune.ui | 940 ++++++++++++++++++ .../src/plugins/config/config.pro | 16 +- .../plugins/config/configautotunewidget.cpp | 31 + .../src/plugins/config/configautotunewidget.h | 55 + .../src/plugins/config/configgadgetwidget.cpp | 4 + .../src/plugins/config/configgadgetwidget.h | 2 +- 6 files changed, 1044 insertions(+), 4 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/autotune.ui create mode 100644 ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/configautotunewidget.h diff --git a/ground/openpilotgcs/src/plugins/config/autotune.ui b/ground/openpilotgcs/src/plugins/config/autotune.ui new file mode 100644 index 000000000..120caab94 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/autotune.ui @@ -0,0 +1,940 @@ + + + AutotuneWidget + + + + 0 + 0 + 443 + 505 + + + + Form + + + + + 20 + 10 + 401 + 121 + + + + Tuning Aggressiveness + + + + + + Rate Tuning: + + + + + + + Qt::Horizontal + + + + + + + Attitude Tuning: + + + + + + + Qt::Horizontal + + + + + + + + + 20 + 250 + 401 + 131 + + + + Computed Values + + + + + + RateKp + + + + + + + Roll + + + + + + + RateKi + + + + + + + AttitudeKp + + + + + + + AttitudeKi + + + + + + + Pitch + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + 0 + + + + + + + + + 20 + 140 + 401 + 101 + + + + Measured Values + + + + + + Roll: + + + + + + + 0 + + + + + + + 0 + + + + + + + Period + + + + + + + Amplitude + + + + + + + Pitch + + + + + + + 0 + + + + + + + 0 + + + + + + + + + 20 + 400 + 401 + 79 + + + + + 0 + 0 + + + + + 0 + 79 + + + + + 16777215 + 79 + + + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 251 + 251 + 251 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 251 + 251 + 251 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 124 + 124 + 124 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 255 + 255 + 255 + + + + + + + 251 + 251 + 251 + + + + + + + 124 + 124 + 124 + + + + + + + 165 + 165 + 165 + + + + + + + 124 + 124 + 124 + + + + + + + 255 + 255 + 255 + + + + + + + 124 + 124 + 124 + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + + + 243 + 243 + 243 + + + + + 250 + 250 + 250 + + + + + + + + + 0 + 0 + 0 + + + + + + + 248 + 248 + 248 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + false + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + Qt::Horizontal + + + + 111 + 10 + + + + + + + + + 120 + 28 + + + + Reloads the saved settings into GCS. +Useful if you have accidentally changed some settings. + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Reload Board Data + + + + button:reload + buttongroup:10 + + + + + + + + + 60 + 28 + + + + Send settings to the board but do not save to the non-volatile memory + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Apply + + + + button:apply + + + + + + + + + 60 + 28 + + + + Send settings to the board and save to the non-volatile memory + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Save + + + + button:save + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 8e49cb54d..1e0bdb3cb 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -36,7 +36,8 @@ HEADERS += configplugin.h \ cfg_vehicletypes/configfixedwingwidget.h \ cfg_vehicletypes/vehicleconfig.h \ configrevowidget.h \ - config_global.h + config_global.h \ + configautotunewidget.h SOURCES += configplugin.cpp \ configgadgetconfiguration.cpp \ configgadgetwidget.cpp \ @@ -67,7 +68,8 @@ SOURCES += configplugin.cpp \ cfg_vehicletypes/configfixedwingwidget.cpp \ cfg_vehicletypes/configccpmwidget.cpp \ outputchannelform.cpp \ - cfg_vehicletypes/vehicleconfig.cpp + cfg_vehicletypes/vehicleconfig.cpp \ + configautotunewidget.cpp FORMS += airframe.ui \ cc_hw_settings.ui \ pro_hw_settings.ui \ @@ -83,5 +85,13 @@ FORMS += airframe.ui \ outputchannelform.ui \ revosensors.ui \ txpid.ui \ - pipxtreme.ui + pipxtreme.ui \ + autotune.ui RESOURCES += configgadget.qrc + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp new file mode 100644 index 000000000..a64787580 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -0,0 +1,31 @@ + +#include "configautotunewidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : + ConfigTaskWidget(parent) +{ + m_autotune = new Ui_AutotuneWidget(); + m_autotune->setupUi(this); + + autoLoadWidgets(); + + disableMouseWheelEvents(); +} + +ConfigAutotuneWidget::~ConfigAutotuneWidget() +{ + // Do nothing +} + + + diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h new file mode 100644 index 000000000..bdaa615de --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * + * @file configautotunewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to adjust or recalculate autotuning + *****************************************************************************/ +/* + * 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 CONFIGAUTOTUNE_H +#define CONFIGAUTOTUNE_H + +#include "ui_autotune.h" +#include "../uavobjectwidgetutils/configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include "stabilizationsettings.h" +#include "relaytuningsettings.h" +#include "relaytuning.h" +#include +#include + +class ConfigAutotuneWidget : public ConfigTaskWidget +{ + Q_OBJECT +public: + explicit ConfigAutotuneWidget(QWidget *parent = 0); + +private: + Ui_AutotuneWidget *m_autotune; +signals: + +public slots: + +}; + +#endif // CONFIGAUTOTUNE_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index c12dbaee8..5c4a38117 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -32,6 +32,7 @@ #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" +#include "configautotunewidget.h" #include "configcamerastabilizationwidget.h" #include "configtxpidwidget.h" #include "config_pro_hw_widget.h" @@ -84,6 +85,9 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) qwd = new ConfigStabilizationWidget(this); ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization")); + qwd = new ConfigAutotuneWidget(this); + ftw->insertTab(ConfigGadgetWidget::autotune, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Autotune")); + qwd = new ConfigCameraStabilizationWidget(this); ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab")); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 5e9dc6e67..0479f5928 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -50,7 +50,7 @@ class ConfigGadgetWidget: public QWidget public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme}; + enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune}; public slots: void onAutopilotConnect(); From dfd1aceb06be44c3ae019664c89877ac932dcd58 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 18:04:42 -0500 Subject: [PATCH 10/43] More work on autotune from GCS --- .../src/plugins/config/autotune.ui | 105 ++++++++++++++---- .../plugins/config/configautotunewidget.cpp | 6 - .../relaytuningsettings.xml | 2 + 3 files changed, 87 insertions(+), 26 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/autotune.ui b/ground/openpilotgcs/src/plugins/config/autotune.ui index 120caab94..55e4448a0 100644 --- a/ground/openpilotgcs/src/plugins/config/autotune.ui +++ b/ground/openpilotgcs/src/plugins/config/autotune.ui @@ -7,7 +7,7 @@ 0 0 443 - 505 + 560 @@ -19,39 +19,77 @@ 20 10 401 - 121 + 131 Tuning Aggressiveness - + Rate Tuning: - + Qt::Horizontal + + + obj1name:RelayTuningSettings + fieldname:RateGain + scale:0.01 + haslimits:yes + + - + Attitude Tuning: - + Qt::Horizontal + + + obj1name:RelayTuningSettings + fieldname:AttitudeGain + elemescale:0.01 + haslimits:yes + + + + + + + + StepSize + + + + + + + Qt::Horizontal + + + + obj1name:RelatyTuningSettings + fieldname:Amplitude + scale:0.01 + haslimits:yes + + @@ -62,7 +100,7 @@ 20 250 401 - 131 + 121 @@ -112,56 +150,56 @@ - + 0 - + 0 - + 0 - + 0 - + 0 - + 0 - + 0 - + 0 @@ -175,11 +213,11 @@ 20 140 401 - 101 + 111 - Measured Values + Measured Properties @@ -206,14 +244,14 @@ - Period + Period (ms) - Amplitude + Amplitude (deg/s) @@ -244,7 +282,7 @@ 20 - 400 + 480 401 79 @@ -934,6 +972,33 @@ border-radius: 4; + + + + 30 + 440 + 371 + 31 + + + + The buttons below change the autotuning settings which +will alter thingsfor the next autotuning flight + + + + + + 270 + 380 + 161 + 24 + + + + Use Computed Values + + diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index a64787580..4e61f473b 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -22,10 +22,4 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : disableMouseWheelEvents(); } -ConfigAutotuneWidget::~ConfigAutotuneWidget() -{ - // Do nothing -} - - diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index 46a5bc124..c73f786e7 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -1,6 +1,8 @@ Setting to run a relay tuning algorithm + + From 52ffec0be4ec8beb208845984e9407e1b23996d1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 21:39:26 -0500 Subject: [PATCH 11/43] Increase the initial tuning amplitude to 0.15 --- shared/uavobjectdefinition/relaytuningsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index c73f786e7..37b60f740 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -3,7 +3,7 @@ Setting to run a relay tuning algorithm - + From 5c00451c9ed2d0901f0af79aaaf4c24e31204a3e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 01:13:05 -0500 Subject: [PATCH 12/43] Remove unused value field from the relaytuning object --- shared/uavobjectdefinition/relaytuning.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml index ef54f8b7f..297b44686 100644 --- a/shared/uavobjectdefinition/relaytuning.xml +++ b/shared/uavobjectdefinition/relaytuning.xml @@ -1,7 +1,6 @@ The input to the relay tuning. - From 7ea14ecc22fa0dac3155f9bbb06209d5ae255d04 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 01:21:57 -0500 Subject: [PATCH 13/43] Track the output gain instead of amplitude so the measured values are consistent if the settings are changed afterwards. --- flight/Modules/Autotune/autotune.c | 5 +---- shared/uavobjectdefinition/relaytuning.xml | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 754136b83..41e8a9542 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -254,16 +254,13 @@ static void update_stabilization_settings() const float gain_ratio_p = 1.0f / 5.0f; const float zero_ratio_p = 1.0f / 5.0f; - float input = relaySettings.Amplitude * 4.0f / M_PI; // amplitude of input (fundamental component of fourier series for the square wave) - // For now just run over roll and pitch for (uint i = 0; i < 2; i++) { - float output = relayTuning.Amplitude[i]; // amplitude of sinusoidal oscillation in output float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) float zc = wc * zero_ratio_r; // controller zero location (rad/s) - float kpu = input / output; // ultimate gain, i.e. the proportional gain for instablity + float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity float kp = kpu * gain_ratio_r; // proportional gain float ki = zc * kp; // integral gain diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml index 297b44686..d781e45ca 100644 --- a/shared/uavobjectdefinition/relaytuning.xml +++ b/shared/uavobjectdefinition/relaytuning.xml @@ -2,7 +2,7 @@ The input to the relay tuning. - + From 28539a80d19edc861cecd8b2ed0d25e32e4936ac Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 01:38:40 -0500 Subject: [PATCH 14/43] Change default tuning settings --- shared/uavobjectdefinition/relaytuningsettings.xml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index 37b60f740..abfa78a2a 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -1,9 +1,11 @@ Setting to run a relay tuning algorithm - - + + + + From 24d9e50c0808aaff47a23b36517958ba9b984f07 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 02:09:10 -0500 Subject: [PATCH 15/43] Based on the selected behavior either apply or save the stabilization settings --- flight/Modules/Autotune/autotune.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 41e8a9542..1a26115ff 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -288,7 +288,19 @@ static void update_stabilization_settings() break; } } - StabilizationSettingsSet(&stabSettings); + switch(relaySettings.Behavior) { + case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: + // Just measure, don't update the stab settings + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: + StabilizationSettingsSet(&stabSettings); + break; + case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: + StabilizationSettingsSet(&stabSettings); + UAVObjSave(StabilizationSettingsHandle(), 0); + break; + } + } /** From 2ccd6605a465d9183352ad997e62f676dc8ac05c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 02:56:08 -0500 Subject: [PATCH 16/43] Now the stabilization refactor is in we can make swapping between tuning the system in rate or attitude mode software configurable. --- flight/Modules/Autotune/autotune.c | 13 ++++++++++--- flight/Modules/ManualControl/manualcontrol.c | 9 ++++++--- .../uavobjectdefinition/manualcontrolsettings.xml | 6 +++--- shared/uavobjectdefinition/stabilizationdesired.xml | 2 +- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 1a26115ff..776e9b842 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -144,7 +144,12 @@ static void AutotuneTask(void *parameters) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - if (0) { // rate mode + RelayTuningSettingsData relaySettings; + RelayTuningSettingsGet(&relaySettings); + + bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; + + if (rate) { // rate mode stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; @@ -188,7 +193,8 @@ static void AutotuneTask(void *parameters) diffTime = xTaskGetTickCount() - lastUpdateTime; // Run relay mode on the roll axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; if (diffTime > MEAURE_TIME) { // Move on to next state state = AT_PITCH; lastUpdateTime = xTaskGetTickCount(); @@ -200,7 +206,8 @@ static void AutotuneTask(void *parameters) diffTime = xTaskGetTickCount() - lastUpdateTime; // Run relay mode on the pitch axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : + STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; if (diffTime > MEAURE_TIME) { // Move on to next state state = AT_FINISHED; lastUpdateTime = xTaskGetTickCount(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 3dc4c53bc..22ced4c82 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -589,7 +589,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : @@ -598,7 +599,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : @@ -607,7 +609,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index b9babcc4d..6a4dab0a9 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -18,9 +18,9 @@ - - - + + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index f24dd9eeb..8c400e68d 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + From c3df203d7ccc4b2a2650fed7686de00fc567c76a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 02:56:54 -0500 Subject: [PATCH 17/43] Make the autotune UI allow recomputing the values based on the measured system properties. --- .../src/plugins/config/autotune.ui | 163 +++++++++++++----- .../plugins/config/configautotunewidget.cpp | 113 +++++++++++- .../src/plugins/config/configautotunewidget.h | 5 + 3 files changed, 238 insertions(+), 43 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/autotune.ui b/ground/openpilotgcs/src/plugins/config/autotune.ui index 55e4448a0..113144535 100644 --- a/ground/openpilotgcs/src/plugins/config/autotune.ui +++ b/ground/openpilotgcs/src/plugins/config/autotune.ui @@ -7,7 +7,7 @@ 0 0 443 - 560 + 575 @@ -26,68 +26,52 @@ Tuning Aggressiveness - + Rate Tuning: - - - - Qt::Horizontal - - - - obj1name:RelayTuningSettings - fieldname:RateGain - scale:0.01 - haslimits:yes - - - - - + Attitude Tuning: - - + + + + 100 + Qt::Horizontal - obj1name:RelayTuningSettings - fieldname:AttitudeGain - elemescale:0.01 - haslimits:yes + objname:RelayTuningSettings + fieldname:RateGain + scale:0.01 + haslimits:no - - - - StepSize + + + + 100 - - - - Qt::Horizontal - obj1name:RelatyTuningSettings - fieldname:Amplitude + objname:RelayTuningSettings + fieldname:AttitudeGain scale:0.01 - haslimits:yes + haslimits:no @@ -232,13 +216,33 @@ 0 + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Period + element:Roll + + - + 0 + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Gain + element:Roll + + @@ -251,7 +255,7 @@ - Amplitude (deg/s) + Gain (deg/s) / output @@ -267,13 +271,33 @@ 0 + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Period + element:Pitch + + - + 0 + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + objname:RelayTuning + fieldname:Gain + element:Pitch + + @@ -989,14 +1013,69 @@ will alter thingsfor the next autotuning flight - 270 + 250 380 - 161 - 24 + 171 + 31 + + + + QPushButton { +border: 1px outset #999; +border-radius: 5; +background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255)); +} + +QPushButton:pressed { + + border-style: inset; + background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255)); + +} + + +QPushButton:hover { + border: 1px outset #999; +border-color: rgb(83, 83, 83); +border-radius: 4; + } + + + Apply Computed Values + + + + + + 40 + 410 + 104 + 20 - Use Computed Values + Step Size + + + + + + 149 + 410 + 266 + 20 + + + + Qt::Horizontal + + + + objname:RelayTuningSettings + fieldname:Amplitude + scale:0.01 + haslimits:no + diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index 4e61f473b..6bef18130 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -10,6 +10,9 @@ #include #include #include +#include "relaytuningsettings.h" +#include "relaytuning.h" +#include "stabilizationsettings.h" ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : ConfigTaskWidget(parent) @@ -17,9 +20,117 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : m_autotune = new Ui_AutotuneWidget(); m_autotune->setupUi(this); + // Connect automatic signals autoLoadWidgets(); - disableMouseWheelEvents(); + + // Whenever any value changes compute new potential stabilization settings + connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); + connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); + + RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); + Q_ASSERT(relayTuning); + if(relayTuning) + connect(relayTuning, SIGNAL(updateRequested(UAVObject*)), this, SLOT(recomputeStabilization())); + + // Connect the apply button for the stabilization settings + connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization())); } +/** + * Apply the stabilization settings computed + */ +void ConfigAutotuneWidget::saveStabilization() +{ + StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); + if(!stabilizationSettings) + return; + // Make sure to recompute in case the other stab settings changed since + // the last time + recomputeStabilization(); + + // Apply this data to the board + stabilizationSettings->setData(stabSettings); + stabilizationSettings->updated(); +} + +/** + * Called whenever the gain ratios or measured values + * are changed + */ +void ConfigAutotuneWidget::recomputeStabilization() +{ + RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager()); + Q_ASSERT(relayTuningSettings); + if (!relayTuningSettings) + return; + + RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); + Q_ASSERT(relayTuning); + if(!relayTuning) + return; + + StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager()); + Q_ASSERT(stabilizationSettings); + if(!stabilizationSettings) + return; + + RelayTuning::DataFields relayTuningData = relayTuning->getData(); + RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData(); + stabSettings = stabilizationSettings->getData(); + + // Need to divide these by 100 because that is what the .ui file does + // to get the UAVO + const double gain_ratio_r = m_autotune->rateTuning->value() / 100.0; + const double zero_ratio_r = m_autotune->rateTuning->value() / 100.0; + const double gain_ratio_p = m_autotune->attitudeTuning->value() / 100.0; + const double zero_ratio_p = m_autotune->attitudeTuning->value() / 100.0; + + // For now just run over roll and pitch + for (int i = 0; i < 2; i++) { + if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0) + continue; + + double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s) + + double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) + double zc = wc * zero_ratio_r; // controller zero location (rad/s) + double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity + double kp = kpu * gain_ratio_r; // proportional gain + double ki = zc * kp; // integral gain + + // Now calculate gains for the next loop out knowing it is the integral of + // the inner loop -- the plant is position/velocity = scale*1/s + double wc2 = wc * gain_ratio_p; // crossover of the attitude loop + double kp2 = wc2; // kp of attitude + double ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude + + switch(i) { + case 0: // Roll + + stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp; + stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = ki; + stabSettings.RollPI[StabilizationSettings::ROLLPI_KP] = kp2; + stabSettings.RollPI[StabilizationSettings::ROLLPI_KI] = ki2; + break; + case 1: // Pitch + stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = kp; + stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki; + stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp2; + stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = ki2; + break; + } + } + + // Display these computed settings + m_autotune->rollRateKp->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP])); + m_autotune->rollRateKi->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI])); + m_autotune->rollAttitudeKp->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP])); + m_autotune->rollAttitudeKi->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KI])); + m_autotune->pitchRateKp->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP])); + m_autotune->pitchRateKi->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI])); + m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP])); + m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI])); +} diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h index bdaa615de..7a89ed373 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -46,10 +46,15 @@ public: private: Ui_AutotuneWidget *m_autotune; + StabilizationSettings::DataFields stabSettings; + signals: public slots: +private slots: + void recomputeStabilization(); + void saveStabilization(); }; #endif // CONFIGAUTOTUNE_H From d0ef95ff9a9269171c2589359b96ba5651834bc1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 03:08:51 -0500 Subject: [PATCH 18/43] Fix idiotic bug in the stab_refactor --- flight/Modules/Stabilization/stabilization.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 0b15b60cd..6f398ed3e 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -245,7 +245,7 @@ static void stabilizationTask(void* parameters) 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]); + bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); previous_mode[i] = stabDesired.StabilizationMode[i]; // Apply the selected control law From 652647fc87ab8f8138d29896bce17b8346fc9f0b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 11:04:37 -0500 Subject: [PATCH 19/43] Make autotuning module optional --- flight/Modules/Autotune/autotune.c | 29 ++++++++++++++++++----- flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 1 + shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/taskinfo.xml | 6 ++--- 4 files changed, 28 insertions(+), 10 deletions(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 776e9b842..ec44a7b72 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -50,6 +50,7 @@ #include "pios.h" #include "flightstatus.h" +#include "hwsettings.h" #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" #include "relaytuning.h" @@ -67,6 +68,7 @@ enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; // Private variables static xTaskHandle taskHandle; +static bool autotuneEnabled; // Private functions static void AutotuneTask(void *parameters); @@ -79,6 +81,19 @@ static void update_stabilization_settings(); int32_t AutotuneInitialize(void) { // Create a queue, connect to manual control command and flightstatus +#ifdef MODULE_AUTOTUNE_BUILTIN + autotuneEnabled = true; +#else + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) + autotuneEnabled = true; + else + autotuneEnabled = false; +#endif return 0; } @@ -89,13 +104,13 @@ int32_t AutotuneInitialize(void) */ int32_t AutotuneStart(void) { - - // Start main task - xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + // Start main task if it is enabled + if(autotuneEnabled) { + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - //TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle); - //PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); - + TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); + } return 0; } @@ -113,6 +128,8 @@ static void AutotuneTask(void *parameters) portTickType lastUpdateTime = xTaskGetTickCount(); while(1) { + + PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); // TODO: // 1. get from queue // 2. based on whether it is flightstatus or manualcontrol diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 10a7a41cd..85143fc17 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -74,6 +74,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_WDG_STABILIZATION 0x0002 #define PIOS_WDG_ATTITUDE 0x0004 #define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_AUTOTUNE 0x0010 //------------------------ // TELEMETRY diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 0cee2a6d9..0a09a7620 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -18,7 +18,7 @@ - + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 40f0d4bdf..97582f7e5 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -1,9 +1,9 @@ Task information - - - + + + From 561cf994b0cbba6c756fad3f653e6cf84d6bd7d0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 12:07:52 -0500 Subject: [PATCH 20/43] Reduce the memory footprint of the sin lookup table by using sin(x+pi) = -sin(x). Still just needs to move into flash and have some options about precision when in its own library function. --- flight/Modules/Autotune/autotune.c | 2 +- flight/Modules/Stabilization/stabilization.c | 21 ++++++++++++++------ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index ec44a7b72..7073d6395 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -60,7 +60,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 824 #define TASK_PRIORITY (tskIDLE_PRIORITY+2) // Private types diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 6f398ed3e..a37b1aca5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -92,7 +92,7 @@ int8_t vbar_gyros_suppress; bool vbar_piro_comp = false; // TODO: Move this to flash -static float sin_lookup[360]; +static float sin_lookup[180]; // Private functions static void stabilizationTask(void* parameters); @@ -101,6 +101,15 @@ 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 */ @@ -110,7 +119,7 @@ int32_t StabilizationStart() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - for(uint32_t i = 0; i < 360; i++) + for(uint32_t i = 0; i < 180; i++) sin_lookup[i] = sinf((float)i * 2 * M_PI / 360.0f); // Listen for updates. @@ -409,8 +418,8 @@ static void stabilizationTask(void* parameters) 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; + 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 @@ -491,8 +500,8 @@ static void stabilizationTask(void* parameters) 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; + 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 From ee4bb84e362aa4032eaf6de08ed3a0ddfdef3f23 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 12:20:36 -0500 Subject: [PATCH 21/43] Fix: Increase teh memory for autotune back to 1024 which leaves 100 free. --- flight/Modules/Autotune/autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index 7073d6395..ec44a7b72 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -60,7 +60,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 824 +#define STACK_SIZE_BYTES 1024 #define TASK_PRIORITY (tskIDLE_PRIORITY+2) // Private types From 8565dfbcc3c415b20775101d7a8452b2cc7991d7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 1 Aug 2012 17:52:53 -0500 Subject: [PATCH 22/43] 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: From 924c91ce1e7055fb3e2ff6fa186d3284113d16fd Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 01:20:57 -0500 Subject: [PATCH 23/43] Fix mistake from previous merge with duplicate the STABILIZATIONOMDE_ATTITUDE case --- flight/Modules/Stabilization/stabilization.c | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 4ffe9b944..bc6887081 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -322,19 +322,6 @@ static void stabilizationTask(void* parameters) break; } - case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: - 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; - break; - case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) pids[PID_RATE_ROLL + i].iAccumulator = 0; From aae0e562c67841f1be16e99b133758f658a3c313 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 01:53:55 -0500 Subject: [PATCH 24/43] Create a sine lookup table that is cached in flash and make relay tuning start ot use this. --- flight/CopterControl/Makefile | 2 + flight/Libraries/math/sin_lookup.c | 99 +++++++++++++++++++ flight/Libraries/math/sin_lookup.h | 39 ++++++++ .../Modules/Stabilization/inc/relay_tuning.h | 1 - flight/Modules/Stabilization/relay_tuning.c | 44 +-------- flight/Modules/Stabilization/stabilization.c | 3 - 6 files changed, 143 insertions(+), 45 deletions(-) create mode 100644 flight/Libraries/math/sin_lookup.c create mode 100644 flight/Libraries/math/sin_lookup.h diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 721ec1896..957089c12 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -268,6 +268,7 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/math/sin_lookup.c SRC += $(FLIGHTLIB)/taskmonitor.c ## CMSIS for STM32 @@ -369,6 +370,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(PIOS) EXTRAINCDIRS += $(PIOSINC) EXTRAINCDIRS += $(FLIGHTLIBINC) +EXTRAINCDIRS += $(FLIGHTLIB)/math EXTRAINCDIRS += $(PIOSSTM32F10X) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c new file mode 100644 index 000000000..575f171d3 --- /dev/null +++ b/flight/Libraries/math/sin_lookup.c @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @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 "math.h" +#include "stdint.h" + + // This is a precomputed sin lookup table over 90 degrees that + const float sin_table[] = { + 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, + 0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f, + 0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f, + 0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f, + 0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f, + 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, + 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, + 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, + 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f + }; + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * to save flash this cheats and uses trig functions to only save + * 90 values + * @param[in] angle Angle in degrees + * @returns sin(angle) +*/ +float sin_lookup_deg(float angle) +{ + int i_ang = ((int32_t) angle) % 360; + if(i_ang > 270) // for 270 to 360 deg + return -sin_table[360 - i_ang]; + else if (i_ang > 180) // for 180 to 270 deg + return -sin_table[i_ang - 180]; + else if (i_ang > 90) // for 90 to 170 deg + return sin_table[180 - i_ang]; + else // for 0 to 90 deg + return sin_table[i_ang]; + + return 0; +} + +/** + * Get cos(angle) using the sine lookup table + * @param[in] angle Angle in degrees + * @returns cos(angle) + */ +float cos_lookup_deg(float angle) +{ + return sin_lookup_deg(angle + 90); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns sin(angle) + */ +float sin_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return sin_lookup_deg(degrees); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns cos(angle) + */ +float cos_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return cos_lookup_deg(degrees); +} \ No newline at end of file diff --git a/flight/Libraries/math/sin_lookup.h b/flight/Libraries/math/sin_lookup.h new file mode 100644 index 000000000..0e0edb274 --- /dev/null +++ b/flight/Libraries/math/sin_lookup.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @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 SIN_LOOKUP_H +#define SIN_LOOKUP_H + +float sin_lookup_deg(float angle); +float cos_lookup_deg(float angle); +float sin_lookup_rad(float angle); +float cos_lookup_rad(float angle); + +#endif \ No newline at end of file diff --git a/flight/Modules/Stabilization/inc/relay_tuning.h b/flight/Modules/Stabilization/inc/relay_tuning.h index d788ae62e..9044986c5 100644 --- a/flight/Modules/Stabilization/inc/relay_tuning.h +++ b/flight/Modules/Stabilization/inc/relay_tuning.h @@ -34,7 +34,6 @@ #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 index ad4c7e064..16b4351fd 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -32,40 +32,15 @@ */ #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" +#include "sin_lookup.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 @@ -129,8 +104,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) 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; + accum_sin += sin_lookup_deg(phase) * error; + accum_cos += sin_lookup_deg(phase + 90) * error; accumulated ++; // Make sure we've had enough time since last transition then check for a change in the output @@ -164,16 +139,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) } -/** - * 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 bc6887081..c72348aa8 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -110,9 +110,6 @@ int32_t StabilizationStart() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // This prepares this optional algorithm - stabilization_relay_init(); - // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue); From 2723ff4be3081c1488428da20aad169c8588c423 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 03:24:18 -0500 Subject: [PATCH 25/43] Factor the virtual flybar code out of the stabilization module --- .../Modules/Stabilization/inc/stabilization.h | 2 + .../Modules/Stabilization/inc/virtualflybar.h | 42 +++++++ flight/Modules/Stabilization/stabilization.c | 50 ++------ flight/Modules/Stabilization/virtualflybar.c | 119 ++++++++++++++++++ 4 files changed, 170 insertions(+), 43 deletions(-) create mode 100644 flight/Modules/Stabilization/inc/virtualflybar.h create mode 100644 flight/Modules/Stabilization/virtualflybar.c diff --git a/flight/Modules/Stabilization/inc/stabilization.h b/flight/Modules/Stabilization/inc/stabilization.h index 89a816c24..ec0ae4e9f 100644 --- a/flight/Modules/Stabilization/inc/stabilization.h +++ b/flight/Modules/Stabilization/inc/stabilization.h @@ -33,6 +33,8 @@ #ifndef STABILIZATION_H #define STABILIZATION_H +enum {ROLL,PITCH,YAW,MAX_AXES}; + int32_t StabilizationInitialize(); #endif // STABILIZATION_H diff --git a/flight/Modules/Stabilization/inc/virtualflybar.h b/flight/Modules/Stabilization/inc/virtualflybar.h new file mode 100644 index 000000000..8ef54ce52 --- /dev/null +++ b/flight/Modules/Stabilization/inc/virtualflybar.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Virtual flybar mode + * @note This file implements the logic for a virtual flybar + * @{ + * + * @file virtualflybar.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 VIRTUALFLYBAR_H + #define VIRTUALFLYBAR_H + +#include "openpilot.h" +#include "stabilizationsettings.h" + +int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings); +int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT); + + #endif /* VIRTUALFLYBAR_H */ \ No newline at end of file diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index c72348aa8..d36505d1d 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -47,6 +47,7 @@ // Includes for various stabilization algorithms #include "relay_tuning.h" +#include "virtualflybar.h" // Private constants #define MAX_QUEUE_SIZE 1 @@ -62,9 +63,6 @@ enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX}; -enum {ROLL,PITCH,YAW,MAX_AXES}; - - // Private types typedef struct { float p; @@ -80,7 +78,6 @@ static xTaskHandle taskHandle; static StabilizationSettingsData settings; static xQueueHandle queue; float gyro_alpha = 0; -float gyro_filtered[3] = {0,0,0}; float axis_lock_accum[3] = {0,0,0}; float vbar_sensitivity[3] = {1, 1, 1}; uint8_t max_axis_lock = 0; @@ -88,11 +85,8 @@ uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; -float vbar_integral[3] = {0, 0, 0}; float vbar_decay = 0.991f; pid_type pids[PID_MAX]; -int8_t vbar_gyros_suppress; -bool vbar_piro_comp = false; // Private functions static void stabilizationTask(void* parameters); @@ -224,6 +218,7 @@ static void stabilizationTask(void* parameters) local_error[2] = fmodf(local_error[2] + 180, 360) - 180; #endif + float gyro_filtered[3]; 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); @@ -278,32 +273,14 @@ static void stabilizationTask(void* parameters) break; case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: - { - if(reinit) - vbar_integral[i] = 0; + // Store for debugging output rateDesiredAxis[i] = attitudeDesiredAxis[i]; - // 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); - - // 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; - } - - // 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); - - actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); + // Run a virtual flybar stabilization algorithm on this axis + stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); break; - } case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { if (reinit) @@ -374,19 +351,8 @@ static void stabilizationTask(void* parameters) } } - // 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 (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) + stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); #if defined(DIAGNOSTICS) RateDesiredSet(&rateDesired); @@ -544,8 +510,6 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) // Compute time constant for vbar decay term based on a tau vbar_decay = expf(-fakeDt / settings.VbarTau); - vbar_gyros_suppress = settings.VbarGyroSuppress; - vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE; } diff --git a/flight/Modules/Stabilization/virtualflybar.c b/flight/Modules/Stabilization/virtualflybar.c new file mode 100644 index 000000000..bbfe69eae --- /dev/null +++ b/flight/Modules/Stabilization/virtualflybar.c @@ -0,0 +1,119 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Virtual flybar mode + * @note This file implements the logic for a virtual flybar + * @{ + * + * @file virtualflybar.c + * @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 + */ + +#include "openpilot.h" +#include "stabilization.h" +#include "stabilizationsettings.h" + +//! Private variables +static float vbar_integral[MAX_AXES]; +static float vbar_decay = 0.991f; + +//! Private methods +static float bound(float val, float range); + +int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings) +{ + float gyro_gain = 1.0f; + float kp = 0, ki = 0; + + if(reinit) + vbar_integral[axis] = 0; + + // Track the angle of the virtual flybar which includes a slow decay + vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT; + vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); + + // Command signal can indicate how much to disregard the gyro feedback (fast flips) + if (settings->VbarGyroSuppress > 0) { + gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f); + gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; + } + + // Get the settings for the correct axis + switch(axis) + { + case ROLL: + kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case PITCH: + kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + case YAW: + kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; + ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + break; + default: + PIOS_DEBUG_Assert(0); + } + + // Command signal is composed of stick input added to the gyro and virtual flybar + *output = command * settings->VbarSensitivity[axis] - + gyro_gain * (vbar_integral[axis] * ki + gyro * kp); + + return 0; +} + +/** + * Want to keep the virtual flybar fixed in world coordinates as we pirouette + * @param[in] z_gyro The deg/s of rotation along the z axis + * @param[in] dT The time since last sample + */ +int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT) +{ + const float F_PI = (float) M_PI; + float cy = cosf(z_gyro / 180.0f * F_PI * dT); + float sy = sinf(z_gyro / 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; + + return 0; +} + +/** + * Bound input value between limits + */ +static float bound(float val, float range) +{ + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; + } + return val; +} From 50c764116255f9a7b7e38d4d0122777685e710cf Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 04:12:12 -0500 Subject: [PATCH 26/43] Move the PID methods into a standalone library --- flight/CopterControl/Makefile | 7 +- flight/Libraries/math/pid.c | 91 +++++++++++++++ flight/Libraries/math/pid.h | 49 ++++++++ flight/Modules/Stabilization/stabilization.c | 116 ++++++------------- 4 files changed, 181 insertions(+), 82 deletions(-) create mode 100644 flight/Libraries/math/pid.c create mode 100644 flight/Libraries/math/pid.h diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 957089c12..7a56df860 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -115,6 +115,8 @@ OPTESTS = ./Tests OPMODULEDIR = ../Modules FLIGHTLIB = ../Libraries FLIGHTLIBINC = $(FLIGHTLIB)/inc +MATHLIB = ../Libraries/math +MATHLIBINC = ../Libraries/math PIOS = ../PiOS PIOSINC = $(PIOS)/inc PIOSSTM32F10X = $(PIOS)/STM32F10x @@ -268,8 +270,9 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c -SRC += $(FLIGHTLIB)/math/sin_lookup.c SRC += $(FLIGHTLIB)/taskmonitor.c +SRC += $(MATHLIB)/sin_lookup.c +SRC += $(MATHLIB)/pid.c ## CMSIS for STM32 SRC += $(CMSISDIR)/core_cm3.c @@ -370,7 +373,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(PIOS) EXTRAINCDIRS += $(PIOSINC) EXTRAINCDIRS += $(FLIGHTLIBINC) -EXTRAINCDIRS += $(FLIGHTLIB)/math +EXTRAINCDIRS += $(MATHLIBINC) EXTRAINCDIRS += $(PIOSSTM32F10X) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c new file mode 100644 index 000000000..58f15ad1f --- /dev/null +++ b/flight/Libraries/math/pid.c @@ -0,0 +1,91 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file pid.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Methods to work with PID structure + * + * @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 "pid.h" + +//! Private method +static float bound(float val, float range); + +float pid_apply(struct pid *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); + pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); + return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); +} + +/** + * Reset a bit + * @param[in] pid The pid to reset + */ +void pid_zero(struct pid *pid) +{ + if (!pid) + return; + + pid->iAccumulator = 0; + pid->lastErr = 0; +} + +/** + * Configure the settings for a pid structure + * @param[out] pid The PID structure to configure + * @param[in] p The proportional term + * @param[in] i The integral term + * @param[in] d The derivative term + */ +void pid_configure(struct pid *pid, float p, float i, float d, float iLim) +{ + if (!pid) + return; + + pid->p = p; + pid->i = i; + pid->d = d; + pid->iLim = iLim; +} + +/** + * Bound input value between limits + */ +static float bound(float val, float range) +{ + if(val < -range) { + val = -range; + } else if(val > range) { + val = range; + } + return val; +} + diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h new file mode 100644 index 000000000..71f9eb56e --- /dev/null +++ b/flight/Libraries/math/pid.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file pid.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Methods to work with PID structure + * + * @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 PID_H +#define PID_H + +//! +struct pid { + float p; + float i; + float d; + float iLim; + float iAccumulator; + float lastErr; +}; + +//! Methods to use the pid structures +float pid_apply(struct pid *pid, const float err, float dT); +void pid_zero(struct pid *pid); +void pid_configure(struct pid *pid, float p, float i, float d, float iLim); + +#endif /* PID_H */ \ No newline at end of file diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index d36505d1d..188f556c8 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -43,7 +43,10 @@ #include "gyros.h" #include "flightstatus.h" #include "manualcontrol.h" // Just to get a macro + +// Math libraries #include "CoordinateConversions.h" +#include "pid.h" // Includes for various stabilization algorithms #include "relay_tuning.h" @@ -61,17 +64,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define FAILSAFE_TIMEOUT_MS 30 -enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX}; - -// Private types -typedef struct { - float p; - float i; - float d; - float iLim; - float iAccumulator; - float lastErr; -} pid_type; +enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX}; // Private variables static xTaskHandle taskHandle; @@ -79,18 +72,16 @@ static StabilizationSettingsData settings; static xQueueHandle queue; float gyro_alpha = 0; float axis_lock_accum[3] = {0,0,0}; -float vbar_sensitivity[3] = {1, 1, 1}; uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; float vbar_decay = 0.991f; -pid_type pids[PID_MAX]; +struct pid pids[PID_MAX]; // Private functions static void stabilizationTask(void* parameters); -static float ApplyPid(pid_type * pid, const float err, float dT); static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); @@ -251,7 +242,7 @@ static void stabilizationTask(void* parameters) 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] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -263,11 +254,11 @@ static void stabilizationTask(void* parameters) } // Compute the outer loop - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = pid_apply(&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] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -291,7 +282,7 @@ static void stabilizationTask(void* parameters) // 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] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -308,12 +299,12 @@ static void stabilizationTask(void* parameters) // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); - actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -333,7 +324,7 @@ static void stabilizationTask(void* parameters) pids[PID_ROLL + i].iAccumulator = 0; // Compute the outer loop like attitude mode - rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); // Run the relay controller which also estimates the oscillation parameters @@ -387,33 +378,15 @@ static void stabilizationTask(void* parameters) } } -/** - * 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); - 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++) { - pids[ct].iAccumulator = 0.0f; - pids[ct].lastErr = 0.0f; - } + for(uint32_t i = 0; i < PID_MAX; i++) + pid_zero(&pids[i]); + for(uint8_t i = 0; i < 3; i++) axis_lock_accum[i] = 0.0f; } @@ -437,54 +410,37 @@ 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]; + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], + 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]; + pid_configure(&pids[PID_RATE_PITCH], 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]; + pid_configure(&pids[PID_RATE_YAW], 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]; + pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], + settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, + 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]; + pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], + pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, + 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]; - - // Set the pitch attitude PI constants - pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP]; - pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI]; - - // 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]; + pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; From fc2f8376bc1dbf56279e72296fb1bd6fd0240358 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 04:31:17 -0500 Subject: [PATCH 27/43] Prevent an alarm from ManualControlCommand when using autotuning mode. --- flight/Modules/ManualControl/inc/manualcontrol.h | 5 +++-- flight/Modules/ManualControl/manualcontrol.c | 4 ++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/flight/Modules/ManualControl/inc/manualcontrol.h b/flight/Modules/ManualControl/inc/manualcontrol.h index f71cdebde..b67366f92 100644 --- a/flight/Modules/ManualControl/inc/manualcontrol.h +++ b/flight/Modules/ManualControl/inc/manualcontrol.h @@ -32,7 +32,7 @@ #include "manualcontrolcommand.h" -typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path; +typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path; #define PARSE_FLIGHT_MODE(x) ( \ (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ @@ -41,7 +41,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \ + (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ + (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \ ) int32_t ManualControlInitialize(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 22ced4c82..02c54b4a3 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -388,6 +388,10 @@ static void manualControlTask(void *parameters) case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; case FLIGHTMODE_GUIDANCE: switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: From 3982ad0046fe1e59512e2cfb547a6a15cfd1aabb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 04:32:59 -0500 Subject: [PATCH 28/43] Enable AUTOTUNE by default for CC --- flight/CopterControl/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 7a56df860..8b74935c4 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -68,7 +68,7 @@ USE_GPS ?= YES USE_TXPID ?= YES USE_I2C ?= NO USE_ALTITUDE ?= NO -USE_AUTOTUNE ?= NO +USE_AUTOTUNE ?= YES TEST_FAULTS ?= NO # List of optional modules to include From 490955dbeaaaf016bcad0814adffaad3f2b21851 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 3 Aug 2012 11:34:05 -0500 Subject: [PATCH 29/43] Use cos_lookup instead of sin_looup(x+90) --- flight/Modules/Stabilization/relay_tuning.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 16b4351fd..5dc538b4a 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -100,12 +100,12 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) // 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]; + int32_t dT = thisTime - lastHighTime; + int32_t phase = (360 * dT) / relay.Period[axis]; if(phase >= 360) - phase = 1; + phase = 0; accum_sin += sin_lookup_deg(phase) * error; - accum_cos += sin_lookup_deg(phase + 90) * error; + accum_cos += cos_lookup_deg(phase) * error; accumulated ++; // Make sure we've had enough time since last transition then check for a change in the output From 9f3c8dddd39b503a5e1b085e7a62397ddf07c68a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 3 Aug 2012 12:15:57 -0500 Subject: [PATCH 30/43] Add a version of the sin lookup table that is in ram instead of flash --- flight/Libraries/math/sin_lookup.c | 50 ++++++++++++++++++++ flight/Libraries/math/sin_lookup.h | 1 + flight/Modules/Stabilization/relay_tuning.c | 4 +- flight/Modules/Stabilization/stabilization.c | 10 ++-- 4 files changed, 60 insertions(+), 5 deletions(-) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index 575f171d3..2a5508fce 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -28,9 +28,14 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include "openpilot.h" #include "math.h" +#include "stdbool.h" #include "stdint.h" +#ifdef FLASH_TABLE + /** Version of the code which precomputes the lookup table in flash **/ + // This is a precomputed sin lookup table over 90 degrees that const float sin_table[] = { 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, @@ -44,6 +49,11 @@ 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f }; +int sin_lookup_initalize() +{ + return 0; +} + /** * Use the lookup table to return sine(angle) where angle is in radians * to save flash this cheats and uses trig functions to only save @@ -66,6 +76,46 @@ float sin_lookup_deg(float angle) return 0; } +#else +/** Version of the code which allocates the lookup table in heap **/ + +const int SIN_RESOLUTION = 180; + +static float *sin_table; +int sin_lookup_initalize() +{ + // Previously initialized + if (sin_table) + return 0; + + sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); + if (sin_table == NULL) + return -1; + + for(uint32_t i = 0; i < 180; i++) + sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f); + + return 0; +} + + + +/** + * Uses the lookup table to calculate sine (angle is in degrees) + * @param[in] angle in degrees + * @returns sin(angle) + */ +float sin_lookup_deg(float angle) { + int i_ang = ((int32_t)angle) % 360; + if (i_ang > 180) + return - sin_table[i_ang-180]; + else + return sin_table[i_ang]; +} + +#endif + + /** * Get cos(angle) using the sine lookup table * @param[in] angle Angle in degrees diff --git a/flight/Libraries/math/sin_lookup.h b/flight/Libraries/math/sin_lookup.h index 0e0edb274..adc29f66a 100644 --- a/flight/Libraries/math/sin_lookup.h +++ b/flight/Libraries/math/sin_lookup.h @@ -31,6 +31,7 @@ #ifndef SIN_LOOKUP_H #define SIN_LOOKUP_H +int sin_lookup_initalize(); float sin_lookup_deg(float angle); float cos_lookup_deg(float angle); float sin_lookup_rad(float angle); diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 5dc538b4a..0f0bb9d55 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -101,11 +101,11 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) // Project the error onto a sine and cosine of the same frequency // to accumulate the average amplitude int32_t dT = thisTime - lastHighTime; - int32_t phase = (360 * dT) / relay.Period[axis]; + float phase = ((float)360 * (float)dT) / relay.Period[axis]; if(phase >= 360) phase = 0; accum_sin += sin_lookup_deg(phase) * error; - accum_cos += cos_lookup_deg(phase) * error; + accum_cos += sin_lookup_deg(phase + 90) * error; accumulated ++; // Make sure we've had enough time since last transition then check for a change in the output diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 188f556c8..c4cc801e8 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -47,6 +47,7 @@ // Math libraries #include "CoordinateConversions.h" #include "pid.h" +#include "sin_lookup.h" // Includes for various stabilization algorithms #include "relay_tuning.h" @@ -117,12 +118,15 @@ int32_t StabilizationInitialize() // Initialize variables StabilizationSettingsInitialize(); ActuatorDesiredInitialize(); - RelayTuningSettingsInitialize(); - RelayTuningInitialize(); #if defined(DIAGNOSTICS) RateDesiredInitialize(); #endif - + + // Code required for relay tuning + sin_lookup_initalize(); + RelayTuningSettingsInitialize(); + RelayTuningInitialize(); + return 0; } From f3dc2dc2ad43ff4364e60f7803edb612dc99c758 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 3 Aug 2012 12:31:15 -0500 Subject: [PATCH 31/43] Store 180 deg in flash now and even make the flash and ram version share a lookup method. Still don't get the same results. --- flight/Libraries/math/sin_lookup.c | 66 +++++++++++++----------------- 1 file changed, 28 insertions(+), 38 deletions(-) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index 2a5508fce..a536f3f34 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -33,6 +33,7 @@ #include "stdbool.h" #include "stdint.h" +//#define FLASH_TABLE #ifdef FLASH_TABLE /** Version of the code which precomputes the lookup table in flash **/ @@ -46,7 +47,16 @@ 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, - 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f + 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f, + 1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f, + 0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f, + 0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f, + 0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f, + 0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f, + 0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f, + 0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f, + 0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f, + 0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f }; int sin_lookup_initalize() @@ -54,28 +64,6 @@ int sin_lookup_initalize() return 0; } -/** - * Use the lookup table to return sine(angle) where angle is in radians - * to save flash this cheats and uses trig functions to only save - * 90 values - * @param[in] angle Angle in degrees - * @returns sin(angle) -*/ -float sin_lookup_deg(float angle) -{ - int i_ang = ((int32_t) angle) % 360; - if(i_ang > 270) // for 270 to 360 deg - return -sin_table[360 - i_ang]; - else if (i_ang > 180) // for 180 to 270 deg - return -sin_table[i_ang - 180]; - else if (i_ang > 90) // for 90 to 170 deg - return sin_table[180 - i_ang]; - else // for 0 to 90 deg - return sin_table[i_ang]; - - return 0; -} - #else /** Version of the code which allocates the lookup table in heap **/ @@ -98,23 +86,25 @@ int sin_lookup_initalize() return 0; } - - -/** - * Uses the lookup table to calculate sine (angle is in degrees) - * @param[in] angle in degrees - * @returns sin(angle) - */ -float sin_lookup_deg(float angle) { - int i_ang = ((int32_t)angle) % 360; - if (i_ang > 180) - return - sin_table[i_ang-180]; - else - return sin_table[i_ang]; -} - #endif +/** + * Use the lookup table to return sine(angle) where angle is in radians + * to save flash this cheats and uses trig functions to only save + * 90 values + * @param[in] angle Angle in degrees + * @returns sin(angle) +*/ +float sin_lookup_deg(float angle) +{ + int i_ang = ((int32_t) angle) % 360; + if (i_ang > 180) // for 180 to 270 deg + return -sin_table[i_ang - 180]; + else // for 0 to 90 deg + return sin_table[i_ang]; + + return 0; +} /** * Get cos(angle) using the sine lookup table From ccbbda1b519e88fce3b796476d5bfde928d86826 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 3 Aug 2012 12:41:00 -0500 Subject: [PATCH 32/43] Make default tuning mode Attitude to make it easier on people like me :) --- shared/uavobjectdefinition/relaytuningsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index abfa78a2a..4ee3cb11b 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -4,7 +4,7 @@ - + From 8eac518a9c5bb5445f02b58708018ce4bf14265e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 3 Aug 2012 12:43:50 -0500 Subject: [PATCH 33/43] If the sin table is empty don't attempt to use it and return 0. --- flight/Libraries/math/sin_lookup.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index a536f3f34..79798b3d4 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -97,6 +97,9 @@ int sin_lookup_initalize() */ float sin_lookup_deg(float angle) { + if (sin_table == NULL) + return 0; + int i_ang = ((int32_t) angle) % 360; if (i_ang > 180) // for 180 to 270 deg return -sin_table[i_ang - 180]; From d6c485459ff570e71d3ca9a2195f7da825fd89ff Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 4 Aug 2012 00:31:45 -0500 Subject: [PATCH 34/43] Fix error Stac caught in sin_lookup --- flight/Libraries/math/sin_lookup.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index 79798b3d4..4444d10d2 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -101,9 +101,9 @@ float sin_lookup_deg(float angle) return 0; int i_ang = ((int32_t) angle) % 360; - if (i_ang > 180) // for 180 to 270 deg + if (i_ang >= 180) // for 180 to 360 deg return -sin_table[i_ang - 180]; - else // for 0 to 90 deg + else // for 0 to 179 deg return sin_table[i_ang]; return 0; From 096f940fee2ea037d3ec701fe5ed48630f01d7db Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 4 Aug 2012 12:03:41 -0500 Subject: [PATCH 35/43] Reenable the flash version now Stac's fix is tested. --- flight/Libraries/math/sin_lookup.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c index 4444d10d2..1e703b87f 100644 --- a/flight/Libraries/math/sin_lookup.c +++ b/flight/Libraries/math/sin_lookup.c @@ -33,7 +33,7 @@ #include "stdbool.h" #include "stdint.h" -//#define FLASH_TABLE +#define FLASH_TABLE #ifdef FLASH_TABLE /** Version of the code which precomputes the lookup table in flash **/ From d201cad7689d16b845ca5b5a1e983f483e50247f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 4 Aug 2012 12:03:53 -0500 Subject: [PATCH 36/43] Connect correct signal to relay tuning so the UI populates the calculated stabilization settings. --- ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index 6bef18130..33106be4c 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -31,7 +31,7 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); if(relayTuning) - connect(relayTuning, SIGNAL(updateRequested(UAVObject*)), this, SLOT(recomputeStabilization())); + connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization())); // Connect the apply button for the stabilization settings connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization())); From 8babe5d8a484e2e793e2c0e3580337e14e419052 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 24 Aug 2012 21:32:11 -0500 Subject: [PATCH 37/43] Autotuning: Increase the measurement time to 30 seconds. --- flight/Modules/Autotune/autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c index ec44a7b72..96051b076 100644 --- a/flight/Modules/Autotune/autotune.c +++ b/flight/Modules/Autotune/autotune.c @@ -137,7 +137,7 @@ static void AutotuneTask(void *parameters) portTickType diffTime; const uint32_t PREPARE_TIME = 2000; - const uint32_t MEAURE_TIME = 10000; + const uint32_t MEAURE_TIME = 30000; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); From a3f1894cd775afabc6c318f2cb2ea66f0f1563bb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 24 Aug 2012 21:33:08 -0500 Subject: [PATCH 38/43] Autotuning: Make the estimator require the value to toggle above and below the hysteresis level. --- flight/Modules/Stabilization/relay_tuning.c | 18 ++++++++++++++---- .../relaytuningsettings.xml | 1 + 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 0f0bb9d55..835786a02 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -53,7 +53,6 @@ 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; @@ -68,6 +67,10 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) static bool rateRelayRunning[MAX_AXES]; + // This indicates the current estimate of the smoothed error. So when it is high + // we are waiting for it to go low. + static bool high = false; + // On first run initialize estimates to something reasonable if(reinit) { rateRelayRunning[axis] = false; @@ -109,8 +112,11 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) 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 */ + bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; + + if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){ + /* POSITIVE CROSSING DETECTED */ + float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; float this_gain = this_amplitude / relaySettings.Amplitude; @@ -130,9 +136,13 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) lastHighTime = thisTime; high = true; RelayTuningSet(&relay); - } else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */ + + } else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) { + /* FALLING CROSSING DETECTED */ + lastLowTime = thisTime; high = false; + } return 0; diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index 4ee3cb11b..58718a53d 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -4,6 +4,7 @@ + From 59d1367d37a1c5086b4fd0aaf07dc75df6c25715 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 24 Aug 2012 22:35:21 -0500 Subject: [PATCH 39/43] Autotuning: Make the output also follow the hysteresis output --- flight/Modules/Stabilization/relay_tuning.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 835786a02..5d53aa7d3 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -93,7 +93,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) RelayTuningSettingsGet(&relaySettings); // Compute output, simple threshold on error - *output = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; + *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; /**** The code below here is to estimate the properties of the oscillation ****/ From 8d70a632fef53e3a53ab807b01aa119d5ea80020 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 24 Aug 2012 23:06:41 -0500 Subject: [PATCH 40/43] Autotune: Increase the default step size for autotuning. --- shared/uavobjectdefinition/relaytuningsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index 58718a53d..df28c8575 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -3,7 +3,7 @@ Setting to run a relay tuning algorithm - + From 8ca99739ed4c97ae0d03f383386a8d293dbe3ec6 Mon Sep 17 00:00:00 2001 From: Igor Van Airde Date: Sun, 9 Sep 2012 13:07:25 +0200 Subject: [PATCH 41/43] Changed rate controller from PID to PIDT1 to improve flight stability. Low pass on D-Term makes the D-Term usable to improve flight stability. Affects rate and stabilize mode. --- flight/Modules/Stabilization/stabilization.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 43bb9b0d5..ae02dbeaa 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -68,6 +68,7 @@ typedef struct { float iLim; float iAccumulator; float lastErr; + float lastDer; } pid_type; // Private variables @@ -405,6 +406,7 @@ static void stabilizationTask(void* parameters) float ApplyPid(pid_type * pid, const float err, float dT) { float diff = (err - pid->lastErr); + float dterm = 0; pid->lastErr = err; // Scale up accumulator by 1000 while computing to avoid losing precision @@ -414,7 +416,16 @@ float ApplyPid(pid_type * pid, const float err, float dT) } else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) { pid->iAccumulator = -pid->iLim * 1000.0f; } - return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); + + // Calculate DT1 term, fixed T1 timeconstant + if(pid->d) + { + dterm = pid->lastDer + (( dT / ( dT + 7.9577e-3)) * ((diff * pid->d / dT) - pid->lastDer)); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + // 7.9577e-3 means 20 Hz f_cutoff + } + + return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); } @@ -423,6 +434,7 @@ static void ZeroPids(void) for(int8_t ct = 0; ct < PID_MAX; ct++) { pids[ct].iAccumulator = 0.0f; pids[ct].lastErr = 0.0f; + pids[ct].lastDer = 0.0f; } for(uint8_t i = 0; i < 3; i++) axis_lock_accum[i] = 0.0f; From fa9a616b4ccf4be6dcbb1fc6efea3ce46bcb3b60 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 10 Sep 2012 03:10:41 -0500 Subject: [PATCH 42/43] PID: Add the 20 Hz low pass filter to the derivative term --- flight/Libraries/math/pid.c | 12 +++++++++++- flight/Libraries/math/pid.h | 1 + 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c index 58f15ad1f..8bb0e6013 100644 --- a/flight/Libraries/math/pid.c +++ b/flight/Libraries/math/pid.c @@ -37,12 +37,21 @@ static float bound(float val, float range); float pid_apply(struct pid *pid, const float err, float dT) { float diff = (err - pid->lastErr); + float dterm = 0; pid->lastErr = err; // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); - return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); + + // Calculate DT1 term, fixed T1 timeconstant + if(pid->d && dT) + { + dterm = pid->lastDer + dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer); + pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) + } // 7.9577e-3 means 20 Hz f_cutoff + + return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); } /** @@ -56,6 +65,7 @@ void pid_zero(struct pid *pid) pid->iAccumulator = 0; pid->lastErr = 0; + pid->lastDer = 0; } /** diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h index 71f9eb56e..e817cab57 100644 --- a/flight/Libraries/math/pid.h +++ b/flight/Libraries/math/pid.h @@ -39,6 +39,7 @@ struct pid { float iLim; float iAccumulator; float lastErr; + float lastDer; }; //! Methods to use the pid structures From 4d87af38b655896dd2b7fddb6dde49503cfeed1d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 10 Sep 2012 10:49:27 -0500 Subject: [PATCH 43/43] Make sure the orders of the UAVOs match --- shared/uavobjectdefinition/flightstatus.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index ba24d9bff..46b4a299c 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - +