Beispiel #1
0
/**
 * Select and use failsafe control
 * @param [in] reset_controller True if previously another controller was used
 */
int32_t failsafe_control_select(bool reset_controller)
{
	if (reset_controller) {
		FlightStatusArmedOptions armed; 
		FlightStatusArmedGet(&armed);
		armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
	}

	uint8_t flight_status;
	FlightStatusFlightModeGet(&flight_status);
	if (flight_status != FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 || reset_controller) {
		flight_status = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
		FlightStatusFlightModeSet(&flight_status);
	}

#ifdef GIMBAL
	// Gimbals do not need failsafe
	StabilizationDesiredData stabilization_desired;
	StabilizationDesiredGet(&stabilization_desired);
	stabilization_desired.Throttle = -1;
	stabilization_desired.Roll = 0;
	stabilization_desired.Pitch = 0;
	stabilization_desired.Yaw = 0;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
	StabilizationDesiredSet(&stabilization_desired);
#else
	// Pick default values that will roughly cause a plane to circle down
	// and a quad to fall straight down
	StabilizationDesiredData stabilization_desired;
	StabilizationDesiredGet(&stabilization_desired);

	if (!armed_when_enabled) {
		/* disable stabilization so outputs do not move when system was not armed */
		stabilization_desired.Throttle = -1;
		stabilization_desired.Roll  = 0;
		stabilization_desired.Pitch = 0;
		stabilization_desired.Yaw   = 0;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;		
	} else {
		/* Pick default values that will roughly cause a plane to circle down and */
		/* a quad to fall straight down */
		stabilization_desired.Throttle = -1;
		stabilization_desired.Roll = -10;
		stabilization_desired.Pitch = 0;
		stabilization_desired.Yaw = -5;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
		stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
	}

	StabilizationDesiredSet(&stabilization_desired);
#endif

	return 0;
}
Beispiel #2
0
/**
 * Select and use transmitter control
 * @param [in] reset_controller True if previously another controller was used
 */
int32_t transmitter_control_select(bool reset_controller)
{
	// Activate the flight mode corresponding to the switch position
	set_flight_mode();

	ManualControlCommandGet(&cmd);

	uint8_t flightMode;
	FlightStatusFlightModeGet(&flightMode);

	// Depending on the mode update the Stabilization or Actuator objects
	static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;

	switch(flightMode) {
	case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
		update_actuator_desired(&cmd);
		break;
	case FLIGHTSTATUS_FLIGHTMODE_ACRO:
	case FLIGHTSTATUS_FLIGHTMODE_ACROPLUS:      
	case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
	case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
	case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
	case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
	case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
	case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
	case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
	case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
		update_stabilization_desired(&cmd, &settings);
		break;
	case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
		// Tuning takes settings directly from manualcontrolcommand.  No need to
		// call anything else.  This just avoids errors.
		break;
	case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
		altitude_hold_desired(&cmd, lastFlightMode != flightMode);
		break;
	case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
		set_loiter_command(&cmd);
	case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
		// The path planner module processes data here
		break;
	case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
		break;
	default:
		set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_UNDEFINED);
		break;
	}
	lastFlightMode = flightMode;

	return 0;
}
/**
 * @brief Determine if the aircraft is safe to arm
 * @returns True if safe to arm, false otherwise
 */
