/**
* 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;
}
char followWaypoints(PathData* current, float* position, float heading, int* setpoint) {
static char recompute = 1;
static float radius = 5; // get from first waypoint?
static Vector current_position, target_position, next_position;
PathData* target = current;
PathData* next;
if (target->next) {
next = target->next;
} else {
next = &home;
}
getCoordinates(target->longitude, target->latitude, (float*)&target_position);
getCoordinates(next->longitude, next->latitude, (float*)&next_position);
static Vector current_heading, target_heading;
get_direction(&target_position, &next_position, &target_heading);
static Circle close, far;
static Line tangent, plane;
static DubinsPath progress = DUBINS_PATH_C1;
if (recompute) {
// printf("Recomputing path...\n");
current_position = (Vector) {
.x = position[0],
.y = position[1],
};
float angle = deg2rad(90 - heading);
current_heading = (Vector) {
.x = cos(angle),
.y = sin(angle),
};
float d = sqrt(pow(target_position.x - current_position.x, 2) + pow(target_position.y - current_position.y, 2));
if (d < 4*target->radius) {
return next->index;
}
plane = (Line) {
.initial = current_position,
.direction = current_heading,
};
if (belongs_to_half_plane(&plane, &next_position)) {
close.center = (Vector) {
.x = current_position.x + current->radius*current_heading.y,
.y = current_position.y - current->radius*current_heading.x,
};
far.center = (Vector) {
.x = target_position.x - target->radius*target_heading.y,
.y = target_position.y + target->radius*target_heading.x,
};
} else {
close.center = (Vector) {
.x = current_position.x - current->radius*current_heading.y,
.y = current_position.y + current->radius*current_heading.x,
};
far.center = (Vector) {
.x = target_position.x + target->radius*target_heading.y,
.y = target_position.y - target->radius*target_heading.x,
};
}
close.radius = target->radius;
far.radius = target->radius;
Line tangents[2];
get_tangents(&close, &far, tangents);
float d1 = sqrt(pow(tangents[0].initial.x - current_position.x, 2) + pow(tangents[0].initial.y - current_position.y, 2));
float d2 = sqrt(pow(tangents[1].initial.x - current_position.x, 2) + pow(tangents[1].initial.y - current_position.y, 2));
tangent = d1 < d2 ? tangents[0] : tangents[1];
progress = DUBINS_PATH_C1;
radius = target->radius;
recompute = 0;
}
plane = (Line) {
.initial = tangent.initial,
.direction = (Vector) {
.x = -tangent.direction.y,
.y = tangent.direction.x,
},
};
// before crossing first tangent point
if (belongs_to_half_plane(&plane, (Vector *)position)) {
char direction = current_heading.x*tangent.direction.y - current_heading.y*tangent.direction.x > 0 ? 1 : -1;
*setpoint = (int)followOrbit((float *)&close.center, close.radius, direction, position, heading);
} else {
plane.initial = (Vector) {
.x = tangent.initial.x + tangent.direction.x,
.y = tangent.initial.y + tangent.direction.y,
};
// before crossing second tangent point
if (belongs_to_half_plane(&plane, (Vector *)position)) {
*setpoint = (int)followStraightPath((float*)&tangent.direction, (float*)&plane.initial, position, heading);
} else {
plane = (Line) {
.initial = target_position,
.direction = (Vector) {
.x = -target_heading.y,
.y = target_heading.x,
},
};
// before crossing target waypoint
if (belongs_to_half_plane(&plane, (Vector *)position)) {
char direction = tangent.direction.x*target_heading.y - tangent.direction.y*target_heading.x > 0 ? 1 : -1;
*setpoint = (int)followOrbit((float *)&far.center, far.radius, direction, position, heading);
} else {
recompute = 1;
return next->index;
}
}
}
return current->index;
}
#else
char followWaypoints(PathData* currentWaypoint, float* position, float heading, int* sp_Heading){
float waypointPosition[3];
getCoordinates(currentWaypoint->longitude, currentWaypoint->latitude, (float*)&waypointPosition);
waypointPosition[2] = currentWaypoint->altitude;
if(currentWaypoint->next == NULL){
//Case for this being the last/only way point in the queue, avoid null pointers
*sp_Heading = followLastLineSegment(currentWaypoint, position, heading);
return currentWaypoint->index;
}
if(currentWaypoint->next->next == NULL){
//Case for only 2 way points remaining in queue
*sp_Heading = followLineSegment(currentWaypoint, position, heading);
return currentWaypoint->index;
}
PathData* targetWaypoint = currentWaypoint->next;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
PathData* nextWaypoint = targetWaypoint->next;
float nextCoordinates[3];
getCoordinates(nextWaypoint->longitude, nextWaypoint->latitude, (float*)&nextCoordinates);
nextCoordinates[2] = nextWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
float nextWaypointDirection[3];
float norm2 = sqrt(pow(nextCoordinates[0] - targetCoordinates[0],2) + pow(nextCoordinates[1] - targetCoordinates[1],2) + pow(nextCoordinates[2] - targetCoordinates[2],2));
nextWaypointDirection[0] = (nextCoordinates[0] - targetCoordinates[0])/norm2;
nextWaypointDirection[1] = (nextCoordinates[1] - targetCoordinates[1])/norm2;
nextWaypointDirection[2] = (nextCoordinates[2] - targetCoordinates[2])/norm2;
float turningAngle = acos(-deg2rad(waypointDirection[0] * nextWaypointDirection[0] + waypointDirection[1] * nextWaypointDirection[1] + waypointDirection[2] * nextWaypointDirection[2]));
float tangentFactor = targetWaypoint->radius/tan(turningAngle/2);
if (orbitPathStatus == PATH){
float halfPlane[3];
halfPlane[0] = targetCoordinates[0] - tangentFactor * waypointDirection[0];
halfPlane[1] = targetCoordinates[1] - tangentFactor * waypointDirection[1];
halfPlane[2] = targetCoordinates[2] - tangentFactor * waypointDirection[2];
float dotProduct = waypointDirection[0] * (position[0] - halfPlane[0]) + waypointDirection[1] * (position[1] - halfPlane[1]) + waypointDirection[2] * (position[2] - halfPlane[2]);
if (dotProduct > 0){
orbitPathStatus = ORBIT;
if (targetWaypoint->type == HOLD_WAYPOINT)
{
inHold = true;
}
}
*sp_Heading = (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
else{
float halfPlane[3];
halfPlane[0] = targetCoordinates[0] + tangentFactor * nextWaypointDirection[0];
halfPlane[1] = targetCoordinates[1] + tangentFactor * nextWaypointDirection[1];
halfPlane[2] = targetCoordinates[2] + tangentFactor * nextWaypointDirection[2];
char turnDirection = waypointDirection[0] * nextWaypointDirection[1] - waypointDirection[1] * nextWaypointDirection[0]>0?1:-1;
float euclideanWaypointDirection = sqrt(pow(nextWaypointDirection[0] - waypointDirection[0],2) + pow(nextWaypointDirection[1] - waypointDirection[1],2) + pow(nextWaypointDirection[2] - waypointDirection[2],2)) * ((nextWaypointDirection[0] - waypointDirection[0]) < 0?-1:1) * ((nextWaypointDirection[1] - waypointDirection[1]) < 0?-1:1) * ((nextWaypointDirection[2] - waypointDirection[2]) < 0?-1:1);
float turnCenter[3];
turnCenter[0] = targetCoordinates[0] + (tangentFactor * (nextWaypointDirection[0] - waypointDirection[0])/euclideanWaypointDirection);
turnCenter[1] = targetCoordinates[1] + (tangentFactor * (nextWaypointDirection[1] - waypointDirection[1])/euclideanWaypointDirection);
turnCenter[2] = targetCoordinates[2] + (tangentFactor * (nextWaypointDirection[2] - waypointDirection[2])/euclideanWaypointDirection);
// if target waypoint is a hold waypoint the plane will follow the orbit until the break hold method is called
if (inHold == true) {
*sp_Heading = (int)followOrbit((float*) &turnCenter, targetWaypoint->radius, turnDirection, (float*)position, heading);
return currentWaypoint->index;
}
float dotProduct = nextWaypointDirection[0] * (position[0] - halfPlane[0]) + nextWaypointDirection[1] * (position[1] - halfPlane[1]) + nextWaypointDirection[2] * (position[2] - halfPlane[2]);
if (dotProduct > 0){
orbitPathStatus = PATH;
return targetWaypoint->index;
}
//If two waypoints are parallel to each other (no turns)
if (euclideanWaypointDirection == 0){
orbitPathStatus = PATH;
return targetWaypoint->index;
}
*sp_Heading = (int)followOrbit((float*) &turnCenter,targetWaypoint->radius, turnDirection, (float*)position, heading);
}
return currentWaypoint->index;
}
#endif
int followLineSegment(PathData* currentWaypoint, float* position, float heading){
float waypointPosition[3];
getCoordinates(currentWaypoint->longitude, currentWaypoint->latitude, (float*)&waypointPosition);
waypointPosition[2] = currentWaypoint->altitude;
PathData* targetWaypoint = currentWaypoint->next;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
return (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
int followLastLineSegment(PathData* currentWaypoint, float* position, float heading){
float waypointPosition[3];
waypointPosition[0] = position[0];
waypointPosition[1] = position[1];
waypointPosition[2] = interchip_send_buffer.pm_data.altitude;
PathData* targetWaypoint = currentWaypoint;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
float dotProduct = waypointDirection[0] * (position[0] - targetCoordinates[0]) + waypointDirection[1] * (position[1] - targetCoordinates[1]) + waypointDirection[2] * (position[2] - targetCoordinates[2]);
if (dotProduct > 0){
returnHome = 1;
}
return (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
