static void process_instructions(void)
{
instructionsProcessed = 0;
while (1)
{
boolean do_fly = process_one_instruction(currentInstructionSet[instructionIndex]);
instructionsProcessed++;
instructionIndex++;
if (instructionIndex >= numInstructionsInCurrentSet) instructionIndex = 0;
if (do_fly && penState == 0 && currentTurtle == PLANE)
break;
if (instructionsProcessed >= MAX_INSTRUCTIONS_PER_CYCLE)
return; // don't update goal if we didn't hit a FLY command
}
waypointIndex = instructionIndex - 1;
if (logo_goal_has_moved())
{
update_goal_from(lastGoal);
navigate_compute_bearing_to_goal();
}
}
void flightplan_logo_update(void)
{
// first run any injected instruction from the serial port
if (logo_inject_pos == LOGO_INJECT_READY)
{
process_one_instruction(logo_inject_instr);
if (logo_inject_instr.cmd == 2 || logo_inject_instr.cmd == 10) // DO / EXEC
{
instructionIndex++;
process_instructions();
}
else
{
if (logo_goal_has_moved())
{
update_goal_from(lastGoal);
navigate_compute_bearing_to_goal();
}
}
logo_inject_pos = 0;
return;
}
// otherwise run the interrupt handler, if configured, and not in-progress
if (interruptIndex && !interruptStackBase)
{
if (logoStackIndex < LOGO_STACK_DEPTH-1)
{
logoStackIndex++;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE;
logoStack[logoStackIndex].arg = 0;
logoStack[logoStackIndex].returnInstructionIndex = instructionIndex-1;
instructionIndex = interruptIndex+1;
interruptStackBase = logoStackIndex;
process_instructions();
navigate_set_goal_height(turtleLocations[PLANE].z);
lastGoal.z = turtleLocations[PLANE].z;
}
}
// waypoint arrival is detected computing distance to the "finish line".
// note: locations are measured in meters
// locations have a range of +-32000 meters (20 miles) from origin
if (desired_behavior._.altitude)
{
if (abs(IMUheight - navigate_get_goal(NULL)) < ((int16_t)altit.HeightMargin)) // reached altitude goal
{
desired_behavior._.altitude = 0;
process_instructions();
}
}
else
{
if (tofinish_line < WAYPOINT_PROXIMITY_RADIUS) // crossed the finish line
{
process_instructions();
}
}
}
static void next_waypoint(void)
{
if (extended_range == 0)
{
#if (USE_MAVLINK == 1)
mavlink_waypoint_reached(waypointIndex);
#endif
waypointIndex++;
if (waypointIndex >= numPointsInCurrentSet) waypointIndex = 0;
DPRINT("next_waypoint(%u)\r\n", waypointIndex);
set_waypoint(waypointIndex);
/*
#if (USE_MAVLINK == 1)
mavlink_waypoint_changed(waypointIndex);
#endif
if (waypointIndex == 0)
{
if (numPointsInCurrentSet > 1)
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[numPointsInCurrentSet-1]);
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
else
{
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
setBehavior(currentWaypointSet[0].flags);
}
else
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[waypointIndex-1]);
current_waypoint = wp_to_relative(currentWaypointSet[waypointIndex]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
}
*/
}
else
{
navigate_set_goal(GPSlocation, current_waypoint.loc);
#if (DEADRECKONING == 0)
navigate_compute_bearing_to_goal();
#endif
}
}
//void run_flightplan(void)
void flightplan_waypoints_update(void)
{
// first run any injected wp from the serial port
if (wp_inject_pos == WP_INJECT_READY)
{
current_waypoint = wp_to_relative(wp_inject);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
navigate_compute_bearing_to_goal();
wp_inject_pos = 0;
return;
}
// steering is based on cross track error.
// waypoint arrival is detected computing distance to the "finish line".
// note: locations are measured in meters
// velocities are in centimeters per second
// locations have a range of +-32000 meters (20 miles) from origin
if (desired_behavior._.altitude)
{
if (abs(IMUheight - navigate_get_goal(NULL)) < ((int16_t)altit.HeightMargin))
{
next_waypoint();
}
}
else
{
if (tofinish_line < WAYPOINT_PROXIMITY_RADIUS) // crossed the finish line
{
if (desired_behavior._.loiter)
{
navigate_set_goal(GPSlocation, wp_to_relative(currentWaypointSet[waypointIndex]).loc);
}
else
{
next_waypoint();
}
}
}
}
void set_waypoint(int16_t index)
{
DPRINT("set_waypoint(%u)\r\n", index);
if (index < numPointsInCurrentSet)
{
waypointIndex = index;
#if (USE_MAVLINK == 1)
mavlink_waypoint_changed(waypointIndex);
#endif
if (waypointIndex == 0)
{
if (numPointsInCurrentSet > 1)
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[numPointsInCurrentSet-1]);
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
else
{
current_waypoint = wp_to_relative(currentWaypointSet[0]);
navigate_set_goal(GPSlocation, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
}
setBehavior(currentWaypointSet[0].flags);
}
else
{
struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[waypointIndex-1]);
current_waypoint = wp_to_relative(currentWaypointSet[waypointIndex]);
navigate_set_goal(previous_waypoint.loc, current_waypoint.loc);
set_camera_view(current_waypoint.viewpoint);
setBehavior(current_waypoint.flags);
}
#if (DEADRECKONING == 0)
navigate_compute_bearing_to_goal();
#endif
}
}