static bool okToArm(void)
{
    // update checks
    configuration_check();

    // read alarms
    SystemAlarmsData alarms;

    SystemAlarmsGet(&alarms);

    // Check each alarm
    for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
        if (SystemAlarmsAlarmToArray(alarms.Alarm)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
            if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
                continue;
            }

            return false;
        }
    }

    StabilizationDesiredStabilizationModeData stabDesired;

    uint8_t flightMode;
    FlightStatusFlightModeGet(&flightMode);
    switch (flightMode) {
    case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
        // Prevent arming if unsafe due to the current Thrust Mode
        StabilizationDesiredStabilizationModeGet(&stabDesired);
        if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ||
            stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) {
            return false;
        } else {
            return true;
        }
        break;

    default:
        return false;

        break;
    }
}
Beispiel #4
0
void updatedCb(UAVObjEvent *ev)
{
    if (!ev || ev->obj == FlightStatusHandle()) {
        static uint8_t last_armed = 0xff;
        static uint8_t last_flightmode = 0xff;
        uint8_t armed;
        uint8_t flightmode;
        FlightStatusArmedGet(&armed);
        FlightStatusFlightModeGet(&flightmode);
        if (last_armed != armed || (armed && flightmode != last_flightmode)) {
            if (armed) {
                PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND);
            } else {
                PIOS_NOTIFICATION_Default_Ext_Led_Play(&notifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND);
            }
        }
        last_armed = armed;
        last_flightmode = flightmode;
    }
}
/**
 * Select and use failsafe control
 * @param [in] reset_controller True if previously another controller was used
 */
int32_t failsafe_control_select(bool reset_controller)
{
	if (reset_controller) {
		FlightStatusArmedOptions armed; 
		FlightStatusArmedGet(&armed);
		armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
		rth_active = false;

		/* Get configured failsafe behavior */
		uint8_t failsafe_behavior;
		ManualControlSettingsFailsafeBehaviorGet(&failsafe_behavior);

		/* check whether RTH failsafe is configured and can be used */
		if (failsafe_behavior == MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_RTH
				&& check_rth_preconditions_met())
			rth_active = true;
	}

	uint8_t flight_mode_desired = rth_active ? FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME : FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
	uint8_t flight_mode_actual;
	FlightStatusFlightModeGet(&flight_mode_actual);
	if (flight_mode_actual != flight_mode_desired || reset_controller) {
		FlightStatusFlightModeSet(&flight_mode_desired);
	}

#ifdef GIMBAL
	// Gimbals do not need failsafe
	StabilizationDesiredData stabilization_desired;
	StabilizationDesiredGet(&stabilization_desired);
	stabilization_desired.Throttle = -1;
	stabilization_desired.Roll = 0;
	stabilization_desired.Pitch = 0;
	stabilization_desired.Yaw = 0;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
	stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
	StabilizationDesiredSet(&stabilization_desired);
#else
	if (!rth_active) {
		// Pick default values that will roughly cause a plane to circle down
		// and a quad to fall straight down
		StabilizationDesiredData stabilization_desired;
		StabilizationDesiredGet(&stabilization_desired);

		if (!armed_when_enabled) {
			/* disable stabilization so outputs do not move when system was not armed */
			stabilization_desired.Throttle = -1;
			stabilization_desired.Roll  = 0;
			stabilization_desired.Pitch = 0;
			stabilization_desired.Yaw   = 0;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
		} else {
			/* Pick default values that will roughly cause a plane to circle down and */
			/* a quad to fall straight down */
			stabilization_desired.Throttle = -1;
			stabilization_desired.Roll = -10;
			stabilization_desired.Pitch = 0;
			stabilization_desired.Yaw = -5;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
			stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
		}

		StabilizationDesiredSet(&stabilization_desired);
	}
#endif

	return 0;
}
Beispiel #6
0
/**
 * Module thread, should not return.
 */
