rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Modules/Stabilization, Modules/ManualControl:

Browse Source 
Move channel StabilizationSettings from ManualControlCommand to
AttitudeDesired, unify channel normalization and put them all into ManualControl

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2797 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2011-02-15 14:55:00 +00:00 committed by corvus
parent 9899c270cb
commit 435072cf4e
3 changed files with 48 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
flight/Modules/ManualControl/manualcontrol.c
View File

@ -458,10 +458,43 @@ static void manualControlTask(void *parameters)
actuator.Throttle = cmd.Throttle;
ActuatorDesiredSet(&actuator);
} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {
attitude.Roll = cmd.Roll * stabSettings.RollMax;
attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ROLL]) {
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
attitude.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
attitude.Roll = cmd.Roll * stabSettings.RollMax;
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
attitude.Roll = cmd.Roll;
break;
}
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_PITCH]) {
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
attitude.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
attitude.Pitch = cmd.Pitch;
break;
}
switch(cmd.StabilizationSettings[MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_YAW]) {
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
attitude.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
attitude.Yaw = cmd.Yaw;
break;
}
attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle;
for(int i = 0; i < 3; i++) {
attitude.StabilizationSettings[i] = cmd.StabilizationSettings[i];
}
AttitudeDesiredSet(&attitude);
}
}

41
flight/Modules/Stabilization/stabilization.c
View File

@ -155,7 +155,6 @@ static void stabilizationTask(void* parameters)
SystemSettingsGet(&systemSettings);
float *manualAxis = &manualControl.Roll;
float *attitudeDesiredAxis = &attitudeDesired.Roll;
float *attitudeActualAxis = &attitudeActual.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
@ -164,19 +163,19 @@ static void stabilizationTask(void* parameters)
//Calculate desired rate
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
switch(manualControl.StabilizationSettings[ct])
switch(attitudeDesired.StabilizationSettings[ct])
{
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
rateDesiredAxis[ct] = manualAxis[ct] * settings.ManualRate[ct];
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE:
rateDesiredAxis[ct] = attitudeDesiredAxis[ct];
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE:
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1);
break;
}
}
uint8_t shouldUpdate = 0;
uint8_t shouldUpdate = 1;
RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
@ -193,36 +192,18 @@ static void stabilizationTask(void* parameters)
}
}
switch(manualControl.StabilizationSettings[ct])
switch(attitudeDesired.StabilizationSettings[ct])
{
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_RATE:
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_ATTITUDE:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0);
actuatorDesiredAxis[ct] = bound(command);
shouldUpdate = 1;
break;
}
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE:
//actuatorDesiredAxis[ct] = bound(manualAxis[ct]);
//shouldUpdate = 1;
switch (ct)
{
case ROLL:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]/settings.RollMax);
shouldUpdate = 1;
break;
case PITCH:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]/settings.PitchMax);
shouldUpdate = 1;
break;
case YAW:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]/180);
shouldUpdate = 1;
break;
}
break;
case ATTITUDEDESIRED_STABILIZATIONSETTINGS_NONE:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
break;
}
}

1
shared/uavobjectdefinition/attitudedesired.xml
View File

@ -5,6 +5,7 @@
<field name="Pitch" units="degrees" type="float" elements="1"/>
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<field name="StabilizationSettings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>