1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00

Make the VtolPathFollower an optional module

Note that RTH mode right now checks that throttle control is enabled, but this
should only occur if the system is a VTOL.
This commit is contained in:
James Cotton 2012-06-06 01:33:30 -05:00
parent d54970a58a
commit 574ab3fcaa
7 changed files with 75 additions and 63 deletions

View File

@ -74,7 +74,6 @@ int32_t BatteryInitialize(void)
#ifdef MODULE_BATTERY_BUILTIN
batteryEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);

View File

@ -40,7 +40,7 @@
#include "baroaltitude.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "guidancesettings.h"
#include "vtolpathfollowersettings.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "manualcontrolcommand.h"
@ -672,8 +672,8 @@ static bool processFailsafe(bool valid_input_detected, ManualControlSettingsData
break;
case MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_RTH:
{
GuidanceSettingsData guidanceSettings;
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsData guidanceSettings;
VtolPathFollowerSettingsGet(&guidanceSettings);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
@ -682,7 +682,7 @@ static bool processFailsafe(bool valid_input_detected, ManualControlSettingsData
// already armed. It would be _really_ nice to have some indication we are actively
// flying to put here. I would like to check the throttle is engaged but that would
// exclude fixed wing.
if (guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_TRUE &&
if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_TRUE &&
flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
setRTH(true);
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_RTH;

View File

@ -49,13 +49,14 @@
#include "vtolpathfollower.h"
#include "accels.h"
#include "attitudeactual.h"
#include "hwsettings.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "guidancesettings.h"
#include "vtolpathfollowersettings.h"
#include "nedaccel.h"
#include "nedposition.h"
#include "stabilizationdesired.h"
@ -76,7 +77,7 @@
// Private variables
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static GuidanceSettingsData guidanceSettings;
static VtolPathFollowerSettingsData guidanceSettings;
// Private functions
static void vtolPathFollowerTask(void *parameters);
@ -86,6 +87,7 @@ static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateVtolDesiredAttitude();
static float bound(float val, float min, float max);
static bool vtolpathfollower_enabled;
/**
* Initialise the module, called on startup
@ -93,9 +95,11 @@ static float bound(float val, float min, float max);
*/
int32_t VtolPathFollowerStart()
{
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
if (vtolpathfollower_enabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
@ -106,10 +110,19 @@ int32_t VtolPathFollowerStart()
*/
int32_t VtolPathFollowerInitialize()
{
GuidanceSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
VelocityDesiredInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
VelocityDesiredInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
}
return 0;
}
@ -135,10 +148,10 @@ static void vtolPathFollowerTask(void *parameters)
portTickType lastUpdateTime;
GuidanceSettingsConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
@ -253,7 +266,7 @@ 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 * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
@ -269,10 +282,10 @@ 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 * guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
@ -305,12 +318,12 @@ void updateEndpointVelocity()
float northPos = 0, eastPos = 0, downPos = 0;
switch (guidanceSettings.PositionSource) {
case GUIDANCESETTINGS_POSITIONSOURCE_EKF:
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case GUIDANCESETTINGS_POSITIONSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
NEDPositionData nedPosition;
NEDPositionGet(&nedPosition);
@ -326,17 +339,17 @@ 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 * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_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 * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
// Limit the maximum velocity
@ -349,10 +362,10 @@ void updateEndpointVelocity()
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 * guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
@ -376,7 +389,7 @@ static void updateVtolDesiredAttitude()
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
VtolPathFollowerSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
@ -390,7 +403,7 @@ static void updateVtolDesiredAttitude()
float downCommand;
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
@ -402,12 +415,12 @@ static void updateVtolDesiredAttitude()
float northVel = 0, eastVel = 0, downVel = 0;
switch (guidanceSettings.VelocitySource) {
case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
break;
case GUIDANCESETTINGS_VELOCITYSOURCE_NEDVEL:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
@ -416,7 +429,7 @@ static void updateVtolDesiredAttitude()
downVel = gpsVelocity.Down;
}
break;
case GUIDANCESETTINGS_VELOCITYSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
@ -437,34 +450,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 * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
northVelIntegral -
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
nedAccel.North * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.North * guidanceSettings.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 * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
eastVelIntegral -
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
nedAccel.East * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.East * guidanceSettings.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 * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
downVelIntegral -
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
@ -477,7 +490,7 @@ static void updateVtolDesiredAttitude()
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
@ -546,7 +559,7 @@ static float bound(float val, float min, float max)
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
}

View File

@ -50,7 +50,7 @@ UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += guidancesettings
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

View File

@ -65,7 +65,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/mixerstatus.h \
$$UAVOBJECT_SYNTHETICS/velocitydesired.h \
$$UAVOBJECT_SYNTHETICS/velocityactual.h \
$$UAVOBJECT_SYNTHETICS/guidancesettings.h \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/ratedesired.h \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
$$UAVOBJECT_SYNTHETICS/i2cstats.h \
@ -133,7 +133,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \
$$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
$$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
$$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
$$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

View File

@ -18,7 +18,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Battery" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Battery,VtolPathFollower,FixedWingPathFollower" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>

View File

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