Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1848r-altvario

Conflicts: flight/modules/Stabilization/outerloop.c
2024-11-29 07:24:13 +01:00 · 2015-05-05 21:47:19 +10:00 · 2015-05-05 21:47:19 +10:00 · 5bffc73b17
commit 5bffc73b17
parent c3c80d4aa8 7bad2f8163
7 changed files with 108 additions and 38 deletions
--- a/flight/libraries/plans.c
+++ b/flight/libraries/plans.c
@ -294,6 +294,14 @@ void plan_run_AutoTakeoff()
    break;

    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+        }
+    }
+    break;
    case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
    // nothing to do. land has been requested. stay here for forever until mode change.
    default:
--- 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] == 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 :
@ -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] == 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 :
@ -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] == 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 :
--- a/flight/modules/Stabilization/outerloop.c
+++ b/flight/modules/Stabilization/outerloop.c
@ -138,6 +138,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];
@ -262,27 +263,81 @@ static void stabilizationOuterloopTask()
        default:
            rateDesiredAxis[t] = stabilizationDesiredAxis[t];
            break;
-        }
-    }
-
-    RateDesiredSet(&rateDesired);
-    {
-        FlightStatusArmedOptions armed;
-        FlightStatusArmedGet(&armed);
-        float throttleDesired;
-        ManualControlCommandThrottleGet(&throttleDesired);
-        if (armed != FLIGHTSTATUS_ARMED_ARMED ||
-            ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
-            // Force all axes to reinitialize when engaged
-            for (t = 0; t < AXES; t++) {
-                previous_mode[t] = 255;
+        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];
+            break;
+        }
+    } else {
+        switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+#ifdef REVOLUTION
+        case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
+            rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
+            break;
+        case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
+            rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
+            break;
+#endif /* REVOLUTION */
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+        default:
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+            break;
        }
    }
+}

-    // update cruisecontrol based on attitude
-    cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
-    stabSettings.monitor.rateupdates = 0;
+RateDesiredSet(&rateDesired);
+{
+    FlightStatusArmedOptions armed;
+    FlightStatusArmedGet(&armed);
+    float throttleDesired;
+    ManualControlCommandThrottleGet(&throttleDesired);
+    if (armed != FLIGHTSTATUS_ARMED_ARMED ||
+        ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
+        // Force all axes to reinitialize when engaged
+        for (t = 0; t < AXES; t++) {
+            previous_mode[t] = 255;
+        }
+    }
+}
+
+// update cruisecontrol based on attitude
+cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
+stabSettings.monitor.rateupdates = 0;
 }


--- 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;
--- a/shared/uavobjectdefinition/flightmodesettings.xml
+++ b/shared/uavobjectdefinition/flightmodesettings.xml
@ -7,69 +7,69 @@
 	<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
        <field name="Stabilization1Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,AxisLock,Manual"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
-			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%NE:Rate::RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>
        <field name="Stabilization2Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,Rate,Manual"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>
        <field name="Stabilization3Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Rate,Rate,Rate,Manual"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>
        <field name="Stabilization4Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>
        <field name="Stabilization5Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Attitude,Attitude,Rate,CruiseControl"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>
        <field name="Stabilization6Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw,Thrust"
-		options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
+		options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"
 		defaultvalue="Rate,Rate,Rate,Manual"
 		limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl; \
 			%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
 			%NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
-			%0401NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
-			%0402NE:Rate:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
+			%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
+			%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
 			/>

 	<!-- Note these options values should be identical to those defined in FlightMode -->
--- a/shared/uavobjectdefinition/stabilizationdesired.xml
+++ b/shared/uavobjectdefinition/stabilizationdesired.xml
@ -6,7 +6,7 @@
        <field name="Yaw" units="degrees" type="float" elements="1"/>
        <field name="Thrust" units="%" type="float" elements="1"/>
 	<!-- These values should match those in FlightModeSettings.Stabilization{1,2,3}Settings -->
-        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"/>
+        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw,Thrust" options="Manual,Rate,RateTrainer,Attitude,AxisLock,WeakLeveling,VirtualBar,Acro+,Rattitude,AltitudeHold,AltitudeVario,CruiseControl"/>
        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="false" updatemode="manual" period="0"/>
        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
--- a/shared/uavobjectdefinition/stabilizationstatus.xml
+++ b/shared/uavobjectdefinition/stabilizationstatus.xml
@ -3,7 +3,7 @@
        <description>Contains status information to control submodules for stabilization.</description>


-	<field name="OuterLoop" units="" type="enum" options="Direct,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario">
+	<field name="OuterLoop" units="" type="enum" options="Direct,DirectWithLimits,Attitude,Rattitude,Weakleveling,Altitude,AltitudeVario">
 		<elementnames>
 			<elementname>Roll</elementname>
 			<elementname>Pitch</elementname>