diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index bbb3995fe..053b9b474 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -30,10 +30,10 @@ struct path_status { float fractional_progress; float error; - float correction_direction[2]; - float path_direction[2]; + float path_vector[3]; + float correction_vector[3]; }; -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status); #endif diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index b4c8b75b8..42213e504 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -52,4 +52,31 @@ static inline float boundf(float val, float boundary1, float boundary2) return val; } +static inline float squaref(float x) +{ + return x * x; +} + +static inline float vector_lengthf(float *vector, const uint8_t dim) +{ + float length = 0.0f; + + for (int t = 0; t < dim; t++) { + length += squaref(vector[t]); + } + return sqrtf(length); +} + +static inline void vector_normalizef(float *vector, const uint8_t dim) +{ + float length = vector_lengthf(vector, dim); + + if (length <= 0.0f || isnan(length)) { + return; + } + for (int t = 0; t < dim; t++) { + vector[t] /= length; + } +} + #endif /* MATHMISC_H */ diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 1cbddb7d2..3bcd0a03d 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -26,49 +26,53 @@ #include #include - -#include "paths.h" +#include #include "uavobjectmanager.h" // <--. #include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, +#include "paths.h" // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status); -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise); /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired structure * @param[in] cur_point Current location - * @param[in] mode Path following mode * @param[out] status Structure containing progress along path and deviation */ -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode) +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status) { - switch (mode) { + switch (path->Mode) { case PATHDESIRED_MODE_FLYVECTOR: + return path_vector(path, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); + return path_vector(path, cur_point, status, false); break; case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); + return path_circle(path, cur_point, status, 1); break; case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); + return path_circle(path, cur_point, status, 0); break; case PATHDESIRED_MODE_FLYENDPOINT: + return path_endpoint(path, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEENDPOINT: default: // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); + return path_endpoint(path, cur_point, status, false); break; } @@ -76,180 +80,198 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc /** * @brief Compute progress towards endpoint. Deviation equals distance - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float diff[3]; float dist_path, dist_diff; - // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = 0; - // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to end - diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + diff[0] = path->End.North - cur_point[0]; + diff[1] = path->End.East - cur_point[1]; + diff[2] = mode3D ? path->End.Down - cur_point[2] : 0.0f; - dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_diff = vector_lengthf(diff, 3); + dist_path = vector_lengthf(status->path_vector, 3); if (dist_diff < 1e-6f) { - status->fractional_progress = 1; - status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; + status->fractional_progress = 1; + status->error = 0.0f; + status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f; + // we have no base movement direction in this mode + status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f; + return; } - status->fractional_progress = 1 - dist_diff / (1 + dist_path); + if (fmaxf(dist_path, 1.0f) > dist_diff) { + status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f); + } else { + status->fractional_progress = 0; // we don't want fractional_progress to become negative + } status->error = dist_diff; - // Compute direction to travel - status->path_direction[0] = diff_north / dist_diff; - status->path_direction[1] = diff_east / dist_diff; + // Compute correction vector + status->correction_vector[0] = diff[0]; + status->correction_vector[1] = diff[1]; + status->correction_vector[2] = diff[2]; + + // base movement direction in this mode is a constant velocity offset on top of correction in the same direction + status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff; + status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff; + status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff; } /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float diff[3]; float dist_path; float dot; - float normal[2]; + float velocity; + float track_point[3]; // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to start - diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + diff[0] = cur_point[0] - path->Start.North; + diff[1] = cur_point[1] - path->Start.East; + diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f; - dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2]; + dist_path = vector_lengthf(status->path_vector, 3); - if (dist_path < 1e-6f) { - // if the path is too short, we cannot determine vector direction. + if (dist_path > 1e-6f) { + // Compute direction to travel & progress + status->fractional_progress = dot / (dist_path * dist_path); + } else { // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status); + path_endpoint(path, cur_point, status, mode3D); status->fractional_progress = 1; return; } + // Compute point on track that is closest to our current position. + track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North; + track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East; + track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down; - // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + status->correction_vector[0] = track_point[0] - cur_point[0]; + status->correction_vector[1] = track_point[1] - cur_point[1]; + status->correction_vector[2] = track_point[2] - cur_point[2]; - status->fractional_progress = dot / (dist_path * dist_path); - status->error = normal[0] * diff_north + normal[1] * diff_east; + status->error = vector_lengthf(status->correction_vector, 3); - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; - status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; - - // Now just want magnitude of error - status->error = fabs(status->error); - - // Compute direction to travel - status->path_direction[0] = path_north / dist_path; - status->path_direction[1] = path_east / dist_path; + // correct movement vector to current velocity + velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity); + status->path_vector[0] = velocity * status->path_vector[0] / dist_path; + status->path_vector[1] = velocity * status->path_vector[1] / dist_path; + status->path_vector[2] = velocity * status->path_vector[2] / dist_path; } /** * @brief Compute progress along circular path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Center point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise) { - float radius_north, radius_east, diff_north, diff_east; + float radius_north, radius_east, diff_north, diff_east, diff_down; float radius, cradius; float normal[2]; float progress; float a_diff, a_radius; // Radius - radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + radius_north = path->End.North - path->Start.North; + radius_east = path->End.East - path->Start.East; // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; + diff_north = cur_point[0] - path->End.North; + diff_east = cur_point[1] - path->End.East; + diff_down = cur_point[2] - path->End.Down; - radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); - cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); + radius = sqrtf(squaref(radius_north) + squaref(radius_east)); + cradius = sqrtf(squaref(diff_north) + squaref(diff_east)); - if (cradius < 1e-6f) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->error = radius; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->path_direction[0] = 1; - status->path_direction[1] = 0; - return; - } - - if (clockwise) { - // Compute the normal to the radius clockwise - normal[0] = -diff_east / cradius; - normal[1] = diff_north / cradius; - } else { - // Compute the normal to the radius counter clockwise - normal[0] = diff_east / cradius; - normal[1] = -diff_north / cradius; - } - - // normalize progress to 0..1 - a_diff = atan2f(diff_north, diff_east); - a_radius = atan2f(radius_north, radius_east); - - if (a_diff < 0) { - a_diff += 2.0f * M_PI_F; - } - if (a_radius < 0) { - a_radius += 2.0f * M_PI_F; - } - - progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); - - if (progress < 0) { - progress += 1.0f; - } else if (progress >= 1.0f) { - progress -= 1.0f; - } - - if (clockwise) { - progress = 1 - progress; - } - - status->fractional_progress = progress; + // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply) + status->path_vector[2] = 0.0f; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; - status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; + if (cradius < 1e-6f) { + // cradius is zero, just fly somewhere + status->fractional_progress = 1; + status->correction_vector[0] = 0; + status->correction_vector[1] = 0; + status->path_vector[0] = path->EndingVelocity; + status->path_vector[1] = 0; + } else { + if (clockwise) { + // Compute the normal to the radius clockwise + normal[0] = -diff_east / cradius; + normal[1] = diff_north / cradius; + } else { + // Compute the normal to the radius counter clockwise + normal[0] = diff_east / cradius; + normal[1] = -diff_north / cradius; + } - // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; + // normalize progress to 0..1 + a_diff = atan2f(diff_north, diff_east); + a_radius = atan2f(radius_north, radius_east); + + if (a_diff < 0) { + a_diff += 2.0f * M_PI_F; + } + if (a_radius < 0) { + a_radius += 2.0f * M_PI_F; + } + + progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); + + if (progress < 0.0f) { + progress += 1.0f; + } else if (progress >= 1.0f) { + progress -= 1.0f; + } + + if (clockwise) { + progress = 1.0f - progress; + } + + status->fractional_progress = progress; + + // Compute direction to travel + status->path_vector[0] = normal[0] * path->EndingVelocity; + status->path_vector[1] = normal[1] * path->EndingVelocity; + + // Compute direction to correct error + status->correction_vector[0] = status->error * diff_north / cradius; + status->correction_vector[1] = status->error * diff_east / cradius; + } + + status->correction_vector[2] = -diff_down; status->error = fabs(status->error); } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 9055419fa..2d009f9ea 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -70,8 +70,6 @@ void plan_setup_positionHold() FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; @@ -79,7 +77,7 @@ void plan_setup_positionHold() pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -110,8 +108,6 @@ void plan_setup_returnToBase() destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; @@ -121,16 +117,27 @@ void plan_setup_returnToBase() pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.Down = destDown; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } +static PiOSDeltatimeConfig landdT; void plan_setup_land() { + float descendspeed; + plan_setup_positionHold(); + + FlightModeSettingsLandingVelocityGet(&descendspeed); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.StartingVelocity = descendspeed; + pathDesired.EndingVelocity = descendspeed; + PathDesiredSet(&pathDesired); + PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } /** @@ -138,12 +145,18 @@ void plan_setup_land() */ void plan_run_land() { + float downPos, descendspeed; PathDesiredEndData pathDesiredEnd; - PathDesiredEndGet(&pathDesiredEnd); + PositionStateDownGet(&downPos); // current down position + PathDesiredEndGet(&pathDesiredEnd); // desired position + PathDesiredEndingVelocityGet(&descendspeed); - PositionStateDownGet(&pathDesiredEnd.Down); - pathDesiredEnd.Down += 5; + // desired position is updated to match the desired descend speed but don't run ahead + // too far if the current position can't keep up. This normaly means we have landed. + if (pathDesiredEnd.Down - downPos < 10) { + pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT); + } PathDesiredEndSet(&pathDesiredEnd); } @@ -366,8 +379,6 @@ void plan_setup_AutoCruise() FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); // initialization is flight in direction of the nose. // the velocity is not relevant, as it will be reset by the run function even during first call @@ -387,7 +398,7 @@ void plan_setup_AutoCruise() pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 365e1af7b..0902cbce7 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -149,7 +149,6 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); - ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)); ADDSEVERITY(navCapableFusion); break; default: diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c deleted file mode 100644 index 556a10db8..000000000 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ /dev/null @@ -1,772 +0,0 @@ -/** - ****************************************************************************** - * - * @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: PositionState - * 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 - -#include "hwsettings.h" -#include "attitudestate.h" -#include "pathdesired.h" // object that will be updated by the module -#include "positionstate.h" -#include "flightstatus.h" -#include "pathstatus.h" -#include "airspeedstate.h" -#include "fixedwingpathfollowersettings.h" -#include "fixedwingpathfollowerstatus.h" -#include "homelocation.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "systemsettings.h" -#include "velocitydesired.h" -#include "velocitystate.h" -#include "taskinfo.h" -#include -#include - -#include "sin_lookup.h" -#include "paths.h" -#include "CoordinateConversions.h" - -// Private constants -#define MAX_QUEUE_SIZE 4 -#define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) - -// 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 airspeedStateUpdatedCb(UAVObjEvent *ev); -static bool correctCourse(float *C, float *V, float *F, float s); - -/** - * 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, "PathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(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(); - HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(&optionalModules); - FrameType_t frameType = GetCurrentFrameType(); - - if ((optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) || - (frameType == FRAME_TYPE_FIXED_WING)) { - followerEnabled = true; - FixedWingPathFollowerSettingsInitialize(); - FixedWingPathFollowerStatusInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - AirspeedStateInitialize(); - } else { - followerEnabled = false; - } - return 0; -} -MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart); - -static float northVelIntegral = 0.0f; -static float eastVelIntegral = 0.0f; -static float downVelIntegral = 0.0f; - -static float courseIntegral = 0.0f; -static float speedIntegral = 0.0f; -static float powerIntegral = 0.0f; -static float airspeedErrorInt = 0.0f; - -// correct speed by measured airspeed -static float indicatedAirspeedStateBias = 0.0f; - -/** - * Module thread, should not return. - */ -static void pathfollowerTask(__attribute__((unused)) void *parameters) -{ - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - AirspeedStateConnectCallback(airspeedStateUpdatedCb); - 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 - if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { - 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_CRITICAL); - } - } else { - 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_CRITICAL); - break; - } - } - } else { - // Be cleaner and get rid of global variables - northVelIntegral = 0.0f; - eastVelIntegral = 0.0f; - downVelIntegral = 0.0f; - courseIntegral = 0.0f; - speedIntegral = 0.0f; - powerIntegral = 0.0f; - } - PathStatusSet(&pathStatus); - } -} - -/** - * Compute desired velocity from the current position and path - * - * Takes in @ref PositionState and compares it to @ref PathDesired - * and computes @ref VelocityDesired - */ -static void updatePathVelocity() -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - - // look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds - float cur[3] = { positionState.North + (velocityState.North * fixedwingpathfollowerSettings.CourseFeedForward), - positionState.East + (velocityState.East * fixedwingpathfollowerSettings.CourseFeedForward), - positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; - struct path_status progress; - - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - 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.Down; - 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) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - break; - } - // make sure groundspeed is not zero - if (groundspeed < 1e-6f) { - groundspeed = 1e-6f; - } - - // calculate velocity - can be zero if waypoints are too close - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - - float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; - - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // in this case the plane effectively needs to be turned around - // indicators: - // difference between correction_direction and velocitystate >90 degrees and - // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - if ( // calculating angles < 90 degrees through dot products - ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) && - ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) { - error_speed = 0.0f; - } - - // 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; - - // scale to correct length - float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - if (l > 1e-9f) { - velocityDesired.North *= groundspeed / l; - velocityDesired.East *= groundspeed / l; - } - - float downError = altitudeSetpoint - positionState.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.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Compute desired attitude from the desired velocity - * - * Takes in @ref NedState which has the acceleration in the - * NED frame as the feedback term and then compares the - * @ref VelocityState against the @ref VelocityDesired - */ -static uint8_t updateFixedDesiredAttitude() -{ - uint8_t result = 1; - - float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; // Convert from [ms] to [s] - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - StabilizationSettingsData stabSettings; - FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - AirspeedStateData airspeedState; - SystemSettingsData systemSettings; - - float groundspeedProjection; - float indicatedAirspeedState; - float indicatedAirspeedDesired; - float airspeedError; - - float pitchCommand; - - float descentspeedDesired; - float descentspeedError; - float powerCommand; - - float airspeedVector[2]; - float fluidMovement[2]; - float courseComponent[2]; - float courseError; - float courseCommand; - - FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus); - - VelocityStateGet(&velocityState); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - StabilizationSettingsGet(&stabSettings); - AirspeedStateGet(&airspeedState); - SystemSettingsGet(&systemSettings); - - /** - * Compute speed error and course - */ - // missing sensors for airspeed-direction we have to assume within - // reasonable error that measured airspeed is actually the airspeed - // component in forward pointing direction - // airspeedVector is normalized - airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); - airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); - - // current ground speed projected in forward direction - groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, - // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction - // than airspeed and gps sensors alone - indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; - - // fluidMovement is a vector describing the aproximate movement vector of - // the surrounding fluid in 2d space (aka wind vector) - fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); - fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); - - // calculate the movement vector we need to fly to reach velocityDesired - - // taking fluidMovement into account - courseComponent[0] = velocityDesired.North - fluidMovement[0]; - courseComponent[1] = velocityDesired.East - fluidMovement[1]; - - indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), - fixedwingpathfollowerSettings.HorizontalVelMin, - fixedwingpathfollowerSettings.HorizontalVelMax); - - // if we could fly at arbitrary speeds, we'd just have to move towards the - // courseComponent vector as previously calculated and we'd be fine - // unfortunately however we are bound by min and max air speed limits, so - // we need to recalculate the correct course to meet at least the - // velocityDesired vector direction at our current speed - // this overwrites courseComponent - bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); - - // Error condition: wind speed too high, we can't go where we want anymore - fixedwingpathfollowerStatus.Errors.Wind = 0; - if ((!valid) && - fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.Wind = 1; - result = 0; - } - - // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; - - // Vertical speed error - descentspeedDesired = boundf( - velocityDesired.Down, - -fixedwingpathfollowerSettings.VerticalVelMax, - fixedwingpathfollowerSettings.VerticalVelMax); - descentspeedError = descentspeedDesired - velocityState.Down; - - // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors.Highspeed = 0; - fixedwingpathfollowerStatus.Errors.Lowspeed = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.Overspeed) { - fixedwingpathfollowerStatus.Errors.Overspeed = 1; - result = 0; - } - if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) { - fixedwingpathfollowerStatus.Errors.Highspeed = 1; - result = 0; - } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) { - fixedwingpathfollowerStatus.Errors.Lowspeed = 1; - result = 0; - } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.Stallspeed) { - fixedwingpathfollowerStatus.Errors.Stallspeed = 1; - result = 0; - } - - /** - * Compute desired thrust command - */ - // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) { - powerIntegral = boundf(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, - fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki - ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); - } else { - powerIntegral = 0.0f; - } - - // Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = boundf( - (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp, - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max, - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max - ); - - // Compute final thrust response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp + - powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki + - speedErrorToPowerCommandComponent; - - // Output internal state to telemetry - fixedwingpathfollowerStatus.Error.Power = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral; - fixedwingpathfollowerStatus.Command.Power = powerCommand; - - // set thrust - stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand, - fixedwingpathfollowerSettings.ThrustLimit.Min, - fixedwingpathfollowerSettings.ThrustLimit.Max); - - // Error condition: plane cannot hold altitude at current speed. - fixedwingpathfollowerStatus.Errors.Lowpower = 0; - if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum - velocityState.Down > 0.0f && // we ARE going down - descentspeedDesired < 0.0f && // we WANT to go up - airspeedError > 0.0f && // we are too slow already - fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.Lowpower = 1; - result = 0; - } - // Error condition: plane keeps climbing despite minimum thrust (opposite of above) - fixedwingpathfollowerStatus.Errors.Highpower = 0; - if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum - velocityState.Down < 0.0f && // we ARE going up - descentspeedDesired > 0.0f && // we WANT to go down - airspeedError < 0.0f && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.Highpower = 1; - result = 0; - } - - - /** - * Compute desired pitch command - */ - if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) { - // Integrate with saturation - airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki, - fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki); - } - - // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp, - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max, - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max - ); - - // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp - + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki - ) + verticalSpeedToPitchCommandComponent; - - fixedwingpathfollowerStatus.Error.Speed = airspeedError; - fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt; - fixedwingpathfollowerStatus.Command.Speed = pitchCommand; - - stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit.Min, - fixedwingpathfollowerSettings.PitchLimit.Max); - - // Error condition: high speed dive - fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0; - if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up - velocityState.Down > 0.0f && // we ARE going down - descentspeedDesired < 0.0f && // we WANT to go up - airspeedError < 0.0f && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1; - result = 0; - } - - /** - * Compute desired roll command - */ - courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; - - if (courseError < -180.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f) { - courseError -= 360.0f; - } - - // overlap calculation. Theres a dead zone behind the craft where the - // counter-yawing of some craft while rolling could render a desired right - // turn into a desired left turn. Making the turn direction based on - // current roll angle keeps the plane committed to a direction once chosen - if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f) - && attitudeState.Roll > 0.0f) { - courseError += 360.0f; - } - if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f) - && attitudeState.Roll < 0.0f) { - courseError -= 360.0f; - } - - courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki, - -fixedwingpathfollowerSettings.CoursePI.ILimit, - fixedwingpathfollowerSettings.CoursePI.ILimit); - courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp + - courseIntegral); - - fixedwingpathfollowerStatus.Error.Course = courseError; - fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral; - fixedwingpathfollowerStatus.Command.Course = courseCommand; - - stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral + - courseCommand, - fixedwingpathfollowerSettings.RollLimit.Min, - 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.Y - stabDesired.Yaw = 0.0f; - - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - - StabilizationDesiredSet(&stabDesired); - - FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus); - - return result; -} - -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); - PathDesiredGet(&pathDesired); -} - -static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - AirspeedStateData airspeedState; - VelocityStateData velocityState; - - AirspeedStateGet(&airspeedState); - VelocityStateGet(&velocityState); - float airspeedVector[2]; - float yaw; - AttitudeStateYawGet(&yaw); - airspeedVector[0] = cos_lookup_deg(yaw); - airspeedVector[1] = sin_lookup_deg(yaw); - // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement - float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; - // note - we do fly by Indicated Airspeed (== calibrated airspeed) however - // since airspeed is updated less often than groundspeed, we use sudden - // changes to groundspeed to offset the airspeed by the same measurement. - // This has a side effect that in the absence of any airspeed updates, the - // pathfollower will fly using groundspeed. -} - - -/** - * Function to calculate course vector C based on airspeed s, fluid movement F - * and desired movement vector V - * parameters in: V,F,s - * parameters out: C - * returns true if a valid solution could be found for V,F,s, false if not - * C will be set to a best effort attempt either way - */ -static bool correctCourse(float *C, float *V, float *F, float s) -{ - // Approach: - // Let Sc be a circle around origin marking possible movement vectors - // of the craft with airspeed s (all possible options for C) - // Let Vl be a line through the origin along movement vector V where fr any - // point v on line Vl v = k * (V / |V|) = k' * V - // Let Wl be a line parallel to Vl where for any point v on line Vl exists - // a point w on WL with w = v - F - // Then any intersection between circle Sc and line Wl represents course - // vector which would result in a movement vector - // V' = k * ( V / |V|) = k' * V - // If there is no intersection point, S is insufficient to compensate - // for F and we can only try to fly in direction of V (thus having wind drift - // but at least making progress orthogonal to wind) - - s = fabsf(s); - float f = sqrtf(F[0] * F[0] + F[1] * F[1]); - - // normalize Cn=V/|V|, |V| must be >0 - float v = sqrtf(V[0] * V[0] + V[1] * V[1]); - if (v < 1e-6f) { - // if |V|=0, we aren't supposed to move, turn into the wind - // (this allows hovering) - C[0] = -F[0]; - C[1] = -F[1]; - // if desired airspeed matches fluidmovement a hover is actually - // intended so return true - return fabsf(f - s) < 1e-3f; - } - float Vn[2] = { V[0] / v, V[1] / v }; - - // project F on V - float fp = F[0] * Vn[0] + F[1] * Vn[1]; - - // find component Fo of F that is orthogonal to V - // (which is exactly the distance between Vl and Wl) - float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; - float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; - - // find k where k * Vn = C - Fo - // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo - // so k^2 + fo^2 = s^2 (since |Vn|=1) - float k2 = s * s - fo2; - if (k2 <= -1e-3f) { - // there is no solution, we will be drifted off either way - // fallback: fly stupidly in direction of V and hope for the best - C[0] = V[0]; - C[1] = V[1]; - return false; - } else if (k2 <= 1e-3f) { - // there is exactly one solution: -Fo - C[0] = -Fo[0]; - C[1] = -Fo[1]; - return true; - } - // we have two possible solutions k positive and k negative as there are - // two intersection points between Wl and Sc - // which one is better? two criteria: - // 1. we MUST move in the right direction, if any k leads to -v its invalid - // 2. we should minimize the speed error - float k = sqrt(k2); - float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; - float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; - // project C+F on Vn to find signed resulting movement vector length - float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; - float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; - if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { - // in this case the angle between course and resulting movement vector - // is greater than 90 degrees - so we actually fly backwards - C[0] = C1[0]; - C[1] = C1[1]; - return true; - } - C[0] = C2[0]; - C[1] = C2[1]; - if (vp2 >= 0.0f) { - // in this case the angle between course and movement vector is less than - // 90 degrees, but we do move in the right direction - return true; - } else { - // in this case we actually get driven in the opposite direction of V - // with both solutions for C - // this might be reached in headwind stronger than maximum allowed - // airspeed. - return false; - } -} diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c new file mode 100644 index 000000000..67226589b --- /dev/null +++ b/flight/modules/PathFollower/pathfollower.c @@ -0,0 +1,1239 @@ +/** + ****************************************************************************** + * + * @file pathfollower.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: PathDesired + * Input object: PositionState + * Input object: ManualControlCommand + * Output object: StabilizationDesired + * + * This module acts as "autopilot" - it controls the setpoints of stabilization + * based on current flight situation and desired flight path (PathDesired) as + * directed by flightmode selection or pathplanner + * This is a periodic delayed callback module + * + * 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 + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Private constants + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define PF_IDLE_UPDATE_RATE_MS 100 + +#define STACK_SIZE_BYTES 2048 + +#define DEADBAND_HIGH 0.10f +#define DEADBAND_LOW -0.10f +// Private types + +struct Globals { + struct pid PIDposH[2]; + struct pid PIDposV; + struct pid PIDvel[3]; + struct pid PIDcourse; + struct pid PIDspeed; + struct pid PIDpower; + float poiRadius; + float vtolEmergencyFallback; + bool vtolEmergencyFallbackSwitch; +}; + + +// Private variables +static DelayedCallbackInfo *pathFollowerCBInfo; +static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; +static struct Globals global; +static PathStatusData pathStatus; +static PathDesiredData pathDesired; +static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; +static VtolPathFollowerSettingsData vtolPathFollowerSettings; + +// correct speed by measured airspeed +static float indicatedAirspeedStateBias = 0.0f; + + +// Private functions +static void pathFollowerTask(void); +static void resetGlobals(); +static void SettingsUpdatedCb(UAVObjEvent *ev); +static uint8_t updateAutoPilotByFrameType(); +static uint8_t updateAutoPilotFixedWing(); +static uint8_t updateAutoPilotVtol(); +static float updateTailInBearing(); +static float updateCourseBearing(); +static float updatePathBearing(); +static float updatePOIBearing(); +static void processPOI(); +static void updatePathVelocity(float kFF, bool limited); +static uint8_t updateFixedDesiredAttitude(); +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); +static void updateFixedAttitude(); +static void updateVtolDesiredAttitudeEmergencyFallback(); +static void airspeedStateUpdatedCb(UAVObjEvent *ev); +static bool correctCourse(float *C, float *V, float *F, float s); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerStart() +{ + // Start main task + PathStatusGet(&pathStatus); + SettingsUpdatedCb(NULL); + PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerInitialize() +{ + // initialize objects + FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); + VtolPathFollowerSettingsInitialize(); + FlightStatusInitialize(); + PathStatusInitialize(); + PathDesiredInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); + VelocityDesiredInitialize(); + StabilizationDesiredInitialize(); + AirspeedStateInitialize(); + AttitudeStateInitialize(); + TakeOffLocationInitialize(); + PoiLocationInitialize(); + ManualControlCommandInitialize(); + SystemSettingsInitialize(); + StabilizationBankInitialize(); + + // reset integrals + resetGlobals(); + + // Create object queue + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); + FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); + AirspeedStateConnectCallback(airspeedStateUpdatedCb); + + return 0; +} +MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); + + +/** + * Module thread, should not return. + */ +static void pathFollowerTask(void) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + resetGlobals(); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } + + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol! + processPOI(); + } + + 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: + { + uint8_t result = updateAutoPilotByFrameType(); + 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; + } + PathStatusSet(&pathStatus); + + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); +} + + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + + pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit); + pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit); + pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit); + + + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + + pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + + PathDesiredGet(&pathDesired); +} + + +static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + AirspeedStateData airspeedState; + VelocityStateData velocityState; + + AirspeedStateGet(&airspeedState); + VelocityStateGet(&velocityState); + float airspeedVector[2]; + float yaw; + AttitudeStateYawGet(&yaw); + airspeedVector[0] = cos_lookup_deg(yaw); + airspeedVector[1] = sin_lookup_deg(yaw); + // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement + float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) however + // since airspeed is updated less often than groundspeed, we use sudden + // changes to groundspeed to offset the airspeed by the same measurement. + // This has a side effect that in the absence of any airspeed updates, the + // pathfollower will fly using groundspeed. +} + + +/** + * reset integrals + */ +static void resetGlobals() +{ + pid_zero(&global.PIDposH[0]); + pid_zero(&global.PIDposH[1]); + pid_zero(&global.PIDposV); + pid_zero(&global.PIDvel[0]); + pid_zero(&global.PIDvel[1]); + pid_zero(&global.PIDvel[2]); + pid_zero(&global.PIDcourse); + pid_zero(&global.PIDspeed); + pid_zero(&global.PIDpower); + global.poiRadius = 0.0f; + global.vtolEmergencyFallback = 0; + global.vtolEmergencyFallbackSwitch = false; +} + +static uint8_t updateAutoPilotByFrameType() +{ + FrameType_t frameType = GetCurrentFrameType(); + + if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) { + switch (vtolPathFollowerSettings.TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + } + } + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + updatePeriod = vtolPathFollowerSettings.UpdatePeriod; + return updateAutoPilotVtol(); + + break; + case FRAME_TYPE_FIXED_WING: + default: + updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod; + return updateAutoPilotFixedWing(); + + break; + } +} + +/** + * fixed wing autopilot: + * straight forward: + * 1. update path velocity for limited motion crafts + * 2. update attitude according to default fixed wing pathfollower algorithm + */ +static uint8_t updateAutoPilotFixedWing() +{ + pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f); + updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true); + return updateFixedDesiredAttitude(); +} + +/** + * vtol autopilot + * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure) + * fall back to emergency fallback autopilot to keep minimum amount of flight control + */ +static uint8_t updateAutoPilotVtol() +{ + enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode; + enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode; + uint8_t result = 0; + + // decide on behaviour based on settings and system state + if (global.vtolEmergencyFallbackSwitch) { + returnmode = RETURN_0; + followermode = FOLLOWER_FALLBACK; + } else { + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { + returnmode = RETURN_1; + followermode = FOLLOWER_FALLBACK; + } else { + returnmode = RETURN_RESULT; + followermode = FOLLOWER_REGULAR; + } + } + + // vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + + switch (followermode) { + case FOLLOWER_REGULAR: + { + // horizontal position control PID loop works according to settings in regular mode, allowing integral terms + pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); + + // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm + bool yaw_attitude = true; + float yaw = 0.0f; + switch (vtolPathFollowerSettings.YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } + result = updateVtolDesiredAttitude(yaw_attitude, yaw); + + // switch to emergency follower if follower indicates problems + if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED)) { + global.vtolEmergencyFallbackSwitch = true; + } + } + break; + case FOLLOWER_FALLBACK: + { + // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly + pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); + + // emergency follower has no return value + updateVtolDesiredAttitudeEmergencyFallback(); + } + break; + } + + switch (returnmode) { + case RETURN_RESULT: + return result; + + default: + // returns either 0 or 1 according to enum definition above + return returnmode; + } +} + + +/** + * Compute bearing of current takeoff location + */ +static float updateTailInBearing() +{ + PositionStateData p; + + PositionStateGet(&p); + TakeOffLocationData t; + TakeOffLocationGet(&t); + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); +} + +/** + * Compute bearing of current movement direction + */ +static float updateCourseBearing() +{ + VelocityStateData v; + + VelocityStateGet(&v); + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(v.East, v.North)); +} + +/** + * Compute bearing of current path direction + */ +static float updatePathBearing() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + + float cur[3] = { positionState.North, + positionState.East, + positionState.Down }; + struct path_status progress; + + path_progress(&pathDesired, cur, &progress); + + // atan2f always returns in between + and - 180 degrees + return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); +} + +/** + * Compute bearing between current position and POI + */ +static float updatePOIBearing() +{ + PoiLocationData poi; + + PoiLocationGet(&poi); + PositionStateData positionState; + PositionStateGet(&positionState); + + const float dT = updatePeriod / 1000.0f; + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } + + return yaw + (pathAngle / 2.0f); +} + +/** + * process POI control logic TODO: this should most likely go into manualcontrol!!!! + * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently + **/ +static void processPOI() +{ + const float dT = updatePeriod / 1000.0f; + + + PositionStateData positionState; + + PositionStateGet(&positionState); + // TODO put commented out camera feature code back in place either + // permanently or optionally or remove it + // CameraDesiredData cameraDesired; + // CameraDesiredGet(&cameraDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + PoiLocationData poi; + PoiLocationGet(&poi); + + float dLoc[3]; + float yaw = 0; + // TODO camera feature + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + + // distance + float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float changeRadius = 0; + // Move closer or further, radially + if (manualControlData.Pitch > DEADBAND_HIGH) { + changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; + } else if (manualControlData.Pitch < DEADBAND_LOW) { + changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; + } + + // move along circular path + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { + // change radius only when not circling + global.poiRadius = distance + changeRadius; + } + + // don't try to move any closer + if (global.poiRadius >= 3.0f || changeRadius > 0) { + if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { + pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.StartingVelocity = 1.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } + } + // not above + if (distance >= 3.0f) { + // TODO camera feature + // You can feed this into camerastabilization + /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ + + // cameraDesired.Yaw=yaw; + // cameraDesired.PitchOrServo2=elevation; + + // CameraDesiredSet(&cameraDesired); + } +} + +/** + * Compute desired velocity from the current position and path + */ +static void updatePathVelocity(float kFF, bool limited) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + + const float dT = updatePeriod / 1000.0f; + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + + path_progress(&pathDesired, cur, &progress); + + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_vector[0]; + velocityDesired.East = progress.path_vector[1]; + velocityDesired.Down = progress.path_vector[2]; + + if (limited && + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocitystate >90 degrees and + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + // calculating angles < 90 degrees through dot products + (vector_lengthf(progress.path_vector, 2) > 1e-6f) && + ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && + ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { + ; + } else { + // calculate correction + velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); + velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); + } + velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_vector[0]; + pathStatus.path_direction_east = progress.path_vector[1]; + pathStatus.path_direction_down = progress.path_vector[2]; + + pathStatus.correction_direction_north = progress.correction_vector[0]; + pathStatus.correction_direction_east = progress.correction_vector[1]; + pathStatus.correction_direction_down = progress.correction_vector[2]; + + VelocityDesiredSet(&velocityDesired); +} + + +/** + * Compute desired attitude from the desired velocity for fixed wing craft + */ +static uint8_t updateFixedDesiredAttitude() +{ + uint8_t result = 1; + + const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s] + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + FixedWingPathFollowerStatusData fixedWingPathFollowerStatus; + AirspeedStateData airspeedState; + SystemSettingsData systemSettings; + + float groundspeedProjection; + float indicatedAirspeedState; + float indicatedAirspeedDesired; + float airspeedError; + + float pitchCommand; + + float descentspeedDesired; + float descentspeedError; + float powerCommand; + + float airspeedVector[2]; + float fluidMovement[2]; + float courseComponent[2]; + float courseError; + float courseCommand; + + FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus); + + VelocityStateGet(&velocityState); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + AirspeedStateGet(&airspeedState); + SystemSettingsGet(&systemSettings); + + /** + * Compute speed error and course + */ + // missing sensors for airspeed-direction we have to assume within + // reasonable error that measured airspeed is actually the airspeed + // component in forward pointing direction + // airspeedVector is normalized + airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); + airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); + + // current ground speed projected in forward direction + groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, + // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; + + // fluidMovement is a vector describing the aproximate movement vector of + // the surrounding fluid in 2d space (aka wind vector) + fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); + fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); + + // calculate the movement vector we need to fly to reach velocityDesired - + // taking fluidMovement into account + courseComponent[0] = velocityDesired.North - fluidMovement[0]; + courseComponent[1] = velocityDesired.East - fluidMovement[1]; + + indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), + fixedWingPathFollowerSettings.HorizontalVelMin, + fixedWingPathFollowerSettings.HorizontalVelMax); + + // if we could fly at arbitrary speeds, we'd just have to move towards the + // courseComponent vector as previously calculated and we'd be fine + // unfortunately however we are bound by min and max air speed limits, so + // we need to recalculate the correct course to meet at least the + // velocityDesired vector direction at our current speed + // this overwrites courseComponent + bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); + + // Error condition: wind speed too high, we can't go where we want anymore + fixedWingPathFollowerStatus.Errors.Wind = 0; + if ((!valid) && + fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Wind = 1; + result = 0; + } + + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; + + // Vertical speed error + descentspeedDesired = boundf( + velocityDesired.Down, + -fixedWingPathFollowerSettings.VerticalVelMax, + fixedWingPathFollowerSettings.VerticalVelMax); + descentspeedError = descentspeedDesired - velocityState.Down; + + // Error condition: plane too slow or too fast + fixedWingPathFollowerStatus.Errors.Highspeed = 0; + fixedWingPathFollowerStatus.Errors.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) { + fixedWingPathFollowerStatus.Errors.Overspeed = 1; + result = 0; + } + if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) { + fixedWingPathFollowerStatus.Errors.Highspeed = 1; + result = 0; + } + if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) { + fixedWingPathFollowerStatus.Errors.Lowspeed = 1; + result = 0; + } + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) { + fixedWingPathFollowerStatus.Errors.Stallspeed = 1; + result = 0; + } + + /** + * Compute desired thrust command + */ + + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = boundf( + (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp, + -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max, + fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max + ); + + // Compute final thrust response + powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) + + speedErrorToPowerCommandComponent; + + // Output internal state to telemetry + fixedWingPathFollowerStatus.Error.Power = descentspeedError; + fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator; + fixedWingPathFollowerStatus.Command.Power = powerCommand; + + // set thrust + stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand, + fixedWingPathFollowerSettings.ThrustLimit.Min, + fixedWingPathFollowerSettings.ThrustLimit.Max); + + // Error condition: plane cannot hold altitude at current speed. + fixedWingPathFollowerStatus.Errors.Lowpower = 0; + if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError > 0.0f && // we are too slow already + fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Lowpower = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum thrust (opposite of above) + fixedWingPathFollowerStatus.Errors.Highpower = 0; + if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum + velocityState.Down < 0.0f && // we ARE going up + descentspeedDesired > 0.0f && // we WANT to go down + airspeedError < 0.0f && // we are too fast already + fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Highpower = 1; + result = 0; + } + + /** + * Compute desired pitch command + */ + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, + -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, + fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max + ); + + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; + + fixedWingPathFollowerStatus.Error.Speed = airspeedError; + fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator; + fixedWingPathFollowerStatus.Command.Speed = pitchCommand; + + stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, + fixedWingPathFollowerSettings.PitchLimit.Min, + fixedWingPathFollowerSettings.PitchLimit.Max); + + // Error condition: high speed dive + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; + if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError < 0.0f && // we are too fast already + fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; + result = 0; + } + + /** + * Compute desired roll command + */ + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + // overlap calculation. Theres a dead zone behind the craft where the + // counter-yawing of some craft while rolling could render a desired right + // turn into a desired left turn. Making the turn direction based on + // current roll angle keeps the plane committed to a direction once chosen + if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll > 0.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll < 0.0f) { + courseError -= 360.0f; + } + + courseCommand = pid_apply(&global.PIDcourse, courseError, dT); + + fixedWingPathFollowerStatus.Error.Course = courseError; + fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator; + fixedWingPathFollowerStatus.Command.Course = courseCommand; + + stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + + courseCommand, + fixedWingPathFollowerSettings.RollLimit.Min, + 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.Y + stabDesired.Yaw = 0.0f; + + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + + StabilizationDesiredSet(&stabDesired); + + FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus); + + return result; +} + + +/** + * Function to calculate course vector C based on airspeed s, fluid movement F + * and desired movement vector V + * parameters in: V,F,s + * parameters out: C + * returns true if a valid solution could be found for V,F,s, false if not + * C will be set to a best effort attempt either way + */ +static bool correctCourse(float *C, float *V, float *F, float s) +{ + // Approach: + // Let Sc be a circle around origin marking possible movement vectors + // of the craft with airspeed s (all possible options for C) + // Let Vl be a line through the origin along movement vector V where fr any + // point v on line Vl v = k * (V / |V|) = k' * V + // Let Wl be a line parallel to Vl where for any point v on line Vl exists + // a point w on WL with w = v - F + // Then any intersection between circle Sc and line Wl represents course + // vector which would result in a movement vector + // V' = k * ( V / |V|) = k' * V + // If there is no intersection point, S is insufficient to compensate + // for F and we can only try to fly in direction of V (thus having wind drift + // but at least making progress orthogonal to wind) + + s = fabsf(s); + float f = vector_lengthf(F, 2); + + // normalize Cn=V/|V|, |V| must be >0 + float v = vector_lengthf(V, 2); + if (v < 1e-6f) { + // if |V|=0, we aren't supposed to move, turn into the wind + // (this allows hovering) + C[0] = -F[0]; + C[1] = -F[1]; + // if desired airspeed matches fluidmovement a hover is actually + // intended so return true + return fabsf(f - s) < 1e-3f; + } + float Vn[2] = { V[0] / v, V[1] / v }; + + // project F on V + float fp = F[0] * Vn[0] + F[1] * Vn[1]; + + // find component Fo of F that is orthogonal to V + // (which is exactly the distance between Vl and Wl) + float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; + float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; + + // find k where k * Vn = C - Fo + // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo + // so k^2 + fo^2 = s^2 (since |Vn|=1) + float k2 = s * s - fo2; + if (k2 <= -1e-3f) { + // there is no solution, we will be drifted off either way + // fallback: fly stupidly in direction of V and hope for the best + C[0] = V[0]; + C[1] = V[1]; + return false; + } else if (k2 <= 1e-3f) { + // there is exactly one solution: -Fo + C[0] = -Fo[0]; + C[1] = -Fo[1]; + return true; + } + // we have two possible solutions k positive and k negative as there are + // two intersection points between Wl and Sc + // which one is better? two criteria: + // 1. we MUST move in the right direction, if any k leads to -v its invalid + // 2. we should minimize the speed error + float k = sqrt(k2); + float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; + float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; + // project C+F on Vn to find signed resulting movement vector length + float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; + float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; + if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { + // in this case the angle between course and resulting movement vector + // is greater than 90 degrees - so we actually fly backwards + C[0] = C1[0]; + C[1] = C1[1]; + return true; + } + C[0] = C2[0]; + C[1] = C2[1]; + if (vp2 >= 0.0f) { + // in this case the angle between course and movement vector is less than + // 90 degrees, but we do move in the right direction + return true; + } else { + // in this case we actually get driven in the opposite direction of V + // with both solutions for C + // this might be reached in headwind stronger than maximum allowed + // airspeed. + return false; + } +} + +/** + * Compute desired attitude from the desired velocity + * + * Takes in @ref NedState which has the acceleration in the + * NED frame as the feedback term and then compares the + * @ref VelocityState against the @ref VelocityDesired + */ +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) +{ + const float dT = updatePeriod / 1000.0f; + uint8_t result = 1; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + SystemSettingsData systemSettings; + + float northError; + float northCommand; + + float eastError; + float eastCommand; + + float downError; + float downCommand; + + SystemSettingsGet(&systemSettings); + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + // Testing code - refactor into manual control command + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + // scale velocity if it is above configured maximum + float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + if (velH > vtolPathFollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; + velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; + } + if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { + velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); + } + + // Compute desired north command + northError = velocityDesired.North - velocityState.North; + northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; + + // Compute desired east command + eastError = velocityDesired.East - velocityState.East; + eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + downCommand = pid_apply(&global.PIDvel[2], downError, dT); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { + attitudeState.Yaw += 120.0f; + if (attitudeState.Yaw > 180.0f) { + attitudeState.Yaw -= 360.0f; + } + } + + if ( // emergency flyaway detection + ( // integral already at its limit + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f + ) && + // angle between desired and actual velocity >90 degrees (by dot product) + (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && + // quad is moving at significant speed (during flyaway it would keep speeding up) + squaref(velocityState.North) + squaref(velocityState.East) > 1.0f + ) { + global.vtolEmergencyFallback += dT; + if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { + // after emergency timeout, trigger alarm - everything else is handled by callers + // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + result = 0; + } + } else { + global.vtolEmergencyFallback = 0.0f; + } + + // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the + // craft should move similarly for 5 deg roll versus 5 deg pitch + stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + if (yaw_attitude) { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Yaw = yaw_direction; + } else { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; + } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); + + return result; +} + +/** + * Compute desired attitude for vtols - emergency fallback + */ +static void updateVtolDesiredAttitudeEmergencyFallback() +{ + const float dT = updatePeriod / 1000.0f; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + + float courseError; + float courseCommand; + + float downError; + float downCommand; + + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + + courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP); + + stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max); + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + downCommand = pid_apply(&global.PIDvel[2], downError, dT); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll; + stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch; + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); +} + + +/** + * 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.Thrust = attitude[3]; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + StabilizationDesiredSet(&stabDesired); +} diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 745a8c17e..32d2afbae 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -525,9 +525,8 @@ static uint8_t conditionLegRemaining() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -550,9 +549,8 @@ static uint8_t conditionBelowError() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } diff --git a/flight/modules/VtolPathFollower/inc/vtolpathfollower.h b/flight/modules/VtolPathFollower/inc/vtolpathfollower.h deleted file mode 100644 index f97f1588b..000000000 --- a/flight/modules/VtolPathFollower/inc/vtolpathfollower.h +++ /dev/null @@ -1,31 +0,0 @@ -/** - ****************************************************************************** - * - * @file vtolpathfollower.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Module to perform path following for VTOL. - * - * @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 - */ -#ifndef VTOLPATHFOLLOWER_H -#define VTOLPATHFOLLOWER_H - -int32_t VtolPathFollowerInitialize(void); - -#endif // VTOLPATHFOLLOWER_H diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c deleted file mode 100644 index 57d96fad9..000000000 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ /dev/null @@ -1,729 +0,0 @@ -/** - ****************************************************************************** - * - * @file vtolpathfollower.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief This module compared @ref PositionState to @ref PathDesired - * and sets @ref Stabilization. It only does this when the FlightMode field - * of @ref FlightStatus is PathPlanner or RTH. - * - * @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: FlightStatus - * Input object: PathDesired - * Input object: PositionState - * Output object: StabilizationDesired - * - * This module will periodically update the value of the @ref StabilizationDesired object based on - * @ref PathDesired and @PositionState when the Flight Mode selected in @FlightStatus is supported - * by this module. Otherwise another module (e.g. @ref ManualControlCommand) is expected to be - * writing to @ref StabilizationDesired. - * - * 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 -#include -#include "vtolpathfollower.h" - -#include "accelstate.h" -#include "attitudestate.h" -#include "hwsettings.h" -#include "pathdesired.h" // object that will be updated by the module -#include "positionstate.h" -#include "manualcontrolcommand.h" -#include "flightstatus.h" -#include "pathstatus.h" -#include "gpsvelocitysensor.h" -#include "gpspositionsensor.h" -#include "homelocation.h" -#include "vtolpathfollowersettings.h" -#include "nedaccel.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "stabilizationbank.h" -#include "systemsettings.h" -#include "velocitydesired.h" -#include "velocitystate.h" -#include "taskinfo.h" - -#include "paths.h" -#include "CoordinateConversions.h" -#include - -#include "cameradesired.h" -#include "poilearnsettings.h" -#include "poilocation.h" -#include "accessorydesired.h" - -// Private constants -#define MAX_QUEUE_SIZE 4 -#define STACK_SIZE_BYTES 1548 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) - -// Private types - -// Private variables -static xTaskHandle pathfollowerTaskHandle; -static PathStatusData pathStatus; -static VtolPathFollowerSettingsData vtolpathfollowerSettings; -static float poiRadius; - -// Private functions -static void vtolPathFollowerTask(void *parameters); -static void SettingsUpdatedCb(UAVObjEvent *ev); -static void updateNedAccel(); -static void updatePOIBearing(); -static void updatePathVelocity(); -static void updateEndpointVelocity(); -static void updateFixedAttitude(float *attitude); -static void updateVtolDesiredAttitude(bool yaw_attitude); -static bool vtolpathfollower_enabled; -static void accessoryUpdated(UAVObjEvent *ev); - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t VtolPathFollowerStart() -{ - if (vtolpathfollower_enabled) { - // Start main task - xTaskCreate(vtolPathFollowerTask, "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } - - return 0; -} - -/** - * Initialise the module, called on startup - * \returns 0 on success or -1 if initialisation failed - */ -int32_t VtolPathFollowerInitialize() -{ - HwSettingsOptionalModulesData optionalModules; - - HwSettingsOptionalModulesGet(&optionalModules); - FrameType_t frameType = GetCurrentFrameType(); - - if ((optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) || - (frameType == FRAME_TYPE_MULTIROTOR)) { - VtolPathFollowerSettingsInitialize(); - NedAccelInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - CameraDesiredInitialize(); - AccessoryDesiredInitialize(); - PoiLearnSettingsInitialize(); - PoiLocationInitialize(); - vtolpathfollower_enabled = true; - } else { - vtolpathfollower_enabled = false; - } - - return 0; -} - -MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart); - -static float northVelIntegral = 0; -static float eastVelIntegral = 0; -static float downVelIntegral = 0; - -static float northPosIntegral = 0; -static float eastPosIntegral = 0; -static float downPosIntegral = 0; - -static float thrustOffset = 0; -/** - * Module thread, should not return. - */ -static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) -{ - SystemSettingsData systemSettings; - FlightStatusData flightStatus; - - portTickType lastUpdateTime; - - VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - AccessoryDesiredConnectCallback(accessoryUpdated); - - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { - // Conditions when this runs: - // 1. Must have VTOL 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_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX)) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } - - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); - - // Convert the accels into the NED frame - updateNedAccel(); - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - - // Check the combinations of flightmode and pathdesired mode - if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - } - } else { - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - } - } - } else { - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - 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_CRITICAL); - break; - } - PathStatusSet(&pathStatus); - } - } else { - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - northPosIntegral = 0; - eastPosIntegral = 0; - downPosIntegral = 0; - - // Track thrust before engaging this mode. Cheap system ident - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - thrustOffset = stabDesired.Thrust; - } - - AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); - } -} - -/** - * Compute bearing and elevation between current position and POI - */ -static void updatePOIBearing() -{ - const float DEADBAND_HIGH = 0.10f; - const float DEADBAND_LOW = -0.10f; - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - PositionStateData positionState; - PositionStateGet(&positionState); - CameraDesiredData cameraDesired; - CameraDesiredGet(&cameraDesired); - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - PoiLocationData poi; - PoiLocationGet(&poi); - - float dLoc[3]; - float yaw = 0; - /*float elevation = 0;*/ - - dLoc[0] = positionState.North - poi.North; - dLoc[1] = positionState.East - poi.East; - dLoc[2] = positionState.Down - poi.Down; - - if (dLoc[1] < 0) { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; - } else { - yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; - } - - // distance - float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); - - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - float changeRadius = 0; - // Move closer or further, radially - if (manualControlData.Pitch > DEADBAND_HIGH) { - changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; - } else if (manualControlData.Pitch < DEADBAND_LOW) { - changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; - } - - // move along circular path - float pathAngle = 0; - if (manualControlData.Roll > DEADBAND_HIGH) { - pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; - } else if (manualControlData.Roll < DEADBAND_LOW) { - pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; - } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { - // change radius only when not circling - poiRadius = distance + changeRadius; - } - - // don't try to move any closer - if (poiRadius >= 3.0f || changeRadius > 0) { - if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.StartingVelocity = 1.0f; - pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } - } - // not above - if (distance >= 3.0f) { - // You can feed this into camerastabilization - /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ - - stabDesired.Yaw = yaw + (pathAngle / 2.0f); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - - // cameraDesired.Yaw=yaw; - // cameraDesired.PitchOrServo2=elevation; - - CameraDesiredSet(&cameraDesired); - StabilizationDesiredSet(&stabDesired); - } -} - -/** - * Compute desired velocity from the current position and path - * - * Takes in @ref PositionState and compares it to @ref PathDesired - * and computes @ref VelocityDesired - */ -static void updatePathVelocity() -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - float downCommand; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - PositionStateData positionState; - PositionStateGet(&positionState); - - float cur[3] = - { positionState.North, positionState.East, positionState.Down }; - struct path_status progress; - - path_progress( - cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); - - float groundspeed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - groundspeed = 0; - } - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - groundspeed = 0; - } - break; - } - - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; - - float error_speed = progress.error * vtolpathfollowerSettings.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 > 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; - - float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1); - - float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); - - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - - VelocityDesiredSet(&velocityDesired); -} - -/** - * Compute desired velocity from the current position - * - * Takes in @ref PositionState and compares it to @ref PositionDesired - * and computes @ref VelocityDesired - */ -void updateEndpointVelocity() -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - - PositionStateData positionState; - VelocityDesiredData velocityDesired; - - PositionStateGet(&positionState); - VelocityDesiredGet(&velocityDesired); - - float northError; - float eastError; - float downError; - float northCommand; - float eastCommand; - float downCommand; - - // Compute desired north command - northError = pathDesired.End.North - positionState.North; - northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral); - - eastError = pathDesired.End.East - positionState.East; - eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral); - - // Limit the maximum velocity - float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; - } - - velocityDesired.North = northCommand * scale; - velocityDesired.East = eastCommand * scale; - - downError = pathDesired.End.Down - positionState.Down; - downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); - - 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.Thrust = attitude[3]; - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Compute desired attitude from the desired velocity - * - * Takes in @ref NedState which has the acceleration in the - * NED frame as the feedback term and then compares the - * @ref VelocityState against the @ref VelocityDesired - */ -static void updateVtolDesiredAttitude(bool yaw_attitude) -{ - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - - VelocityDesiredData velocityDesired; - VelocityStateData velocityState; - StabilizationDesiredData stabDesired; - AttitudeStateData attitudeState; - NedAccelData nedAccel; - StabilizationBankData stabSettings; - SystemSettingsData systemSettings; - - float northError; - float northCommand; - - float eastError; - float eastCommand; - - float downError; - float downCommand; - - SystemSettingsGet(&systemSettings); - VelocityStateGet(&velocityState); - VelocityDesiredGet(&velocityDesired); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeStateGet(&attitudeState); - StabilizationBankGet(&stabSettings); - NedAccelGet(&nedAccel); - - float northVel = 0, eastVel = 0, downVel = 0; - switch (vtolpathfollowerSettings.VelocitySource) { - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_STATE_ESTIMATION: - northVel = velocityState.North; - eastVel = velocityState.East; - downVel = velocityState.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_VELNED: - { - GPSVelocitySensorData gpsVelocity; - GPSVelocitySensorGet(&gpsVelocity); - northVel = gpsVelocity.North; - eastVel = gpsVelocity.East; - downVel = gpsVelocity.Down; - } - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPS_GROUNDSPEED: - { - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading)); - eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading)); - downVel = velocityState.Down; - } - break; - default: - PIOS_Assert(0); - break; - } - - // Testing code - refactor into manual control command - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - // Compute desired north command - northError = velocityDesired.North - northVel; - northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd - + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired east command - eastError = velocityDesired.East - eastVel; - eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd - + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired down command - downError = velocityDesired.Down - downVel; - // Must flip this sign - downError = -downError; - downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, - -vtolpathfollowerSettings.VerticalVelPID.ILimit, - vtolpathfollowerSettings.VerticalVelPID.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); - - stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1); - - // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the - // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + - -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + - eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - - if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) { - // For now override thrust with manual control. Disable at your risk, quad goes to China. - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - stabDesired.Thrust = manualControl.Thrust; - } - - stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - if (yaw_attitude) { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - } else { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; - } - stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - StabilizationDesiredSet(&stabDesired); -} - -/** - * Keep a running filtered version of the acceleration in the NED frame - */ -static void updateNedAccel() -{ - float accel[3]; - float q[4]; - float Rbe[3][3]; - float accel_ned[3]; - - // Collect downsampled attitude data - AccelStateData accelState; - - AccelStateGet(&accelState); - accel[0] = accelState.x; - accel[1] = accelState.y; - accel[2] = accelState.z; - - // rotate avg accels into earth frame and store it - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - q[0] = attitudeState.q1; - q[1] = attitudeState.q2; - q[2] = attitudeState.q3; - q[3] = attitudeState.q4; - Quaternion2R(q, Rbe); - for (uint8_t i = 0; i < 3; i++) { - accel_ned[i] = 0; - for (uint8_t j = 0; j < 3; j++) { - accel_ned[i] += Rbe[j][i] * accel[j]; - } - } - accel_ned[2] += 9.81f; - - NedAccelData accelData; - NedAccelGet(&accelData); - accelData.North = accel_ned[0]; - accelData.East = accel_ned[1]; - accelData.Down = accel_ned[2]; - NedAccelSet(&accelData); -} - -static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); -} - -static void accessoryUpdated(UAVObjEvent *ev) -{ - if (ev->obj != AccessoryDesiredHandle()) { - return; - } - - AccessoryDesiredData accessory; - PoiLearnSettingsData poiLearn; - PoiLearnSettingsGet(&poiLearn); - - if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { - if (accessory.AccessoryVal < -0.5f) { - PositionStateData positionState; - PositionStateGet(&positionState); - PoiLocationData poi; - PoiLocationGet(&poi); - poi.North = positionState.North; - poi.East = positionState.East; - poi.Down = positionState.Down; - PoiLocationSet(&poi); - } - } - } -} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..bc8e72959 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -35,7 +35,6 @@ USE_DSP_LIB ?= NO #MODULES += Airspeed #MODULES += AltitudeHold #MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Actuator MODULES += GPS @@ -45,7 +44,7 @@ MODULES += Battery MODULES += FirmwareIAP #MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..52b5f8a64 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -46,7 +45,7 @@ MODULES += Battery MODULES += FirmwareIAP MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 39d2d7f3b..9933124a3 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -45,7 +44,7 @@ MODULES += CameraStab MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 72b2acb47..f8a654d73 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -33,8 +33,7 @@ DEBUG ?= YES # List of modules to include MODULES = ManualControl Stabilization GPS MODULES += PathPlanner -MODULES += FixedWingPathFollower -MODULES += VtolPathFollower +MODULES += PathFollower MODULES += CameraStab MODULES += Telemetry MODULES += Logging diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 2e3f56306..d14a816eb 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -3,7 +3,7 @@ include(openpilotgcs.pri) TEMPLATE = subdirs # Copy Qt runtime libraries into the build directory (to run or package) -equals(copydata, 1) { +equals(copyqt, 1) { GCS_LIBRARY_PATH diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 31c0ed454..fd97c3c96 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -83,16 +83,26 @@ macx { GCS_DATA_BASENAME = Resources GCS_DOC_PATH = $$GCS_DATA_PATH/doc copydata = 1 + copyqt = 1 } else { + !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 win32 { contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1 GCS_APP_TARGET = openpilotgcs + copyqt = $$copydata } else { GCS_APP_WRAPPER = openpilotgcs GCS_APP_TARGET = openpilotgcs.bin GCS_QT_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5 GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml + lib_dir_is_in_tools = $$[QT_INSTALL_LIBS] + lib_dir_is_in_tools ~= s,$$(TOOLS_DIR)*,TRUE + equals(lib_dir_is_in_tools, "TRUE") { + copyqt = 1 + } else { + copyqt = 0 + } } GCS_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/openpilotgcs GCS_PLUGIN_PATH = $$GCS_LIBRARY_PATH/plugins @@ -100,7 +110,6 @@ macx { GCS_DATA_PATH = $$GCS_BUILD_TREE/share/openpilotgcs GCS_DATA_BASENAME = share/openpilotgcs GCS_DOC_PATH = $$GCS_BUILD_TREE/share/doc - !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 } diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 8624867c4..598e4988a 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -8,6 +8,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -20,6 +21,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -36,6 +38,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 83bc91e25..8a7e5d064 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -15,9 +15,9 @@ - + - + @@ -38,7 +38,7 @@ in relation to vertical speed error (absolute but including crossfeed) --> - + @@ -46,7 +46,7 @@ + defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" /> diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index b4b5a1c50..5511345ae 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -110,11 +110,10 @@ + - - diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..c41187009 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -22,7 +22,7 @@ - + diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 52352dc4a..812b7a1cf 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -7,7 +7,12 @@ - + + + + + + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index a5fb7ad4b..f8018ab53 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -19,7 +19,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -52,7 +51,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -89,7 +87,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 8a7d9991a..fec77cb55 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,18 +1,24 @@ Settings for the @ref VtolPathFollowerModule - - - - - - - + + + + + + + + + - - - - + + + + + + + +