mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
Refactored Guidance to apropriate pathfollower in uavobjects
This commit is contained in:
Corvus Corax
parent
25e21fde73
commit
63cd7b063f
@ -58,7 +58,7 @@
|
||||
#include "pathstatus.h"
|
||||
#include "gpsvelocity.h"
|
||||
#include "gpsposition.h"
|
||||
#include "guidancesettings.h"
|
||||
#include "vtolpathfollowersettings.h"
|
||||
#include "nedaccel.h"
|
||||
#include "nedposition.h"
|
||||
#include "stabilizationdesired.h"
|
||||
@ -80,7 +80,7 @@
|
||||
static bool followerEnabled = false;
|
||||
static xTaskHandle pathfollowerTaskHandle;
|
||||
static PathDesiredData pathDesired;
|
||||
static GuidanceSettingsData guidanceSettings;
|
||||
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
|
||||
|
||||
// Private functions
|
||||
static void vtolPathFollowerTask(void *parameters);
|
||||
@ -118,7 +118,7 @@ int32_t VtolPathFollowerInitialize()
|
||||
HwSettingsOptionalModulesGet(optionalModules);
|
||||
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
|
||||
followerEnabled = true;
|
||||
GuidanceSettingsInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
NedAccelInitialize();
|
||||
PathDesiredInitialize();
|
||||
PathStatusInitialize();
|
||||
@ -152,10 +152,10 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
|
||||
portTickType lastUpdateTime;
|
||||
|
||||
GuidanceSettingsConnectCallback(SettingsUpdatedCb);
|
||||
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
|
||||
PathDesiredConnectCallback(SettingsUpdatedCb);
|
||||
|
||||
GuidanceSettingsGet(&guidanceSettings);
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
// Main task loop
|
||||
@ -186,7 +186,7 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
}
|
||||
|
||||
// Continue collecting data if not enough time
|
||||
vTaskDelayUntil(&lastUpdateTime, guidanceSettings.UpdatePeriod / portTICK_RATE_MS);
|
||||
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
|
||||
|
||||
// Convert the accels into the NED frame
|
||||
updateNedAccel();
|
||||
@ -260,7 +260,7 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
*/
|
||||
static void updatePathVelocity()
|
||||
{
|
||||
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
float downCommand;
|
||||
|
||||
PositionActualData positionActual;
|
||||
@ -280,14 +280,14 @@ static void updatePathVelocity()
|
||||
velocityDesired.North = progress.path_direction[0] * groundspeed;
|
||||
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
||||
|
||||
float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP];
|
||||
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP];
|
||||
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
|
||||
progress.correction_direction[1] * error_speed};
|
||||
|
||||
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
|
||||
float scale = 1;
|
||||
if(total_vel > guidanceSettings.HorizontalVelMax)
|
||||
scale = guidanceSettings.HorizontalVelMax / total_vel;
|
||||
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
|
||||
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
|
||||
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
|
||||
@ -296,13 +296,13 @@ static void updatePathVelocity()
|
||||
bound(progress.fractional_progress,0,1);
|
||||
|
||||
float downError = altitudeSetpoint - positionActual.Down;
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
|
||||
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-guidanceSettings.VerticalVelMax,
|
||||
guidanceSettings.VerticalVelMax);
|
||||
-vtolpathfollowerSettings.VerticalVelMax,
|
||||
vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -315,7 +315,7 @@ static void updatePathVelocity()
|
||||
*/
|
||||
void updateEndpointVelocity()
|
||||
{
|
||||
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
PositionActualData positionActual;
|
||||
VelocityDesiredData velocityDesired;
|
||||
@ -331,7 +331,7 @@ void updateEndpointVelocity()
|
||||
float downCommand;
|
||||
|
||||
float northPos = 0, eastPos = 0, downPos = 0;
|
||||
switch (guidanceSettings.PositionSource) {
|
||||
switch (vtolpathfollowerSettings.PositionSource) {
|
||||
case GUIDANCESETTINGS_POSITIONSOURCE_EKF:
|
||||
northPos = positionActual.North;
|
||||
eastPos = positionActual.East;
|
||||
@ -353,36 +353,36 @@ void updateEndpointVelocity()
|
||||
|
||||
// Compute desired north command
|
||||
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
|
||||
northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
|
||||
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
|
||||
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
|
||||
northPosIntegral);
|
||||
|
||||
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
|
||||
eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
|
||||
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
|
||||
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
|
||||
eastPosIntegral);
|
||||
|
||||
// Limit the maximum velocity
|
||||
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
|
||||
float scale = 1;
|
||||
if(total_vel > guidanceSettings.HorizontalVelMax)
|
||||
scale = guidanceSettings.HorizontalVelMax / total_vel;
|
||||
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
|
||||
velocityDesired.North = northCommand * scale;
|
||||
velocityDesired.East = eastCommand * scale;
|
||||
|
||||
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
|
||||
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-guidanceSettings.VerticalVelMax,
|
||||
guidanceSettings.VerticalVelMax);
|
||||
-vtolpathfollowerSettings.VerticalVelMax,
|
||||
vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -414,14 +414,14 @@ static void updateVtolFixedAttitude(float* attitude)
|
||||
*/
|
||||
static void updateVtolDesiredAttitude()
|
||||
{
|
||||
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityActualData velocityActual;
|
||||
StabilizationDesiredData stabDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
NedAccelData nedAccel;
|
||||
GuidanceSettingsData guidanceSettings;
|
||||
VtolPathFollowerSettingsData vtolpathfollowerSettings;
|
||||
StabilizationSettingsData stabSettings;
|
||||
SystemSettingsData systemSettings;
|
||||
|
||||
@ -435,7 +435,7 @@ static void updateVtolDesiredAttitude()
|
||||
float downCommand;
|
||||
|
||||
SystemSettingsGet(&systemSettings);
|
||||
GuidanceSettingsGet(&guidanceSettings);
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
|
||||
VelocityActualGet(&velocityActual);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
@ -446,7 +446,7 @@ static void updateVtolDesiredAttitude()
|
||||
NedAccelGet(&nedAccel);
|
||||
|
||||
float northVel = 0, eastVel = 0, downVel = 0;
|
||||
switch (guidanceSettings.VelocitySource) {
|
||||
switch (vtolpathfollowerSettings.VelocitySource) {
|
||||
case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
|
||||
northVel = velocityActual.North;
|
||||
eastVel = velocityActual.East;
|
||||
@ -482,34 +482,34 @@ static void updateVtolDesiredAttitude()
|
||||
|
||||
// Compute desired north command
|
||||
northError = velocityDesired.North - northVel;
|
||||
northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
|
||||
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
||||
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
|
||||
-vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
||||
northVelIntegral -
|
||||
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.North * guidanceSettings.VelocityFeedforward);
|
||||
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired east command
|
||||
eastError = velocityDesired.East - eastVel;
|
||||
eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
|
||||
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
||||
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
|
||||
-vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
||||
eastVelIntegral -
|
||||
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.East * guidanceSettings.VelocityFeedforward);
|
||||
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired down command
|
||||
downError = velocityDesired.Down - downVel;
|
||||
// Must flip this sign
|
||||
downError = -downError;
|
||||
downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI],
|
||||
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
|
||||
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
|
||||
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
|
||||
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI],
|
||||
-vtolpathfollowerSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
|
||||
downVelIntegral -
|
||||
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
|
||||
nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
|
||||
|
||||
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
|
||||
|
||||
@ -517,12 +517,12 @@ static void updateVtolDesiredAttitude()
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
|
||||
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
|
||||
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
|
||||
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
|
||||
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
|
||||
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
|
||||
if(vtolpathfollowerSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
|
||||
// For now override throttle with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
@ -591,7 +591,7 @@ static float bound(float val, float min, float max)
|
||||
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
GuidanceSettingsGet(&guidanceSettings);
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
PathDesiredGet(&pathDesired);
|
||||
}
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<xml>
|
||||
<object name="GuidanceSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref GuidanceModule</description>
|
||||
<object name="FixedWingPathFollowerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref FixedWingPathFollowerModule</description>
|
||||
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
|
||||
<field name="PathMode" units="" type="enum" elements="1" options="ENDPOINT,PATH" defaultvalue="ENDPOINT"/>
|
||||
<field name="ReturnTobaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="75.0"/>
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
<xml>
|
||||
<object name="GuidanceStatus" singleinstance="true" settings="false">
|
||||
<object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
|
||||
<description>Object Storing Debugging Information on PID internals</description>
|
||||
<field name="E" units="" type="float" elementnames="Course,Speed,Accel,Power" />
|
||||
<field name="A" units="" type="float" elementnames="Course,Speed,Accel,Power" />
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<xml>
|
||||
<object name="GuidanceSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref GuidanceModule</description>
|
||||
<object name="VtolPathFollowerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref VtolPathFollowerModule</description>
|
||||
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
|
||||
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
|
||||
Loading…
x
Reference in New Issue
Block a user