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();
}
}
}
