From 09e2c6f00e09fac5b2d5f5e4a09f0671841ed2b4 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Tue, 28 Apr 2015 21:54:58 +1000 Subject: [PATCH 1/3] OP-1863 RateTrainer Initial commit for code review --- flight/modules/Stabilization/outerloop.c | 37 ++++++++++++++++++ flight/modules/Stabilization/stabilization.c | 4 ++ .../flightmodesettings.xml | 38 +++++++++---------- .../stabilizationdesired.xml | 2 +- .../stabilizationstatus.xml | 2 +- 5 files changed, 62 insertions(+), 21 deletions(-) diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 995003208..0c670efc7 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -120,6 +120,7 @@ static void stabilizationOuterloopTask() case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: rpy_desired[t] = stabilizationDesiredAxis[t]; break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: default: rpy_desired[t] = ((float *)&attitudeState.Roll)[t]; @@ -148,6 +149,8 @@ static void stabilizationOuterloopTask() } #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ } + + for (t = 0; t < AXES; t++) { bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t]; @@ -239,6 +242,40 @@ static void stabilizationOuterloopTask() rateDesiredAxis[t] = rate_input + weak_leveling; } break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes + // now test limits for pitch and/or roll + if (t == 1) { // pitch + if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } + } else if (t == 0) { // roll + if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { + // attitude exceeds roll max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { + // attitude exceeds roll max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: default: rateDesiredAxis[t] = stabilizationDesiredAxis[t]; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index efe727d33..0247e89e6 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -137,6 +137,10 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER: + StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS; + StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; + break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 223fa169f..877e94a32 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -7,69 +7,69 @@ diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index 2b8af749e..fcd11977e 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index c4533f227..5126ed9c1 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 From ca03092fb492adbe7afabea01355adec56d03553 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 4 May 2015 08:40:06 +1000 Subject: [PATCH 2/3] OP-1863 ratetrainer update stab handler to support new outerloop directwithlimits mode --- flight/modules/ManualControl/stabilizedhandler.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 4621a7171..6584bd79d 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -123,6 +123,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? 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 : @@ -134,6 +135,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? 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 : @@ -155,6 +157,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? 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 : From 2ec40072ab3a84c6465f26741e45207035bd81ed Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 4 May 2015 17:36:04 +1000 Subject: [PATCH 3/3] OP-1863 ratetrainer fix coding error. ratetrainer should now work. --- flight/modules/ManualControl/stabilizedhandler.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index b927445a3..e2361b1f9 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -123,7 +123,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? 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 : @@ -135,7 +135,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? 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 : @@ -157,7 +157,7 @@ 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] == STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? 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 :