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