static void altitudeHoldTask(void *parameters)
{
	bool engaged = false;

	AltitudeHoldDesiredData altitudeHoldDesired;
	StabilizationDesiredData stabilizationDesired;
	AltitudeHoldSettingsData altitudeHoldSettings;

	UAVObjEvent ev;
	struct pid velocity_pid;

	// Listen for object updates.
	AltitudeHoldDesiredConnectQueue(queue);
	AltitudeHoldSettingsConnectQueue(queue);
	FlightStatusConnectQueue(queue);

	AltitudeHoldSettingsGet(&altitudeHoldSettings);
	pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
		          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);

	AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);

	// Main task loop
	const uint32_t dt_ms = 5;
	const float dt_s = dt_ms * 0.001f;
	uint32_t timeout = dt_ms;

	while (1) {
		if (PIOS_Queue_Receive(queue, &ev, timeout) != true) {

		} else if (ev.obj == FlightStatusHandle()) {

			uint8_t flight_mode;
			FlightStatusFlightModeGet(&flight_mode);

			if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) {
				// Copy the current throttle as a starting point for integral
				StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator);
				velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000
				engaged = true;
			} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
				engaged = false;

			// Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged
			timeout = engaged ? dt_ms : 100;

		} else if (ev.obj == AltitudeHoldDesiredHandle()) {
			AltitudeHoldDesiredGet(&altitudeHoldDesired);
		} else if (ev.obj == AltitudeHoldSettingsHandle()) {
			AltitudeHoldSettingsGet(&altitudeHoldSettings);

			pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
				          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
		}

		bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE;

		// For landing mode allow throttle to go negative to allow the integrals
		// to stop winding up
		const float min_throttle = landing ? -0.1f : 0.0f;

		// When engaged compute altitude controller output
		if (engaged) {
			float position_z, velocity_z, altitude_error;

			PositionActualDownGet(&position_z);
			VelocityActualDownGet(&velocity_z);
			position_z = -position_z; // Use positive up convention
			velocity_z = -velocity_z; // Use positive up convention

			// Compute the altitude error
			altitude_error = altitudeHoldDesired.Altitude - position_z;

			// Velocity desired is from the outer controller plus the set point
			float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
			float throttle_desired = pid_apply_antiwindup(&velocity_pid, 
			                    velocity_desired - velocity_z,
			                    min_throttle, 1.0f, // positive limits since this is throttle
			                    dt_s);

			if (altitudeHoldSettings.AttitudeComp > 0) {
				// Throttle desired is at this point the mount desired in the up direction, we can
				// account for the attitude if desired
				AttitudeActualData attitudeActual;
				AttitudeActualGet(&attitudeActual);

				// Project a unit vector pointing up into the body frame and
				// get the z component
				float fraction = attitudeActual.q1 * attitudeActual.q1 -
				                 attitudeActual.q2 * attitudeActual.q2 -
				                 attitudeActual.q3 * attitudeActual.q3 +
				                 attitudeActual.q4 * attitudeActual.q4;

				// Add ability to scale up the amount of compensation to achieve
				// level forward flight
				fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f);

				// Dividing by the fraction remaining in the vertical projection will
				// attempt to compensate for tilt. This acts like the thrust is linear
				// with the output which isn't really true. If the fraction is starting
				// to go negative we are inverted and should shut off throttle
				throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f;
			}

			StabilizationDesiredGet(&stabilizationDesired);
			stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f);

			if (landing) {
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
				stabilizationDesired.Roll = 0;
				stabilizationDesired.Pitch = 0;
				stabilizationDesired.Yaw = 0;
			} else {
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
				stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
				stabilizationDesired.Roll = altitudeHoldDesired.Roll;
				stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
				stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
			}
			StabilizationDesiredSet(&stabilizationDesired);
		}

	}
}
/**
 * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
 * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
 * @output: PathDesired
 */
