/** * 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); }