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

Add ability to run guidance based on raw GPS velocity instead

Browse Source
This commit is contained in:
James Cotton 2012-04-14 13:34:30 -05:00
parent 1a98a46606
commit be21ec441e
2 changed files with 36 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
flight/Modules/Guidance/guidance.c
View File

@ -51,6 +51,8 @@
#include "positionactual.h" #include "positionactual.h"
#include "manualcontrol.h" #include "manualcontrol.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "guidancesettings.h" #include "guidancesettings.h"
#include "nedaccel.h" #include "nedaccel.h"
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
@ -64,6 +66,7 @@
#define MAX_QUEUE_SIZE 4 #define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548 #define STACK_SIZE_BYTES 1548
#define TASK_PRIORITY (tskIDLE_PRIORITY+2) #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
#define F_PI 3.14159265358979323846f
// Private types // Private types
// Private variables // Private variables
@ -255,7 +258,6 @@ void updateVtolDesiredVelocity()
float northCommand; float northCommand;
float eastCommand; float eastCommand;
float downCommand; float downCommand;
// Check how long since last update // Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
@ -346,13 +348,40 @@ static void updateVtolDesiredAttitude()
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel); NedAccelGet(&nedAccel);
float northVel, eastVel, downVel;
switch (guidanceSettings.VelocitySource) {
case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
break;
case GUIDANCESETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
northVel = gpsVelocity.North;
eastVel = gpsVelocity.East;
downVel = gpsVelocity.Down;
}
case GUIDANCESETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
downVel = velocityActual.Down;
}
default:
break;
}
// Testing code - refactor into manual control command // Testing code - refactor into manual control command
ManualControlCommandData manualControlData; ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData); ManualControlCommandGet(&manualControlData);
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
// Compute desired north command // Compute desired north command
northError = velocityDesired.North - velocityActual.North; northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT], -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]); guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
@ -361,7 +390,7 @@ static void updateVtolDesiredAttitude()
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]); nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired east command // Compute desired east command
eastError = velocityDesired.East - velocityActual.East; eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT], -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]); guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
@ -370,7 +399,7 @@ static void updateVtolDesiredAttitude()
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]); nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired down command // Compute desired down command
downError = velocityDesired.Down - velocityActual.Down; downError = velocityDesired.Down - downVel;
// Must flip this sign // Must flip this sign
downError = -downError; downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI], downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI],

4
shared/uavobjectdefinition/guidancesettings.xml
View File

@ -5,10 +5,12 @@
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/> <field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/> <field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="1,0,0"/> <field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="1,0,0"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="10,0,1,0"/> <field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="5,0,1,0"/>
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,200"/> <field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,200"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/> <field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/>
<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/> <field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VelocitySource" units="" type="enum" elements="1" options="EKF,NEDVEL,GPSPOS" defaultvalue="EKF"/>
<field name="PositionSource" units="" type="enum" elements="1" options="EKF,GPSPOS" defaultvalue="EKF"/>
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/> <field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
<field name="UpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/> <field name="UpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>