/**
* @brief setup pathplanner/pathfollower for positionhold
*/
void plan_setup_positionHold()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float startingVelocity;
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = startingVelocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
GroundPathFollowerSettingsGet(&guidanceSettings);
// Configure the velocity control PID loops
pid_configure(&ground_pids[VELOCITY],
guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
// Configure the position control (velocity output) PID loops
pid_configure(&ground_pids[NORTH_POSITION],
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
pid_configure(&ground_pids[EAST_POSITION],
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
PathDesiredGet(&pathDesired);
}
void PathDesired_Set(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if (Param[0]->Val->Pointer == NULL)
return;
PathDesiredData data;
PathDesiredGet(&data);
// use the same struct like picoc. see below
struct mystruct {
double Start[3];
double End[3];
double StartingVelocity;
double EndingVelocity;
double ModeParameters;
unsigned char Mode;
} *pdata;
pdata = Param[0]->Val->Pointer;
data.Start[0] = pdata->Start[0];
data.Start[1] = pdata->Start[1];
data.Start[2] = pdata->Start[2];
data.End[0] = pdata->End[0];
data.End[1] = pdata->End[1];
data.End[2] = pdata->End[2];
data.StartingVelocity = pdata->StartingVelocity;
data.EndingVelocity = pdata->EndingVelocity;
data.ModeParameters = pdata->ModeParameters;
data.Mode = pdata->Mode;
PathDesiredSet(&data);
}
void plan_setup_land()
{
float descendspeed;
plan_setup_positionHold();
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
/**
* @brief setup pathplanner/pathfollower for return to base
*/
void plan_setup_returnToBase()
{
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
float positionStateDown;
PositionStateDownGet(&positionStateDown);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
TakeOffLocationData takeoffLocation;
TakeOffLocationGet(&takeoffLocation);
// TODO: right now VTOLPF does fly straight to destination altitude.
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
float destDown;
FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East;
pathDesired.End.Down = destDown;
pathDesired.Start.North = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = destDown;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
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_setup_assistedcontrol(uint8_t timeout_occurred)
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
float brakeRate;
VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
// Calculate the velocity
float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
velocity = sqrtf(velocity);
// Calculate the desired time to zero velocity.
float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
time_to_stopped += velocity / brakeRate;
// Sanity check the brake rate by ensuring that the time to stop is within a range.
if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
} else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
}
// calculate the distance we will travel
float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
net_delta = sqrtf(net_delta);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down;
pathDesired.End.North = positionState.North + north_delta;
pathDesired.End.East = positionState.East + east_delta;
pathDesired.End.Down = positionState.Down + down_delta;
pathDesired.StartingVelocity = velocity;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
}
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
PathDesiredSet(&pathDesired);
}
/**
* @brief execute autocruise
*/
void plan_run_AutoCruise()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float controlVector[4];
ManualControlCommandRollGet(&controlVector[0]);
ManualControlCommandPitchGet(&controlVector[1]);
ManualControlCommandYawGet(&controlVector[2]);
controlVector[3] = 0.5f; // dummy, thrust is normalized separately
normalizeDeadband(controlVector); // return value ignored
ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
// normalize old desired movement vector
float vector[3] = { pathDesired.End.North - hold_position[0],
pathDesired.End.East - hold_position[1],
pathDesired.End.Down - hold_position[2] };
float length = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
if (length < 1e-9f) {
length = 1.0f; // should not happen since initialized properly in setup()
}
vector[0] /= length;
vector[1] /= length;
vector[2] /= length;
// start position is advanced according to actual movement - in the direction of desired vector only
// projection using scalar product
float kp = (positionState.North - hold_position[0]) * vector[0]
+ (positionState.East - hold_position[1]) * vector[1]
+ (positionState.Down - hold_position[2]) * vector[2];
if (kp > 0.0f) {
hold_position[0] += kp * vector[0];
hold_position[1] += kp * vector[1];
hold_position[2] += kp * vector[2];
}
// new angle is equal to old angle plus offset depending on yaw input and time
// (controlVector is normalized with a deadband, change is zero within deadband)
float angle = RAD2DEG(atan2f(vector[1], vector[0]));
float dT = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
angle += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
// resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
pathDesired.End.North = hold_position[0] + vector[0];
pathDesired.End.East = hold_position[1] + vector[1];
pathDesired.End.Down = hold_position[2] + vector[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;
PathDesiredSet(&pathDesired);
}
static void plan_run_PositionVario(vario_type type)
{
float controlVector[4];
float alpha;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
ManualControlCommandRollGet(&controlVector[0]);
ManualControlCommandPitchGet(&controlVector[1]);
ManualControlCommandYawGet(&controlVector[2]);
ManualControlCommandThrustGet(&controlVector[3]);
FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
controlVector[0] = vario_control_lowpass[0];
controlVector[1] = vario_control_lowpass[1];
controlVector[2] = vario_control_lowpass[2];
// check if movement is desired
if (normalizeDeadband(controlVector) == false) {
// no movement desired, re-enter positionHold at current start-position
if (!vario_hold) {
vario_hold = true;
// new hold position is the position that was previously the start position
pathDesired.End.North = hold_position[0];
pathDesired.End.East = hold_position[1];
pathDesired.End.Down = hold_position[2];
// while the new 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;
PathDesiredSet(&pathDesired);
}
} else {
PositionStateData positionState;
PositionStateGet(&positionState);
// flip pitch to have pitch down (away) point north
controlVector[1] = -controlVector[1];
getVector(controlVector, type);
// layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4}
if (vario_hold) {
// start position is the position that was previously the hold position
vario_hold = false;
hold_position[0] = pathDesired.End.North;
hold_position[1] = pathDesired.End.East;
hold_position[2] = pathDesired.End.Down;
} else {
// start position is advanced according to movement - in the direction of ControlVector only
// projection using scalar product
float kp = (positionState.North - hold_position[0]) * controlVector[0]
+ (positionState.East - hold_position[1]) * controlVector[1]
+ (positionState.Down - hold_position[2]) * -controlVector[2];
if (kp > 0.0f) {
hold_position[0] += kp * controlVector[0];
hold_position[1] += kp * controlVector[1];
hold_position[2] += kp * -controlVector[2];
}
}
// new destination position is advanced based on controlVector
pathDesired.End.North = hold_position[0] + controlVector[0] * controlVector[3];
pathDesired.End.East = hold_position[1] + controlVector[1] * controlVector[3];
pathDesired.End.Down = hold_position[2] - controlVector[2] * controlVector[3];
// the new 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;
PathDesiredSet(&pathDesired);
}
}
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod));
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
for (uint32_t i = 0; i < VTOL_PID_NUM; i++)
pid_zero(&vtol_pids[i]);
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
FixedWingAirspeedsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
// Force update of all the settings
SettingsUpdatedCb(NULL);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
path_desired_updated = false;
PathDesiredGet(&pathDesired);
PathDesiredConnectCallback(pathDesiredUpdated);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod);
static uint8_t last_flight_mode;
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PositionActualData positionActual;
static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE;
// Check whether an update to the path desired occured and we should
// process it. This makes sure that the follower alarm state is
// updated.
bool process_path_desired_update =
(last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ||
last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
path_desired_updated;
path_desired_updated = false;
// Process most of these when the flight mode changes
// except when in path following mode in which case
// each iteration must make sure this has the latest
// PathDesired
if (flightStatus.FlightMode != last_flight_mode ||
process_path_desired_update) {
last_flight_mode = flightStatus.FlightMode;
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = 0;
pathDesired.End[1] = 0;
pathDesired.End[2] = -30.0f;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = positionActual.North;
pathDesired.End[1] = positionActual.East;
pathDesired.End[2] = positionActual.Down;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
state = FW_FOLLOWER_RUNNING;
PathDesiredGet(&pathDesired);
switch(pathDesired.Mode) {
case PATHDESIRED_MODE_ENDPOINT:
case PATHDESIRED_MODE_VECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
break;
default:
state = FW_FOLLOWER_ERR;
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
PathStatusSet(&pathStatus);
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
break;
}
break;
default:
state = FW_FOLLOWER_IDLE;
break;
}
}
switch(state) {
case FW_FOLLOWER_RUNNING:
{
updatePathVelocity();
uint8_t result = updateFixedDesiredAttitude();
if (result) {
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(&pathStatus);
break;
}
case FW_FOLLOWER_IDLE:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
bearingIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
airspeedErrorInt = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
break;
case FW_FOLLOWER_ERR:
default:
// Leave alarms set above
break;
}
}
}
/**
* Module thread, should not return.
*/
static void groundPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
GroundPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have GROUND type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, guidanceSettings.UpdatePeriod);
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateGroundDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateGroundDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
northPosIntegral = 0;
eastPosIntegral = 0;
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
uint8_t waypointFollowing(uint8_t flightMode, FixedWingPathFollowerSettingsCCData fixedwingpathfollowerSettings)
{
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s]
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
float trueAirspeed;
float calibratedAirspeedActual;
float airspeedDesired;
float airspeedError;
float pitchCommand;
float powerCommand;
float headingError_R;
float rollCommand;
//TODO: Move these out of the loop
FixedWingAirspeedsData fixedwingAirspeeds;
FixedWingAirspeedsGet(&fixedwingAirspeeds);
VelocityActualGet(&velocityActual);
StabilizationDesiredGet(&stabDesired);
// TODO: Create UAVO that merges airspeed together
AirspeedActualTrueAirspeedGet(&trueAirspeed);
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if (flightStatusUpdate) {
//Reset integrals
integral->totalEnergyError = 0;
integral->airspeedError = 0;
integral->lineError = 0;
integral->circleError = 0;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME) {
// Simple Return To Home mode: climb 10 meters and fly to home position
pathDesired.Start[PATHDESIRED_START_NORTH] =
positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] =
positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] =
positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] =
positionActual.Down - 10;
pathDesired.StartingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.EndingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
homeOrbit = false;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) {
// Simple position hold: stay at present altitude and position
//Offset by one so that the start and end points don't perfectly coincide
pathDesired.Start[PATHDESIRED_START_NORTH] =
positionActual.North - 1;
pathDesired.Start[PATHDESIRED_START_EAST] =
positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] =
positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] =
positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] =
positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] =
positionActual.Down;
pathDesired.StartingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.EndingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
}
PathDesiredSet(&pathDesired);
flightStatusUpdate = false;
}
/**
* Compute speed error (required for throttle and pitch)
*/
// Current airspeed
calibratedAirspeedActual = trueAirspeed; //BOOOOOOOOOO!!! Where's the conversion from TAS to CAS?
// Current heading
float headingActual_R =
atan2f(velocityActual.East, velocityActual.North);
// Desired airspeed
airspeedDesired = pathDesired.EndingVelocity;
// Airspeed error
airspeedError = airspeedDesired - calibratedAirspeedActual;
/**
* Compute desired throttle command
*/
//Proxy because instead of m*(1/2*v^2+g*h), it's v^2+2*gh. This saves processing power
float totalEnergyProxySetpoint = powf(pathDesired.EndingVelocity,
2.0f) -
2.0f * GRAVITY * pathDesired.End[2];
float totalEnergyProxyActual =
powf(trueAirspeed, 2.0f) - 2.0f * GRAVITY * positionActual.Down;
float errorTotalEnergy =
totalEnergyProxySetpoint - totalEnergyProxyActual;
float throttle_kp =
fixedwingpathfollowerSettings.
ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KP];
float throttle_ki = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KI];
float throttle_ilimit = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_ILIMIT];
//Integrate with bound. Make integral leaky for better performance. Approximately 30s time constant.
if (throttle_ilimit > 0.0f) {
integral->totalEnergyError =
bound(integral->totalEnergyError + errorTotalEnergy * dT,
-throttle_ilimit / throttle_ki,
throttle_ilimit / throttle_ki) * (1.0f -
1.0f / (1.0f +
30.0f /
dT));
}
powerCommand = errorTotalEnergy * throttle_kp
+ integral->totalEnergyError * throttle_ki;
float throttlelimit_neutral =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_NEUTRAL];
float throttlelimit_min =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MIN];
float throttlelimit_max =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MAX];
// set throttle
stabDesired.Throttle = bound(powerCommand + throttlelimit_neutral,
throttlelimit_min, throttlelimit_max);
/**
* Compute desired pitch command
*/
float airspeed_kp =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KP];
float airspeed_ki =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KI];
float airspeed_ilimit =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_ILIMIT];
if (airspeed_ki > 0.0f) {
//Integrate with saturation
integral->airspeedError =
bound(integral->airspeedError + airspeedError * dT,
-airspeed_ilimit / airspeed_ki,
airspeed_ilimit / airspeed_ki);
}
//Compute the cross feed from altitude to pitch, with saturation
float pitchcrossfeed_kp =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_KP];
float pitchcrossfeed_min =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX];
float pitchcrossfeed_max =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX];
float alitudeError = -(pathDesired.End[2] - positionActual.Down); //Negative to convert from Down to altitude
float altitudeToPitchCommandComponent =
bound(alitudeError * pitchcrossfeed_kp,
-pitchcrossfeed_min,
pitchcrossfeed_max);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * airspeed_kp
+ integral->airspeedError * airspeed_ki) +
altitudeToPitchCommandComponent;
//Saturate pitch command
float pitchlimit_neutral =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_NEUTRAL];
float pitchlimit_min =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MIN];
float pitchlimit_max =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MAX];
stabDesired.Pitch = bound(pitchlimit_neutral +
pitchCommand, pitchlimit_min, pitchlimit_max);
/**
* Compute desired roll command
*/
float p[3] =
{ positionActual.North, positionActual.East, positionActual.Down };
float *c = pathDesired.End;
float *r = pathDesired.Start;
float q[3] = { pathDesired.End[0] - pathDesired.Start[0],
pathDesired.End[1] - pathDesired.Start[1],
pathDesired.End[2] - pathDesired.Start[2]
};
float k_path = fixedwingpathfollowerSettings.VectorFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed
float k_orbit = fixedwingpathfollowerSettings.OrbitFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed
float k_psi_int = fixedwingpathfollowerSettings.FollowerIntegralGain;
//========================================
//SHOULD NOT BE HARD CODED
bool direction;
float chi_inf = PI / 4.0f; //THIS NEEDS TO BE A FUNCTION OF HOW LONG OUR PATH IS.
//Saturate chi_inf. I.e., never approach the path at a steeper angle than 45 degrees
chi_inf = chi_inf < PI / 4.0f ? PI / 4.0f : chi_inf;
//========================================
float rho;
float headingCommand_R;
float pncn = p[0] - c[0];
float pece = p[1] - c[1];
float d = sqrtf(pncn * pncn + pece * pece);
//Assume that we want a 15 degree bank angle. This should yield a nice, non-agressive turn
#define ROLL_FOR_HOLDING_CIRCLE 15.0f
//Calculate radius, rho, using r*omega=v and omega = g/V_g * tan(phi)
//THIS SHOULD ONLY BE CALCULATED ONCE, INSTEAD OF EVERY TIME
rho = powf(pathDesired.EndingVelocity,
2) / (GRAVITY * tanf(fabs(ROLL_FOR_HOLDING_CIRCLE * DEG2RAD)));
typedef enum {
LINE,
ORBIT
} pathTypes_t;
pathTypes_t pathType;
//Determine if we should fly on a line or orbit path.
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (d < rho + 5.0f * pathDesired.EndingVelocity || homeOrbit == true) { //When approx five seconds from the circle, start integrating into it
pathType = ORBIT;
homeOrbit = true;
} else {
pathType = LINE;
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
pathType = ORBIT;
break;
default:
pathType = LINE;
break;
}
//Check to see if we've gone past our destination. Since the path follower
//is simply following a vector, it has no concept of where the vector ends.
//It will simply keep following it to infinity if we don't stop it. So while
//we don't know why the commutation to the next point failed, we don't know
//we don't want the plane flying off.
if (pathType == LINE) {
//Compute the norm squared of the horizontal path length
//IT WOULD BE NICE TO ONLY DO THIS ONCE PER WAYPOINT UPDATE, INSTEAD OF
//EVERY LOOP
float pathLength2 = q[0] * q[0] + q[1] * q[1];
//Perform a quick vector math operation, |a| < a.b/|a| = |b|cos(theta),
//to test if we've gone past the waypoint. Add in a distance equal to 5s
//of flight time, for good measure to make sure we don't add jitter.
if (((p[0] - r[0]) * q[0] + (p[1] - r[1]) * q[1]) >
pathLength2 + 5.0f * pathDesired.EndingVelocity) {
//Whoops, we've really overflown our destination point, and haven't
//received any instructions. Start circling
//flightMode will reset the next time a waypoint changes, so there's
//no danger of it getting stuck in orbit mode.
flightMode = FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD;
pathType = ORBIT;
}
}
switch (pathType) {
case ORBIT:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT) {
direction = false;
} else {
//In the case where the direction is undefined, always fly in a
//clockwise fashion
direction = true;
}
headingCommand_R =
followOrbit(c, rho, direction, p, headingActual_R, k_orbit,
k_psi_int, dT);
break;
case LINE:
headingCommand_R =
followStraightLine(r, q, p, headingActual_R, chi_inf,
k_path, k_psi_int, dT);
break;
}
//Calculate heading error
headingError_R = headingCommand_R - headingActual_R;
//Wrap heading error around circle
if (headingError_R < -PI)
headingError_R += 2.0f * PI;
if (headingError_R > PI)
headingError_R -= 2.0f * PI;
//GET RID OF THE RAD2DEG. IT CAN BE FACTORED INTO HeadingPI
float rolllimit_neutral =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_NEUTRAL];
float rolllimit_min =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MIN];
float rolllimit_max =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MAX];
float headingpi_kp =
fixedwingpathfollowerSettings.
HeadingPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_HEADINGPI_KP];
rollCommand = (headingError_R * headingpi_kp) * RAD2DEG;
//Turn heading
stabDesired.Roll = bound(rolllimit_neutral +
rollCommand, rolllimit_min, rolllimit_max);
#ifdef SIM_OSX
fprintf(stderr, " headingError_R: %f, rollCommand: %f\n",
headingError_R, rollCommand);
#endif
/**
* Compute desired yaw command
*/
// Coordinated flight, so we reset the desired yaw.
stabDesired.Yaw = 0;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] =
STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] =
STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] =
STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired);
//Stuff some debug variables into PathDesired, because right now these
//fields aren't being used.
pathDesired.ModeParameters[0] = pitchCommand;
pathDesired.ModeParameters[1] = airspeedError;
pathDesired.ModeParameters[2] = integral->airspeedError;
pathDesired.ModeParameters[3] = alitudeError;
pathDesired.UID = errorTotalEnergy;
PathDesiredSet(&pathDesired);
return 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);
}
}
