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();
}
}
}
// In the future, we could include more than 2 flight plans...
// flightplanNum is 0 for the main lgo instructions, and 1 for RTL instructions
//void init_flightplan(int16_t flightplanNum)
void flightplan_logo_begin(int16_t flightplanNum)
{
struct relative2D curHeading;
struct relative3D IMUloc;
int8_t earth_yaw;
int16_t angle;
if (flightplanNum == 1) // RTL instructions set
{
currentInstructionSet = (struct logoInstructionDef*)rtlInstructions;
numInstructionsInCurrentSet = NUM_RTL_INSTRUCTIONS;
}
else if (flightplanNum == 0) // Main instructions set
{
currentInstructionSet = (struct logoInstructionDef*)instructions;
numInstructionsInCurrentSet = NUM_INSTRUCTIONS;
}
instructionIndex = 0;
logoStackIndex = 0;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE;
logoStack[logoStackIndex].arg = 0;
logoStack[logoStackIndex].returnInstructionIndex = -1; // When starting over, begin on instruction 0
currentTurtle = PLANE;
penState = 0; // 0 means down. more than 0 means up
turtleLocations[PLANE].x._.W1 = IMUlocationx._.W1;
turtleLocations[PLANE].y._.W1 = IMUlocationy._.W1;
turtleLocations[PLANE].z = IMUlocationz._.W1;
turtleLocations[CAMERA].x._.W1 = IMUlocationx._.W1;
turtleLocations[CAMERA].y._.W1 = IMUlocationy._.W1;
turtleLocations[CAMERA].z = IMUlocationz._.W1;
// Calculate heading from Direction Cosine Matrix (rather than GPS),
// So that this code works when the plane is static. e.g. at takeoff
curHeading.x = -rmat[1];
curHeading.y = rmat[4];
earth_yaw = rect_to_polar(&curHeading); // (0=East, ccw)
angle = (earth_yaw * 180 + 64) >> 7; // (ccw, 0=East)
angle = -angle + 90; // (clockwise, 0=North)
turtleAngles[PLANE] = turtleAngles[CAMERA] = angle;
setBehavior(0);
IMUloc.x = IMUlocationx._.W1;
IMUloc.y = IMUlocationy._.W1;
IMUloc.z = IMUlocationz._.W1;
update_goal_from(IMUloc);
interruptIndex = 0;
interruptStackBase = 0;
process_instructions();
}
