From 17878b32f30ebe6dce7db72bae3a4ca6df111be1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 09:49:17 -0500 Subject: [PATCH] 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)