rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'abeck/OP-1863-rate-maxpitch' into next

Browse Source
This commit is contained in:
abeck70 2015-05-05 21:28:56 +10:00
commit 30a70ca3ec
6 changed files with 65 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -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 :

37
flight/modules/Stabilization/outerloop.c
View File

@ -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];

4
flight/modules/Stabilization/stabilization.c
View File

@ -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;

38
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -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 -->

2
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -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"/>

2
shared/uavobjectdefinition/stabilizationstatus.xml
View File

@ -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>