From 93f82652b716f221dd70f9567a1fe2ce9f6c5319 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 27 Apr 2014 15:15:59 +0200 Subject: [PATCH] OP-1309 outer loop fully implemented - some cleanups and changes in control flow to accomodate new logic --- .../modules/ManualControl/stabilizedhandler.c | 12 +- .../modules/Stabilization/inc/stabilization.h | 3 +- flight/modules/Stabilization/innerloop.c | 51 ++++-- flight/modules/Stabilization/outerloop.c | 152 +++++++++++++++--- .../revolution/firmware/inc/pios_config.h | 2 +- .../stabilizationsettings.xml | 1 + .../stabilizationstatus.xml | 4 +- 7 files changed, 180 insertions(+), 45 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index e98de70d4..a1da0e406 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -88,11 +88,11 @@ void stabilizedHandler(bool newinit) stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Roll * stabSettings.RollMax : 0; // this is an invalid mode @@ -100,11 +100,11 @@ void stabilizedHandler(bool newinit) stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : 0; // this is an invalid mode @@ -122,11 +122,11 @@ void stabilizedHandler(bool newinit) stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax : 0; // this is an invalid mode diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index d9962309b..d1e7e34ee 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -49,7 +49,8 @@ typedef struct { float cruise_control_power_trim; int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!! float cruise_control_neutral_thrust; - } cruiseControl; + } cruiseControl; + float rattitude_mode_transition_stick_position; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 44a16d733..1ee8323cd 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -65,9 +65,11 @@ static DelayedCallbackInfo *callbackHandle; static struct pid pids[3]; static float gyro_filtered[3] = { 0, 0, 0 }; +static float axis_lock_accum[3] = { 0, 0, 0 }; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; static float speedScaleFactor = 1.0f; +static StabilizationBankData stabBank; // Private functions static void stabilizationInnerloopTask(); @@ -133,9 +135,30 @@ static void stabilizationInnerloopTask() stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); break; case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING: + rate[t] = boundf(rate[t], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + ); stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit); break; + case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK: + if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) { + // While getting strong commands act like rate mode + axis_lock_accum[t] = 0; + } else { + // For weaker commands or no command simply attitude lock (almost) on no gyro change + axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT; + axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock); + rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp; + } + // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication! + // keep order as it is, RATE must follow! case STABILIZATIONSTATUS_INNERLOOP_RATE: + // limit rate to maximum configured limits (once here instead of 5 times in outer loop) + rate[t] = boundf(rate[t], + -cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t], + cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t] + ); actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: @@ -186,27 +209,25 @@ static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank; - - StabilizationBankGet(&bank); + StabilizationBankGet(&stabBank); // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], stabBank.RollRatePID.Kp, + stabBank.RollRatePID.Ki, + stabBank.RollRatePID.Kd, + stabBank.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], stabBank.PitchRatePID.Kp, + stabBank.PitchRatePID.Ki, + stabBank.PitchRatePID.Kd, + stabBank.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], stabBank.YawRatePID.Kp, + stabBank.YawRatePID.Ki, + stabBank.YawRatePID.Kd, + stabBank.YawRatePID.ILimit); } #ifdef REVOLUTION diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 52db1a1b5..1943a9c6a 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -37,14 +37,14 @@ #include #include #include -// #include -// #include +#include #include -// #include #include + #include #include +#include // Private constants #define STACK_SIZE_BYTES 256 @@ -63,6 +63,8 @@ static DelayedCallbackInfo *callbackHandle; static struct pid pids[3]; static AttitudeStateData attitude; +static StabilizationBankData stabBank; + static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; @@ -94,6 +96,7 @@ void stabilizationOuterloopInit() */ static void stabilizationOuterloopTask() { + AttitudeStateData attitudeState; RateDesiredData rateDesired; StabilizationDesiredData stabilizationDesired; StabilizationStatusOuterLoopData enabled; @@ -104,7 +107,41 @@ static void stabilizationOuterloopTask() float *stabilizationDesiredAxis = &stabilizationDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; int t; - // float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + float local_error[3]; + { +#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]; + + rpy_desired[0] = stabilizationDesiredAxis[0]; + rpy_desired[1] = stabilizationDesiredAxis[1]; + rpy_desired[2] = stabilizationDesiredAxis[2]; + + RPY2Quaternion(rpy_desired, q_desired); + quat_inverse(q_desired); + quat_mult(q_desired, &attitudeState.q1, q_error); + quat_inverse(q_error); + Quaternion2RPY(q_error, local_error); + +#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ + // Simpler algorithm for CC, less memory + local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll; + local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch; + local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw; + + // find shortest way + float modulo = fmodf(local_error[2] + 180.0f, 360.0f); + if (modulo < 0) { + local_error[2] = modulo + 180.0f; + } else { + local_error[2] = modulo - 180.0f; + } +#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ + } for (t = 0; t < AXES; t++) { bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); @@ -115,7 +152,81 @@ static void stabilizationOuterloopTask() pids[t].iAccumulator = 0; } switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { - case STABILIZATIONSTATUS_OUTERLOOP_MANUAL: + case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: + rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabBank.YawMax, -1.0f, 1.0f); + float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t]; + rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT); + // Compute the weighted average rate desired + // Using max() rather than sqrt() for cpu speed; + // - this makes the stick region into a square; + // - this is a feature! + // - hold a roll angle and add just pitch without the stick sensitivity changing + float magnitude = stickinput[t]; + if (t < 2) { + magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); + } + + // modify magnitude to move the Att to Rate transition to the place + // specified by the user + // we are looking for where the stick angle == transition angle + // and the Att rate equals the Rate rate + // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] + // == Rate x StickAngle [Rate pulling up according to stick angle] + // * StickAngle [X Ratt proportion] + // so 1-x == x*x or x*x+x-1=0 where xE(0,1) + // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 + // and quadratic formula says that is 0.618033989f + // I tested 14.01 and came up with .61 without even remembering this number + // I thought that moving the P,I, and maxangle terms around would change this value + // and that I would have to take these into account, but varying + // all P's and I's by factors of 1/2 to 2 didn't change it noticeably + // and varying maxangle from 4 to 120 didn't either. + // so for now I'm not taking these into account + // while working with this, it occurred to me that Attitude mode, + // set up with maxangle=190 would be similar to Ratt, and it is. + #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f + + // the following assumes the transition would otherwise be at 0.618033989f + // and THAT assumes that Att ramps up to max roll rate + // when a small number of degrees off of where it should be + + // if below the transition angle (still in attitude mode) + // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { + magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; + } else { + magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) + * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) + / (1.0f - stabSettings.rattitude_mode_transition_stick_position) + + STICK_VALUE_AT_MODE_TRANSITION; + } + rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; + } + break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + // FIXME: local_error[] is rate - attitude for Weak Leveling + // The only ramifications are: + // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS + // Changing Rate mode max rate currently requires a change to Kp + // That would be changed to Attitude mode max angle affecting Kp + // Also does not take dT into account + { + float rate_input = cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabBank, stabBank.RollMax)[t]; + float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; + weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); + + // Compute desired rate as input biased towards leveling + rateDesiredAxis[t] = rate_input + weak_leveling; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: default: rateDesiredAxis[t] = stabilizationDesiredAxis[t]; break; @@ -130,6 +241,9 @@ static void stabilizationOuterloopTask() } RateDesiredSet(&rateDesired); + + // update cruisecontrol based on attitude + cruisecontrol_compute_factor(&attitudeState); } @@ -143,27 +257,25 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - StabilizationBankData bank; - - StabilizationBankGet(&bank); + StabilizationBankGet(&stabBank); // Set the roll rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], bank.RollRatePID.Kp, - bank.RollRatePID.Ki, - bank.RollRatePID.Kd, - bank.RollRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], stabBank.RollRatePID.Kp, + stabBank.RollRatePID.Ki, + stabBank.RollRatePID.Kd, + stabBank.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], bank.PitchRatePID.Kp, - bank.PitchRatePID.Ki, - bank.PitchRatePID.Kd, - bank.PitchRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], stabBank.PitchRatePID.Kp, + stabBank.PitchRatePID.Ki, + stabBank.PitchRatePID.Kd, + stabBank.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], bank.YawRatePID.Kp, - bank.YawRatePID.Ki, - bank.YawRatePID.Kd, - bank.YawRatePID.ILimit); + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], stabBank.YawRatePID.Kp, + stabBank.YawRatePID.Ki, + stabBank.YawRatePID.Kd, + stabBank.YawRatePID.ILimit); } /** diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index c60e9a453..5b46f059a 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -144,7 +144,7 @@ #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ -/* #define PIOS_QUATERNION_STABILIZATION */ +#define PIOS_QUATERNION_STABILIZATION /* Performance counters */ #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 51d6497bd..a14279df9 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -22,6 +22,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index 89c6923cf..f58200853 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch @@ -11,7 +11,7 @@ Thrust - + Roll Pitch