예제 #1
0
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();
	}
}
예제 #2
0
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();
		}
	}
}