void pathFollowerHandler(bool newinit)
{
    if (newinit) {
        plan_initialize();
    }

    FlightStatusFlightModeOptions flightMode;
    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
    FlightStatusFlightModeAssistOptions flightModeAssist;
    FlightStatusFlightModeGet(&flightMode);
    FlightStatusFlightModeAssistGet(&flightModeAssist);
    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);

    if (newinit) {
        // After not being in this mode for a while init at current height
        switch (flightMode) {
        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
            plan_setup_returnToBase();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
            if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
                (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
                // Switch from primary (just entered this PH flight mode) into brake
                plan_setup_assistedcontrol();
            } else {
                plan_setup_positionHold();
            }
            break;
        case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
            plan_setup_CourseLock();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
            plan_setup_VelocityRoam();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
            plan_setup_HomeLeash();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
            plan_setup_AbsolutePosition();
            break;

        case FLIGHTSTATUS_FLIGHTMODE_LAND:
            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
                plan_setup_land();
            } else {
                plan_setup_VelocityRoam();
            }
            break;
        case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
            plan_setup_AutoTakeoff();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
            plan_setup_AutoCruise();
            break;

        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
            if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
                // Just initiated braking after returning from stabi control
                plan_setup_assistedcontrol();
            }
            break;

        default:
            plan_setup_positionHold();
            break;
        }
    }

    switch (flightMode) {
    case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
        plan_run_CourseLock();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
        plan_run_VelocityRoam();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
        plan_run_HomeLeash();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
        plan_run_AbsolutePosition();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_LAND:
        if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
            plan_run_VelocityRoam();
        }
        break;
    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
        plan_run_AutoTakeoff();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
        plan_run_AutoCruise();
        break;
    default:
        break;
    }
}
Beispiel #8
0
void plan_run_VelocityRoam()
{
    // float alpha;
    PathDesiredData pathDesired;
    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
    FlightStatusFlightModeOptions flightMode;

    PathDesiredGet(&pathDesired);
    FlightModeSettingsPositionHoldOffsetData offset;
    FlightModeSettingsPositionHoldOffsetGet(&offset);
    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
    FlightStatusFlightModeGet(&flightMode);
    StabilizationBankData stabSettings;
    StabilizationBankGet(&stabSettings);

    ManualControlCommandData cmd;
    ManualControlCommandGet(&cmd);

    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);

    bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);

    if (!flagRollPitchHasInput) {
        // no movement desired, re-enter positionHold at current start-position
        if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
            // initiate braking and change assisted control flight mode to braking
            if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
                // avoid brake then hold sequence to continue descent.
                plan_setup_land_from_velocityroam();
            } else {
                plan_setup_assistedcontrol();
            }
        }
        // otherwise nothing to do in braking/hold modes
    } else {
        PositionStateData positionState;
        PositionStateGet(&positionState);

        // Revert assist control state to primary, which in this case implies
        // we are in roaming state (a GPS vector assisted velocity roam)
        assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;

        // Calculate desired velocity in each direction
        float angle;
        AttitudeStateYawGet(&angle);
        angle = DEG2RAD(angle);
        float cos_angle  = cosf(angle);
        float sine_angle = sinf(angle);
        float rotated[2] = {
            -cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
            -cmd.Pitch * sine_angle + cmd.Roll * cos_angle
        };
        // flip pitch to have pitch down (away) point north
        float horizontalVelMax;
        float verticalVelMax;
        VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
        VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
        float velocity_north = rotated[0] * horizontalVelMax;
        float velocity_east  = rotated[1] * horizontalVelMax;
        float velocity_down  = 0.0f;

        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
            FlightModeSettingsLandingVelocityGet(&velocity_down);
        }

        float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
        velocity = sqrtf(velocity);

        // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
        // expect new stick input
        // if two stick input pilot is fighting wind manually and we use fly by velocity
        // in reality setting velocity desired to zero will fight wind anyway.

        pathDesired.Start.North = positionState.North;
        pathDesired.Start.East  = positionState.East;
        pathDesired.Start.Down  = positionState.Down;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]  = velocity_east;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]  = velocity_down;

        pathDesired.End.North = positionState.North;
        pathDesired.End.East  = positionState.East;
        pathDesired.End.Down  = positionState.Down;

        pathDesired.StartingVelocity = velocity;
        pathDesired.EndingVelocity   = velocity;
        pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
            pathDesired.Mode = PATHDESIRED_MODE_LAND;
            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
        } else {
            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
        }
        PathDesiredSet(&pathDesired);
        FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
    }
}