void plan_setup_VelocityRoam()
{
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
AttitudeStateYawGet(&vario_course);
}
static void getVector(float controlVector[4], vario_type type)
{
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
// scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive
controlVector[3] *= offset.Vertical / offset.Horizontal;
float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]);
if (length <= 1e-9f) {
length = 1.0f; // should never happen as getVector is not called if control within deadband
}
{
float direction[3] = {
controlVector[1] / length, // pitch is north
controlVector[0] / length, // roll is east
controlVector[3] / length // thrust is down
};
controlVector[0] = direction[0];
controlVector[1] = direction[1];
controlVector[2] = direction[2];
}
controlVector[3] = length * offset.Horizontal;
// rotate north and east - rotation angle based on type
float angle;
switch (type) {
case NSEW:
angle = 0.0f;
// NSEW no rotation takes place
break;
case FPV:
// local rotation, using current yaw
AttitudeStateYawGet(&angle);
break;
case LOS:
// determine location based on vector from takeoff to current location
{
PositionStateData positionState;
PositionStateGet(&positionState);
TakeOffLocationData takeoffLocation;
TakeOffLocationGet(&takeoffLocation);
angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North));
}
break;
}
// rotate horizontally by angle
{
float rotated[2] = {
controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle),
controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle)
};
controlVector[0] = rotated[0];
controlVector[1] = rotated[1];
}
}
static void plan_setup_PositionVario()
{
vario_hold = true;
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
AttitudeStateYawGet(&vario_course);
plan_setup_positionHold();
}
void plan_setup_AutoCruise()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
// initialization is flight in direction of the nose.
// the velocity is not relevant, as it will be reset by the run function even during first call
float angle;
AttitudeStateYawGet(&angle);
float vector[2] = {
cos_lookup_deg(angle),
sin_lookup_deg(angle)
};
hold_position[0] = positionState.North;
hold_position[1] = positionState.East;
hold_position[2] = positionState.Down;
pathDesired.End.North = hold_position[0] + vector[0];
pathDesired.End.East = hold_position[1] + vector[1];
pathDesired.End.Down = hold_position[2];
// start position has the same offset as in position hold
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
// re-iniztializing deltatime is valid and also good practice here since
// getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode.
PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
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);
}
}
