LibrePilot/flight/modules/PathFollower/pathfollower.c

/**
 ******************************************************************************
 *
 * @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 <openpilot.h>

#include <callbackinfo.h>

#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <pios_struct_helper.h>
#include <sin_lookup.h>
#include <paths.h>
#include <sanitycheck.h>


#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <pathdesired.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>


// 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 Integrals {
    float vel[3];
    float course;
    float speed;
    float power;
    float airspeed;
    float poiRadius;
    bool  vtolEmergencyFallback;
};


// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static struct Integrals i;
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 resetIntegrals();
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, float kH, float kV, 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
    resetIntegrals();

    // 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) {
        resetIntegrals();
        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 that shit 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);
    VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
    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 resetIntegrals()
{
    i.vel[0]    = 0.0f;
    i.vel[1]    = 0.0f;
    i.vel[2]    = 0.0f;
    i.course    = 0.0f;
    i.speed     = 0.0f;
    i.power     = 0.0f;
    i.airspeed  = 0.0f;
    i.poiRadius = 0.0f;
    i.vtolEmergencyFallback = 0;
}

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()
{
    updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, 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()
{
    if (!i.vtolEmergencyFallback) {
        if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
            updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true);
            updateVtolDesiredAttitudeEmergencyFallback();
            return 1;
        } else {
            updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false);
            uint8_t result    = 1;
            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;
            }
            updateVtolDesiredAttitude(yaw_attitude, yaw);
            if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) {
                i.vtolEmergencyFallback = true;
            }
            return result;
        }
    } else {
        updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true);
        updateVtolDesiredAttitudeEmergencyFallback();
        return 0;
    }
}


/**
 * 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
    float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
    // result is in between 0 and 360 degrees
    if (yaw < 0.0f) {
        yaw += 360.0f;
    }
    return yaw;
}

/**
 * Compute bearing of current movement direction
 */
static float updateCourseBearing()
{
    VelocityStateData v;

    VelocityStateGet(&v);
    // atan2f always returns in between + and - 180 degrees
    float yaw = RAD2DEG(atan2f(v.East, v.North));
    // result is in between 0 and 360 degrees
    if (yaw < 0.0f) {
        yaw += 360.0f;
    }
    return yaw;
}

/**
 * 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(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
                  cast_struct_to_array(pathDesired.End, pathDesired.End.North),
                  cur, &progress, pathDesired.Mode);

    // atan2f always returns in between + and - 180 degrees
    float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]));
    // result is in between 0 and 360 degrees
    if (yaw < 0.0f) {
        yaw += 360.0f;
    }
    return yaw;
}

/**
 * Compute bearing between current position and POI
 */
static float updatePOIBearing()
{
    PoiLocationData poi;

    PoiLocationGet(&poi);
    PositionStateData positionState;
    PositionStateGet(&positionState);

    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()
{
    float dT = updatePeriod / 1000.0f;


    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
        i.poiRadius = distance + changeRadius;
    }

    // don't try to move any closer
    if (i.poiRadius >= 3.0f || changeRadius > 0) {
        if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
            pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
            pathDesired.End.East  = poi.East + (i.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));*/

        // cameraDesired.Yaw=yaw;
        // cameraDesired.PitchOrServo2=elevation;

        // CameraDesiredSet(&cameraDesired);
    }
}

/**
 * Compute desired velocity from the current position and path
 */
static void updatePathVelocity(float kFF, float kH, float kV, bool limited)
{
    PositionStateData positionState;

    PositionStateGet(&positionState);
    VelocityStateData velocityState;
    VelocityStateGet(&velocityState);

    // 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(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:
    case PATHDESIRED_MODE_FLYVECTOR:
    case PATHDESIRED_MODE_DRIVEVECTOR:
    default:
        groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
                      boundf(progress.fractional_progress, 0.0f, 1.0f);
        break;
    }
    // make sure groundspeed is not zero
    if (limited && 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];
    velocityDesired.Down  = progress.path_direction[2];

    float error_speed_horizontal = progress.error * kH;
    float error_speed_vertical   = progress.error * kV;

    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
        ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) &&
        ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) {
        error_speed_horizontal = 0.0f;
        error_speed_vertical   = 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_horizontal;
    velocityDesired.East  += progress.correction_direction[1] * error_speed_horizontal;
    velocityDesired.Down  += progress.correction_direction[2] * error_speed_vertical;

    // scale to correct length
    float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down);
    if (l > 1e-9f) {
        velocityDesired.North *= groundspeed / l;
        velocityDesired.East  *= groundspeed / l;
        velocityDesired.Down  *= groundspeed / l;
    } else {
        velocityDesired.North = 0.0f;
        velocityDesired.East  = 0.0f;
        velocityDesired.Down  = 0.0f;
    }

    // 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 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 saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
    if (fixedWingPathFollowerSettings.PowerPI.Ki > 0.0f) {
        i.power = boundf(i.power + -descentspeedError * dT,
                         -fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki,
                         fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki
                         ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
    } else {
        i.power = 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 +
                   i.power * fixedWingPathFollowerSettings.PowerPI.Ki +
                   speedErrorToPowerCommandComponent;

    // Output internal state to telemetry
    fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
    fixedWingPathFollowerStatus.ErrorInt.Power = i.power;
    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
        i.airspeed = boundf(i.airspeed + 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
                     + i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki
                     ) + verticalSpeedToPitchCommandComponent;

    fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
    fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed;
    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;
    }

    i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki,
                      -fixedWingPathFollowerSettings.CoursePI.ILimit,
                      fixedWingPathFollowerSettings.CoursePI.ILimit);
    courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp +
                     i.course);

    fixedWingPathFollowerStatus.Error.Course    = courseError;
    fixedWingPathFollowerStatus.ErrorInt.Course = i.course;
    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)
{
    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);

    // Compute desired north command
    northError   = velocityDesired.North - velocityState.North;
    i.vel[0]     = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki,
                          -vtolPathFollowerSettings.HorizontalVelPI.ILimit,
                          vtolPathFollowerSettings.HorizontalVelPI.ILimit);
    northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0]
                    + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward);

    // Compute desired east command
    eastError   = velocityDesired.East - velocityState.East;
    i.vel[1]    = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki,
                         -vtolPathFollowerSettings.HorizontalVelPI.ILimit,
                         vtolPathFollowerSettings.HorizontalVelPI.ILimit);
    eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1]
                   + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward);

    // Compute desired down command
    downError   = velocityDesired.Down - velocityState.Down;
    // Must flip this sign
    downError   = -downError;
    i.vel[2]    = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki,
                         -vtolPathFollowerSettings.VerticalVelPI.ILimit,
                         vtolPathFollowerSettings.VerticalVelPI.ILimit);
    downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]);

    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
        (fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit
        velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees
        ) {
        result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
    }

    // 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()
{
    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;
    i.vel[2]    = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki,
                         -vtolPathFollowerSettings.VerticalVelPI.ILimit,
                         vtolPathFollowerSettings.VerticalVelPI.ILimit);
    downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]);

    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);
}