From 4653c7729616154a9de02234b0dd9ed0afebe23c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 20 Aug 2014 17:00:50 +0200 Subject: [PATCH] OP-1156 removed old pathfollower implementations --- .../fixedwingpathfollower.c | 772 ------------------ .../VtolPathFollower/inc/vtolpathfollower.h | 31 - .../VtolPathFollower/vtolpathfollower.c | 737 ----------------- 3 files changed, 1540 deletions(-) delete mode 100644 flight/modules/FixedWingPathFollower/fixedwingpathfollower.c delete mode 100644 flight/modules/VtolPathFollower/inc/vtolpathfollower.h delete mode 100644 flight/modules/VtolPathFollower/vtolpathfollower.c 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/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 8d8b753b7..000000000 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ /dev/null @@ -1,737 +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; - - PathDesiredData pathDesired; - - PathDesiredGet(&pathDesired); - PositionStateData positionState; - PositionStateGet(&positionState); - - float current_position[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), - current_position, &progress, pathDesired.Mode); - - float speed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - speed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - speed = 0; - } - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - speed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); - if (progress.fractional_progress > 1) { - speed = 0; - } - break; - } - - VelocityDesiredData velocityDesired; - - northPosIntegral += progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; - eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; - downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - - northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - - velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + - progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + - progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + - progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; - - // Make sure the desired velocities don't exceed PathFollower limits. - float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); - - if (groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax) { - velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; - velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; - } - - velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); - - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_direction[0]; - pathStatus.path_direction_east = progress.path_direction[1]; - pathStatus.path_direction_down = progress.path_direction[2]; - - pathStatus.correction_direction_north = progress.correction_direction[0]; - pathStatus.correction_direction_east = progress.correction_direction[1]; - pathStatus.correction_direction_down = progress.correction_direction[2]; - 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); - } - } - } -}