From 4c3ca17e343ce1fed79d6b6adbc478efb89b0cbf Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 6 Jun 2012 01:39:45 -0500 Subject: [PATCH] Backport the fixed wing path follower --- .../fixedwingpathfollower.c | 643 ++++++++++++++++++ flight/Revolution/Makefile | 2 +- flight/Revolution/Makefile.osx | 2 +- flight/Revolution/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../fixedwingpathfollowersettings.xml | 55 ++ .../fixedwingpathfollowerstatus.xml | 13 + 7 files changed, 716 insertions(+), 2 deletions(-) create mode 100644 flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c create mode 100644 shared/uavobjectdefinition/fixedwingpathfollowersettings.xml create mode 100644 shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c new file mode 100644 index 000000000..ba22ea636 --- /dev/null +++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -0,0 +1,643 @@ +/** + ****************************************************************************** + * + * @file fixedwingpathfollower.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * and sets @ref AttitudeDesired. It only does this when the FlightMode field + * of @ref ManualControlCommand is Auto. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Input object: ActiveWaypoint + * Input object: PositionActual + * Input object: ManualControlCommand + * Output object: AttitudeDesired + * + * This module will periodically update the value of the AttitudeDesired object. + * + * The module executes in its own thread in this example. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "openpilot.h" +#include "paths.h" + +#include "accels.h" +#include "hwsettings.h" +#include "attitudeactual.h" +#include "pathdesired.h" // object that will be updated by the module +#include "positionactual.h" +#include "manualcontrol.h" +#include "flightstatus.h" +#include "pathstatus.h" +#include "baroairspeed.h" +#include "gpsvelocity.h" +#include "gpsposition.h" +#include "fixedwingpathfollowersettings.h" +#include "fixedwingpathfollowerstatus.h" +#include "homelocation.h" +#include "nedposition.h" +#include "stabilizationdesired.h" +#include "stabilizationsettings.h" +#include "systemsettings.h" +#include "velocitydesired.h" +#include "velocityactual.h" +#include "CoordinateConversions.h" + +// Private constants +#define MAX_QUEUE_SIZE 4 +#define STACK_SIZE_BYTES 1548 +#define TASK_PRIORITY (tskIDLE_PRIORITY+2) +#define F_PI 3.14159265358979323846f +#define RAD2DEG (180.0f/F_PI) +#define DEG2RAD (F_PI/180.0f) +#define GEE 9.81f +// Private types + +// Private variables +static bool followerEnabled = false; +static xTaskHandle pathfollowerTaskHandle; +static PathDesiredData pathDesired; +static PathStatusData pathStatus; +static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; + +// Private functions +static void pathfollowerTask(void *parameters); +static void SettingsUpdatedCb(UAVObjEvent * ev); +static void updatePathVelocity(); +static uint8_t updateFixedDesiredAttitude(); +static void updateFixedAttitude(); +static void baroAirspeedUpdatedCb(UAVObjEvent * ev); +static float bound(float val, float min, float max); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t FixedWingPathFollowerStart() +{ + if (followerEnabled) { + // Start main task + xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); + } + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t FixedWingPathFollowerInitialize() +{ + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + followerEnabled = true; + FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); + PathDesiredInitialize(); + PathStatusInitialize(); + VelocityDesiredInitialize(); + BaroAirspeedInitialize(); + } else { + followerEnabled = false; + } + return 0; +} +MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart) + +static float northVelIntegral = 0; +static float eastVelIntegral = 0; +static float downVelIntegral = 0; + +static float courseIntegral = 0; +static float speedIntegral = 0; +static float accelIntegral = 0; +static float powerIntegral = 0; + +// correct speed by measured airspeed +static float baroAirspeedBias = 0; + +/** + * Module thread, should not return. + */ +static void pathfollowerTask(void *parameters) +{ + SystemSettingsData systemSettings; + FlightStatusData flightStatus; + + portTickType lastUpdateTime; + + BaroAirspeedConnectCallback(baroAirspeedUpdatedCb); + FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); + + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); + + // Main task loop + lastUpdateTime = xTaskGetTickCount(); + while (1) { + + // Conditions when this runs: + // 1. Must have FixedWing type airframe + // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + + SystemSettingsGet(&systemSettings); + if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && + (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) + { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); + vTaskDelay(1000); + continue; + } + + // Continue collecting data if not enough time + vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + + + FlightStatusGet(&flightStatus); + PathStatusGet(&pathStatus); + + uint8_t result; + // Check the combinations of flightmode and pathdesired mode + switch(flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); + } + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch(pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); + break; + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + courseIntegral = 0; + speedIntegral = 0; + accelIntegral = 0; + powerIntegral = 0; + + break; + } + PathStatusSet(&pathStatus); + } +} + +/** + * Compute desired velocity from the current position and path + * + * Takes in @ref PositionActual and compares it to @ref PathDesired + * and computes @ref VelocityDesired + */ +static void updatePathVelocity() +{ + PositionActualData positionActual; + PositionActualGet(&positionActual); + VelocityActualData velocityActual; + VelocityActualGet(&velocityActual); + + // look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds + float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward), + positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward), + positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward) + }; + struct path_status progress; + + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + + float groundspeed; + float altitudeSetpoint; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + altitudeSetpoint = pathDesired.End[2]; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + bound(progress.fractional_progress,0,1); + altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * + bound(progress.fractional_progress,0,1); + break; + } + + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0] * bound(groundspeed,1e-6,groundspeed); + velocityDesired.East = progress.path_direction[1] * bound(groundspeed,1e-6,groundspeed); + + float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; + + // calculate correction - can also be zero if correction vector is 0 or no error present + velocityDesired.North += progress.correction_direction[0] * error_speed; + velocityDesired.East += progress.correction_direction[1] * error_speed; + + float downError = altitudeSetpoint - positionActual.Down; + velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + + VelocityDesiredSet(&velocityDesired); +} + + +/** + * Compute desired attitude from a fixed preset + * + */ +static void updateFixedAttitude(float* attitude) +{ + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Throttle = attitude[3]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + StabilizationDesiredSet(&stabDesired); +} + +/** + * Compute desired attitude from the desired velocity + * + * Takes in @ref NedActual which has the acceleration in the + * NED frame as the feedback term and then compares the + * @ref VelocityActual against the @ref VelocityDesired + */ +static uint8_t updateFixedDesiredAttitude() +{ + + uint8_t result = 1; + + float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; + + VelocityDesiredData velocityDesired; + VelocityActualData velocityActual; + StabilizationDesiredData stabDesired; + AttitudeActualData attitudeActual; + AccelsData accels; + FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; + StabilizationSettingsData stabSettings; + FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; + + float groundspeedActual; + float groundspeedDesired; + float airspeedActual; + float airspeedDesired; + float speedError; + float accelActual; + float accelDesired; + float accelError; + float accelCommand; + + float pitchCommand; + + float climbspeedDesired; + float climbspeedError; + float powerError; + float powerCommand; + + float courseError; + float courseCommand; + + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + + FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); + + VelocityActualGet(&velocityActual); + VelocityDesiredGet(&velocityDesired); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeActualGet(&attitudeActual); + AccelsGet(&accels); + StabilizationSettingsGet(&stabSettings); + + + /** + * Compute speed error (required for throttle and pitch) + */ + + // Current ground speed + groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); + airspeedActual = groundspeedActual + baroAirspeedBias; + + // Desired ground speed + groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); + airspeedDesired = bound( groundspeedDesired + baroAirspeedBias, + fixedwingpathfollowerSettings.AirSpeedMin, + fixedwingpathfollowerSettings.AirSpeedMax); + + // Airspeed error + speedError = airspeedDesired - ( airspeedActual ); + // Vertical error + climbspeedDesired = bound ( + velocityDesired.Down, + -fixedwingpathfollowerSettings.VerticalVelMax, + fixedwingpathfollowerSettings.VerticalVelMax); + climbspeedError = climbspeedDesired - velocityActual.Down; + + // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; + if (groundspeedDesired - baroAirspeedBias <= 0 ) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; + result = 0; + } + // Error condition: plane too slow or too fast + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; + if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; + result = 0; + } + if (airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + result = 0; + } + + if (airspeedActual<1e-6) { + // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes + // also we cannot handle planes flying backwards, lets just wait until the nose drops + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + return 0; + } + + /** + * Compute desired throttle command + */ + // compute desired power + powerError = -climbspeedError + + bound ( + (speedError/airspeedActual) * fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP], + -fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX], + fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX] + ); + powerIntegral = bound(powerIntegral + powerError * dT * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], + -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT], + fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]); + powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + + powerIntegral) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL]; + + // prevent integral running out of bounds + if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) { + powerIntegral = bound( + powerIntegral - + ( powerCommand + - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]), + -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT], + fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]); + powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]; + } + if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) { + powerIntegral = bound( + powerIntegral - + ( powerCommand + - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]), + -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT], + fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]); + powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]; + } + + fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_POWER] = powerError; + fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_POWER] = powerIntegral; + fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_POWER] = powerCommand; + + // set throttle + stabDesired.Throttle = powerCommand; + + // Error condition: plane cannot hold altitude at current speed. + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; + if ( + powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum + && velocityActual.Down > 0 // we ARE going down + && climbspeedDesired < 0 // we WANT to go up + && speedError > 0 // we are too slow already + ) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum throttle (opposite of above) + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; + if ( + powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum + && velocityActual.Down < 0 // we ARE going up + && climbspeedDesired > 0 // we WANT to go down + && speedError < 0 // we are too fast already + ) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; + result = 0; + } + + + /** + * Compute desired pitch command + */ + // compute desired acceleration + accelDesired = bound( (speedError/airspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP], + -fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX], + fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]); + + fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = (speedError/airspeedActual); + fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f; + fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired; + + // exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster + accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE); + + accelError = accelDesired - accelActual; + accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI], + -fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT], + fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]); + accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] + + accelIntegral); + + fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_ACCEL] = accelError; + fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_ACCEL] = accelIntegral; + fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand; + + // compute desired pitch + pitchCommand= -accelCommand + bound ( (-climbspeedError/airspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], + -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], + fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] + ); + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], + fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); + + // Error condition: high speed dive + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; + if ( + pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up + && velocityActual.Down > 0 // we ARE going down + && climbspeedDesired < 0 // we WANT to go up + && speedError < 0 // we are too fast already + ) { + fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; + result = 0; + } + + + /** + * Compute desired roll command + */ + if (groundspeedDesired> 1e-6) { + courseError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North)); + } else { + // if we are not supposed to move, keep going wherever we are now. Don't make things worse by changing direction. + courseError = 0; + } + + if (courseError<-180.0f) courseError+=360.0f; + if (courseError>180.0f) courseError-=360.0f; + + courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI], + -fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT], + fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]); + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] + + courseIntegral); + + fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_COURSE] = courseError; + fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_COURSE] = courseIntegral; + fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_COURSE] = courseCommand; + + stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + + courseCommand, + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], + fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] ); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + + + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.X + stabDesired.Yaw = 0; + + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; + + StabilizationDesiredSet(&stabDesired); + + FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); + + return result; +} + + +/** + * Bound input value between limits + */ +static float bound(float val, float min, float max) +{ + if (val < min) { + val = min; + } else if (val > max) { + val = max; + } + return val; +} + +static void SettingsUpdatedCb(UAVObjEvent * ev) +{ + FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); + PathDesiredGet(&pathDesired); +} + +static void baroAirspeedUpdatedCb(UAVObjEvent * ev) +{ + + BaroAirspeedData baroAirspeed; + VelocityActualData velocityActual; + + BaroAirspeedGet(&baroAirspeed); + if (baroAirspeed.Connected != BAROAIRSPEED_CONNECTED_TRUE) { + baroAirspeedBias = 0; + } else { + VelocityActualGet(&velocityActual); + float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); + + baroAirspeedBias = baroAirspeed.Airspeed - groundspeed; + } + +} diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index 55470c781..88d154ba5 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -53,7 +53,7 @@ MODULES = Sensors Attitude/revolution ManualControl Stabilization Actuator MODULES += Battery MODULES += Altitude/revolution GPS FirmwareIAP MODULES += Airspeed/revolution -MODULES += AltitudeHold VtolPathFollower PathPlanner +MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner MODULES += CameraStab MODULES += OveroSync MODULES += Telemetry diff --git a/flight/Revolution/Makefile.osx b/flight/Revolution/Makefile.osx index bc1f50988..91bddb9a5 100644 --- a/flight/Revolution/Makefile.osx +++ b/flight/Revolution/Makefile.osx @@ -54,7 +54,7 @@ USE_THUMB_MODE = YES # List of modules to include MODULES += Actuator ManualControl Stabilization -MODULES += AltitudeHold VtolPathFollower PathPlanner +MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner MODULES += Attitude/revolution #MODULES += OveroSync/simulated diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index fcd19a4fa..00ef1fbe8 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -37,6 +37,7 @@ UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroairspeed +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 11a1410d3..f639f0631 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -40,6 +40,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/magnetometer.h \ $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ + $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ $$UAVOBJECT_SYNTHETICS/systemalarms.h \ $$UAVOBJECT_SYNTHETICS/objectpersistence.h \ @@ -120,6 +121,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \ $$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorcommand.cpp \ + $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml new file mode 100644 index 000000000..667153a3c --- /dev/null +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -0,0 +1,55 @@ + + + Settings for the @ref FixedWingPathFollowerModule + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml new file mode 100644 index 000000000..b384434ca --- /dev/null +++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml @@ -0,0 +1,13 @@ + + + Object Storing Debugging Information on PID internals + + + + + + + + + +