/**
* 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;
}
/**
* 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;
}
}
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(¬ifications[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;
}
/**
* 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;
}
}
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);
}
}