void GPSListener::triggerGpsFailure() { // arret du watchdog timerGpsCommunicationFailure->stop(); if ( initOK == 1 ) { std::cout << "\nInit etait Ok, nettoyage complet\n"; cleanCloseEverything(); // Init etait OK, } else // Init pas OK, juste le watchdog a nettoyer, le reste n'a pas ete cree { std::cout << "\nDefaillance Init, nettoyage watchdog\n"; close(gpsDev); disconnect(timerGpsCommunicationFailure); free(timerGpsCommunicationFailure); } std::cout << "TIMER TRIGGER, GPS FAILURE !, Retrying in " << FIRST_RETRY_DELAY << " seconds...\n"; sleep(FIRST_RETRY_DELAY); // Tous les Timers sont arretes grace aux lignes ci-dessus, on met ce que l'on veut en delai std::cout << "Reinit " << gps_device << "\n"; int status = initGPS(gps_device); while ( status < 0 ) { std::cout << "(loop) TIMER TRIGGER, GPS FAILURE !, Retrying in " << RETRY_DELAY << " seconds...\n"; sleep(RETRY_DELAY); std::cout << "Reinit " << gps_device << "\n"; status = initGPS(gps_device); } //exit(-1); }
GPSListener::GPSListener(const char *device) { // !!! Reglages ttyUSBx !!! // stty -F /dev/ttyUSBx speed 4800 cs8 -cstopb -parenb // verifier avec cat < /dev/ttyUSBx int initStatus = initGPS(device); while (initStatus == -1 ) { std::cout << "INIT GPS DEVICE FAILED, RETRYING IN " << FIRST_RETRY_DELAY << " SEC.\n"; sleep(FIRST_RETRY_DELAY); initStatus = initGPS(device); } }
void initSensors() { initBarometer(); initSonar(); initVoltage(); initGPS(); }
void systemInit(void) { // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); /////////////////////////////////// checkFirstTime(false); readEEPROM(); if (eepromConfig.receiverType == SPEKTRUM) checkSpektrumBind(); checkResetType(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority initMixer(); ledInit(); cliInit(); BLUE_LED_ON; delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough..... adcInit(); batteryInit(); gpsInit(); i2cInit(I2C1); i2cInit(I2C2); pwmEscInit(eepromConfig.escPwmRate); pwmServoInit(eepromConfig.servoPwmRate); rxInit(); spiInit(SPI2); spiInit(SPI3); telemetryInit(); timingFunctionsInit(); initFirstOrderFilter(); initGPS(); initMax7456(); initPID(); GREEN_LED_ON; initMPU6000(); initMag(HMC5883L_I2C); initPressure(MS5611_I2C); }
void initTruck(){ //Setup Debugging Connection initUART1(); //Check for any errors checkErrorCodes(); //Setup GPS initGPS(); initTimer4(); //Setup Input and Output initPWM(0b11,0b11); PWMOutputCalibration(1,HUNTER_TRUCK_STEERING_SCALE_FACTOR, HUNTER_TRUCK_STEERING_OFFSET); PWMOutputCalibration(2,HUNTER_TRUCK_THROTTLE_SCALE_FACTOR, HUNTER_TRUCK_THROTTLE_OFFSET); setThrottle(0); setSteering(0); //Setup Datalink // initDataLink(); }
int main(){ int i = 0; int mapCount = 0, clearMapCount = 0, dumpCount=0; int revFrameCount = 0; #ifdef USE_NORTH targetsGPS[maxTargets].lat = ADVANCED5LAT; targetsGPS[maxTargets].lon = ADVANCED5LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED6LAT; targetsGPS[maxTargets].lon = ADVANCED6LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED7LAT; targetsGPS[maxTargets].lon = ADVANCED7LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED8LAT; targetsGPS[maxTargets].lon = ADVANCED8LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED2LAT; targetsGPS[maxTargets].lon = ADVANCED2LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED1LAT; targetsGPS[maxTargets].lon = ADVANCED1LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED3LAT; targetsGPS[maxTargets].lon = ADVANCED3LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED12LAT; targetsGPS[maxTargets].lon = ADVANCED12LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED4LAT; targetsGPS[maxTargets].lon = ADVANCED4LON; maxTargets++; #else targetsGPS[maxTargets].lat = ADVANCED4LAT; targetsGPS[maxTargets].lon = ADVANCED4LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED1LAT; targetsGPS[maxTargets].lon = ADVANCED1LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED2LAT; targetsGPS[maxTargets].lon = ADVANCED2LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED3LAT; targetsGPS[maxTargets].lon = ADVANCED3LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED11LAT; targetsGPS[maxTargets].lon = ADVANCED11LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED8LAT; targetsGPS[maxTargets].lon = ADVANCED8LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED7LAT; targetsGPS[maxTargets].lon = ADVANCED7LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED6LAT; targetsGPS[maxTargets].lon = ADVANCED6LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED11LAT; targetsGPS[maxTargets].lon = ADVANCED11LON; maxTargets++; targetsGPS[maxTargets].lat = ADVANCED5LAT; targetsGPS[maxTargets].lon = ADVANCED5LON; maxTargets++; #endif maxTargetIndex=maxTargets-1; for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data. targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude); targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude); } currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude targetXY = targetListXY[currentTargetIndex];//sets first target GPS point nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point nextXY = targetListXY[nextTargetIndex];// ?? previousXY.x = GPSX(startLongitude, startLongitude);// why? previousXY.y = GPSY(startLatitude, startLatitude);//Why? initRoboteq(); /* Initialize roboteq */ initGuide();//what is guide? #ifdef USE_VISION // if USE_vision is defined, then initialize vision. initVision(); #endif //USE_VISION #ifdef USE_GPS// if USE_GPS is defined, then initialize GPS. initGPS(); initParser(); #endif //USE_GPS #ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR. initObjects(); initSICK(); #endif //USE_LIDAR #ifdef DEBUG_VISUALIZER// if defined, then use visualizer. initVisualizer(); #endif //DEBUG_VISUALIZER #ifdef USE_MAP//////>>>>>>>>>>>???? initMap(0,0,0); #endif //USE_MAP #ifdef DUMP_GPS// dump GPS data into file FILE *fp; fp = fopen("gpsdump.txt", "w"); #endif // DUMP_GPS while(1){ double dir = 1.0; double speed = 0.0, turn = 0.0; static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0 static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables if (joystick() != 0) {// is joystick is connected if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do????????????????????????????????? speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward turn = joy0.axis[0]; lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed }else{ //stop the robot rSpeed=lSpeed=0; } if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ??????????????????? saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h }else{ if (saveImage) saveImage--; // turn off if button wasn't pressed? } if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed autoOn = 1; mode=1; } if(joy0.buttons & Y_BTN){ // turn off autonomous mode autoOn = 0; mode =0; } lastButtons = joy0.buttons;//is this just updating buttons? } else{ // printf("No Joystick Found!\n"); rSpeed=lSpeed=0; } // // printf("3: %f %f\n",BASIC3LAT,BASIC3LON); // printf("4: %f %f\n",BASIC4LAT,BASIC4LON); // printf("5: %f %f\n",BASIC5LAT,BASIC5LON); // getchar(); #ifdef AUTO_SWAP//what is this if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){ startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC); targetIndexMem = currentTargetIndex; }else{ currentTime=(float)(clock()/CLOCKS_PER_SEC); } totalTime = currentTime-startTime; if(totalTime>=SWAPTIME&&autoOn){ swap(); targetIndexMem = 0; } #endif //AUTO_SWAP #ifdef USE_GPS readGPS(); currentXY.x = GPSX(gpsvar.longitude,startLongitude); currentXY.y = GPSY(gpsvar.latitude,startLatitude); robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course)); #else currentXY.x = 0.0; currentXY.y = 0.0; robotTheta = 0.0; #endif //USE_GPS if(autoOn&&!flagPointSet){//this whole thing????? flagXY.x=currentXY.x+FLAG_X_ADJUST; flagXY.y=currentXY.y; flagPointSet=1; startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC); } if(autoOn){ currentAutoTime=(float)(clock()/CLOCKS_PER_SEC); totalAutoTime = currentAutoTime-startAutoTime; if(totalAutoTime>=MODE2DELAY){ mode1TimeUp=1;//what is mode1 time up? } printf("TIMEING\n"); } // if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){ if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values? //OPEN_FIELD_INDEX is set to 0 above...? approachingThresh=4.0; destinationThresh=3.0; }else{//otherwise set your thresholds to a bit closer. // destinationThresh=1.0; destinationThresh=0.75; approachingThresh=2.5; } //mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus. //mode3= gps mode in open field, but vision is toned down to not get distracted by random grass. //mode 4= flag tracking if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached printf("REACHED TARGET\n"); initGuide();// reset PID control stuff. problably resets all control variables. previousXY = targetXY;//update last target if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets. allTargetsReached = 1; }else{//otherwise update all the target information currentTargetIndex = (currentTargetIndex + 1); nextTargetIndex = (currentTargetIndex + 1)% maxTargets; targetXY = targetListXY[currentTargetIndex]; nextXY = targetListXY[nextTargetIndex]; } } if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){ //if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target. mode =1;//wtf is mode distanceMultiplier = 50;//wthis is how heavily to rely on vision } else if((autoOn&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){ mode =2; distanceMultiplier = 50; } else if((autoOn&¤tTargetIndex!=0)){ mode =3; distanceMultiplier = 12; } flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point? if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){ mode =4;// what is mode } #ifdef FLAG_TESTING /*FLAG TESTING*/ mode=4; #endif //FLAG_TESTING /*Current Target Heading PID Control Adjustment*/ cvar.lookAhead = 0.00;//? cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15; turn = cvar.turn; int bestVisGpsMask = 99; int h = 0; double minVisGpsTurn = 9999; for(h=0;h<11;h++){ if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){ minVisGpsTurn=fabs((cvar.turn-turn_angle[h])); bestVisGpsMask = h; } } bestGpsMask = bestVisGpsMask; // printf("bvg: %d \n", bestVisGpsMask); // printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn); #ifdef USE_VISION // double visTurnBoost = 0.50; double visTurnBoost = 1.0; if(imageProc(mode) == -1) break; if(mode==1||mode==2){ turn = turn_angle[bestmask]; turn *= visTurnBoost; }else if(mode==3 && fabs(turn_angle[bestmask])>0.70){ turn = turn_angle[bestmask]; turn *= visTurnBoost; } #endif //USE_VISION #ifdef USE_LIDAR updateSick(); // findObjects(); #endif //USE_LIDAR #ifdef USE_COMBINED_BUFFER//?????????? #define WORSTTHRESH 10 #define BESTTHRESH 3 if(mode==4){ #ifdef USE_NORTH turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]); #else turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]); #endif turn *= 0.75; } combinedTargDist = cvar.targdist; if(((approachingTarget||inLastTarget)&¤tTargetIndex>OPEN_FIELD_INDEX &¤tTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){ getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data }else{ getCombinedBufferAngles(0,1);//Use Vision Radar Data } if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){ if(mode == 1 || mode==2 || mode==3 || mode==4){ // if(mode == 1 || mode==2 || mode==3){ // if(mode==2 || mode==3){ // if(mode==3){ if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){ double revTurn; double revDistLeft, revDistRight; int revIdx; if(fabs(turn)<0.10) dir = -1.0; if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0; if(dir<0){ revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4; revIdx = MIN(revIdx,1080); revIdx = MAX(revIdx,0); revDistLeft = LMSdata[revIdx]; revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4; revIdx = MIN(revIdx,1080); revIdx = MAX(revIdx,0); revDistRight = LMSdata[revIdx]; if(revDistLeft>=revDistRight){ revTurn = combinedBufferAngles.left; }else { revTurn = combinedBufferAngles.right; } turn = revTurn; }else{ turn = turn_angle[bestmask]; } } else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){ // } else if(turn<=0){ turn = combinedBufferAngles.right; }else { turn = combinedBufferAngles.left; } } } #endif //USE_COMBINED_BUFFER if(dir<0||revFrameCount!=0){ dir = -1.0; revFrameCount = (revFrameCount+1)%REVFRAMES; } // turn *= dir; turn = SIGN(turn) * MIN(fabs(turn), 1.0); speed = 1.0/(1.0+1.0*fabs(turn))*dir; speed = SIGN(speed) * MIN(fabs(speed), 1.0); if(!autoOn){ maxSpeed = 60; targetIndexMem = 0; }else if(dir<0){ maxSpeed = 30; }else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){ maxSpeed = 60 - 25*fabs(turn); // maxSpeed = 70 - 35*fabs(turn); // maxSpeed = 90 - 50*fabs(turn); // maxSpeed = 100 - 65*fabs(turn); }else if(mode==4){ maxSpeed = 45-20*fabs(turn); }else{ maxSpeed = 85 - 50*fabs(turn); // maxSpeed = 100 - 65*fabs(turn); // maxSpeed = 110 - 70*fabs(turn); // maxSpeed = 120 - 85*fabs(turn); } if(autoOn){ lSpeed = (speed + turnBoost*turn) * maxSpeed; rSpeed = (speed - turnBoost*turn) * maxSpeed; } #ifdef DEBUG_MAIN printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime); #endif //DEBUG_MAIN #ifdef DUMP_GPS if(dumpCount==0){ if (fp != NULL) { fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time); } } dumpCount = dumpCount+1%DUMPGPSDELAY; #endif //DUMP_GPS #ifdef DEBUG_TARGET debugTarget(); #endif //DEBUG_TARGET #ifdef DEBUG_GUIDE debugGuide(); #endif //DEBUG_GUIDE #ifdef DEBUG_GPS debugGPS(); #endif //DEBUG_GPS #ifdef DEBUG_LIDAR debugSICK(); #endif //DEBUG_LIDAR #ifdef DEBUG_BUFFER debugCombinedBufferAngles(); #endif //DEBUG_BUFFE #ifdef DEBUG_VISUALIZER robotX = currentXY.x; robotY = currentXY.y; robotTheta = robotTheta;//redundant I know.... targetX = targetXY.x; targetY = targetXY.y; // should probably pass the above to the function... paintPathPlanner(robotX,robotY,robotTheta); showPlot(); #endif //VISUALIZER #ifdef USE_MAP if(mapCount==0){ // mapRobot(currentXY.x,currentXY.y,robotTheta); if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta); else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY; mapVSICK(currentXY.x,currentXY.y,robotTheta); // mapVSICK(0,0,0); #ifdef USE_LIDAR mapSICK(currentXY.x,currentXY.y,robotTheta); #endif showMap(); // printf("MAPPING\n"); } mapCount= (mapCount+1)%MAPDELAY; #endif //USE_MAP sendSpeed(lSpeed,rSpeed); Sleep(5); } #ifdef DUMP_GPS fclose(fp); #endif return 0; }
void setup() { CardReaderValue.cardStatus = MM_UNDEFINED_ERROR; Serial.begin(115200); Serial.println("Init"); //init Display displayStatus = TFTInit(); switch (displayStatus) { case MM_SUCCESS: DisplayMaster("Display Initialisiert\n", DebugPC); Serial.println("Display Initialisiert"); break; default: DisplayError("Display Init-ERROR!", DebugPC); Serial.println("Display Init-ERROR!"); DebugPC = false; break; } //init GPS p_myGPSData = &myGPSData; initGPS(); //Versuche ein GPS Signal zu bekommen, damit die Logdatei das Datum als Namen bekommt while(p_myGPSData->status != MM_SUCCESS) { DisplayMaster("GPS noch nicht Initialisiert\n", DebugPC); Serial.println("GPS noch nicht Initialisiert"); delay(2500); readGPS(p_myGPSData); } //init CardReader char * FileName = ""; String sFileName = "GPS_Log" + String(myGPSData.year, DEC) + "_" + String(myGPSData.month, DEC)+ "_" + String(myGPSData.day, DEC) + ".txt"; sFileName.toCharArray(FileName, sFileName.length() + 1); CardReaderValue = SDInit(FileName); switch (CardReaderValue.cardStatus) { case MM_SUCCESS: DisplayMaster("CardReader Initialisiert\n", DebugPC); Serial.println("CardReader Initialisiert"); break; default: DisplayError("CardReader Init-ERROR!", DebugPC); Serial.println("CardReader Init-ERROR!"); break; } }
void cliCom(void) { uint8_t i2cReadBuff; // TEMP uint8_t index; uint8_t numChannels = 8; char mvlkToggleString[5] = { 0, 0, 0, 0, 0 }; if (eepromConfig.receiverType == PPM) numChannels = eepromConfig.ppmChannels; if ((cliPortAvailable() && !validCliCommand)) { // Pull one character from buffer to find command cliQuery = cliPortRead(); // Check to see if we should toggle MAVLink state (pound sign) if (cliQuery == '#') { while (cliPortAvailable == false); // Check to see if we have 4 pound signs readStringCLI(mvlkToggleString, 5); if ((mvlkToggleString[0] == '#') && (mvlkToggleString[1] == '#') && (mvlkToggleString[2] == '#') && (mvlkToggleString[3] == '#')) { // Toggle MAVLink if (eepromConfig.mavlinkEnabled == false) eepromConfig.mavlinkEnabled = true; else eepromConfig.mavlinkEnabled = false; // Write EEPROM state if pounds were followed by W if (mvlkToggleString[4] == 'W') { cliPortPrint("\nWriting EEPROM Parameters....\n"); writeEEPROM(); } } } } validCliCommand = false; // If MAVLink is disabled and we aren't toggling MAVLink, assume CLI command if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#')) { switch (cliQuery) { /////////////////////////////// case 'a': // Rate PIDs cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_RATE_PID].P, eepromConfig.PID[ROLL_RATE_PID].I, eepromConfig.PID[ROLL_RATE_PID].D, eepromConfig.PID[ROLL_RATE_PID].Limit); cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_RATE_PID].P, eepromConfig.PID[PITCH_RATE_PID].I, eepromConfig.PID[PITCH_RATE_PID].D, eepromConfig.PID[PITCH_RATE_PID].Limit); cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[YAW_RATE_PID].P, eepromConfig.PID[YAW_RATE_PID].I, eepromConfig.PID[YAW_RATE_PID].D, eepromConfig.PID[YAW_RATE_PID].Limit); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'b': // Attitude PIDs cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_ATT_PID].P, eepromConfig.PID[ROLL_ATT_PID].I, eepromConfig.PID[ROLL_ATT_PID].D, eepromConfig.PID[ROLL_ATT_PID].Limit); cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_ATT_PID].P, eepromConfig.PID[PITCH_ATT_PID].I, eepromConfig.PID[PITCH_ATT_PID].D, eepromConfig.PID[PITCH_ATT_PID].Limit); cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HEADING_PID].P, eepromConfig.PID[HEADING_PID].I, eepromConfig.PID[HEADING_PID].D, eepromConfig.PID[HEADING_PID].Limit); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'c': // Velocity PIDs cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[NDOT_PID].P, eepromConfig.PID[NDOT_PID].I, eepromConfig.PID[NDOT_PID].D, eepromConfig.PID[NDOT_PID].Limit); cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[EDOT_PID].P, eepromConfig.PID[EDOT_PID].I, eepromConfig.PID[EDOT_PID].D, eepromConfig.PID[EDOT_PID].Limit); cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HDOT_PID].P, eepromConfig.PID[HDOT_PID].I, eepromConfig.PID[HDOT_PID].D, eepromConfig.PID[HDOT_PID].Limit); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'd': // Position PIDs cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[N_PID].P, eepromConfig.PID[N_PID].I, eepromConfig.PID[N_PID].D, eepromConfig.PID[N_PID].Limit); cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[E_PID].P, eepromConfig.PID[E_PID].I, eepromConfig.PID[E_PID].D, eepromConfig.PID[E_PID].Limit); cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[H_PID].P, eepromConfig.PID[H_PID].I, eepromConfig.PID[H_PID].D, eepromConfig.PID[H_PID].Limit); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'e': // Loop Delta Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz, deltaTime500Hz, deltaTime100Hz, deltaTime50Hz, deltaTime10Hz, deltaTime5Hz, deltaTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'f': // Loop Execution Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz, executionTime500Hz, executionTime100Hz, executionTime50Hz, executionTime10Hz, executionTime5Hz, executionTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'g': // 100 Hz Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'h': // 100 hz Earth Axis Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS], earthAxisAccels[YAXIS], earthAxisAccels[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'i': // 500 hz Gyros cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D, sensors.gyro500Hz[PITCH] * R2D, sensors.gyro500Hz[YAW ] * R2D, mpu6000Temperature); validCliCommand = false; break; /////////////////////////////// case 'j': // 10 Hz Mag Data cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'k': // Vertical Axis Variables cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature, aglRead()); validCliCommand = false; break; /////////////////////////////// case 'l': // Attitudes cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D, sensors.attitude500Hz[PITCH] * R2D, sensors.attitude500Hz[YAW ] * R2D); validCliCommand = false; break; /////////////////////////////// case 'm': // Axis PIDs cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ], ratePID[PITCH], ratePID[YAW ]); validCliCommand = false; break; /////////////////////////////// case 'n': // GPS Data cliPortPrintF("ITOW:%12ld, LAT:%12ld, LONG:%12ld, HEAD:%12ld, HEIGHT:%12ld, HMSL:%12ld, FIX:%4d, NUMSAT:%4d\n", gps.iTOW, gps.latitude, gps.longitude, gps.hDop, gps.height, gps.hMSL, gps.fix, gps.numSats); validCliCommand = false; break; /////////////////////////////// case 'o': cliPortPrintF("%9.4f\n", batteryVoltage); validCliCommand = false; break; /////////////////////////////// case 'p': cameraCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'q': adcCLI(); cliQuery = 'x'; validCliCommand = false; break; ///////////////////////////////FLIGHT MODE case 'r': if (flightMode == RATE) cliPortPrint("Flight Mode:RATE "); else if (flightMode == ATTITUDE) cliPortPrint("Flight Mode:ATTITUDE "); else if (flightMode == GPS) cliPortPrint("Flight Mode:GPS "); if (headingHoldEngaged == true) cliPortPrint("Heading Hold:ENGAGED "); else cliPortPrint("Heading Hold:DISENGAGED "); switch (verticalModeState) { case ALT_DISENGAGED_THROTTLE_ACTIVE: cliPortPrint("Alt:Disenaged Throttle Active "); break; case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: cliPortPrint("Alt:Hold Fixed at Engagement Alt "); break; case ALT_HOLD_AT_REFERENCE_ALTITUDE: cliPortPrint("Alt:Hold at Reference Alt "); break; case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: cliPortPrint("Alt:Velocity Hold at Reference Vel "); break; case ALT_DISENGAGED_THROTTLE_INACTIVE: cliPortPrint("Alt:Disengaged Throttle Inactive "); break; } if (rxCommand[AUX3] > MIDCOMMAND) cliPortPrint("Mode:Simple "); else cliPortPrint("Mode:Normal "); if (rxCommand[AUX4] > MIDCOMMAND) cliPortPrint("Emergency Bail:Active\n"); else cliPortPrint("Emergency Bail:Inactive\n"); validCliCommand = false; break; /////////////////////////////// case 's': // Raw Receiver Commands if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0)) { for (index = 0; index < maxChannelNum - 1; index++) cliPortPrintF("%4ld, ", spektrumBuf[index]); cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]); } else if (eepromConfig.receiverType == SBUS) { for (index = 0; index < 7; index++) cliPortPrintF("%4ld, ", sBusRead(index)); cliPortPrintF("%4ld\n", sBusRead(7)); } else { for (index = 0; index < numChannels - 1; index++) cliPortPrintF("%4i, ", Inputs[index].pulseWidth); cliPortPrintF("%4i\n", Inputs[numChannels - 1].pulseWidth); } validCliCommand = false; break; /////////////////////////////// case 't': // Processed Receiver Commands for (index = 0; index < numChannels - 1; index++) cliPortPrintF("%8.2f, ", rxCommand[index]); cliPortPrintF("%8.2f\n", rxCommand[numChannels - 1]); validCliCommand = false; break; /////////////////////////////// case 'u': // Command in Detent Discretes cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false"); cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false"); cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false"); validCliCommand = false; break; /////////////////////////////// case 'v': // ESC PWM Outputs cliPortPrintF("%4ld, ", TIM8->CCR4); cliPortPrintF("%4ld, ", TIM8->CCR3); cliPortPrintF("%4ld, ", TIM8->CCR2); cliPortPrintF("%4ld, ", TIM8->CCR1); cliPortPrintF("%4ld, ", TIM2->CCR1); cliPortPrintF("%4ld, ", TIM2->CCR2); cliPortPrintF("%4ld, ", TIM3->CCR1); cliPortPrintF("%4ld\n", TIM3->CCR2); validCliCommand = false; break; /////////////////////////////// case 'w': // Servo PWM Outputs cliPortPrintF("%4ld, ", TIM5->CCR3); cliPortPrintF("%4ld, ", TIM5->CCR2); cliPortPrintF("%4ld\n", TIM5->CCR1); validCliCommand = false; break; /////////////////////////////// case 'x': validCliCommand = false; break; /////////////////////////////// case 'y': // ESC Calibration escCalibration(); cliQuery = 'x'; break; /////////////////////////////// case 'z': // ADC readings cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n", adcValue(1), adcValue(2), adcValue(3), adcValue(4), adcValue(5), adcValue(6), adcValue(7)); break; /////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////// case 'A': // Read Roll Rate PID Values readCliPID(ROLL_RATE_PID); cliPortPrint( "\nRoll Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'B': // Read Pitch Rate PID Values readCliPID(PITCH_RATE_PID); cliPortPrint( "\nPitch Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'C': // Read Yaw Rate PID Values readCliPID(YAW_RATE_PID); cliPortPrint( "\nYaw Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'D': // Read Roll Attitude PID Values readCliPID(ROLL_ATT_PID); cliPortPrint( "\nRoll Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'E': // Read Pitch Attitude PID Values readCliPID(PITCH_ATT_PID); cliPortPrint( "\nPitch Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'F': // Read Heading Hold PID Values readCliPID(HEADING_PID); cliPortPrint( "\nHeading PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'G': // Read nDot PID Values readCliPID(NDOT_PID); cliPortPrint( "\nnDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'H': // Read eDot PID Values readCliPID(EDOT_PID); cliPortPrint( "\neDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'I': // Read hDot PID Values readCliPID(HDOT_PID); cliPortPrint( "\nhDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'J': // Read n PID Values readCliPID(N_PID); cliPortPrint( "\nn PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'K': // Read e PID Values readCliPID(E_PID); cliPortPrint( "\ne PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'L': // Read h PID Values readCliPID(H_PID); cliPortPrint( "\nh PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'M': // MAX7456 CLI max7456CLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'N': // Mixer CLI mixerCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'O': // Receiver CLI receiverCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'P': // Sensor CLI sensorCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Q': // GPS Type Selection cliBusy = true; eepromConfig.gpsType = (uint8_t)readFloatCLI(); switch(eepromConfig.gpsType) { case NO_GPS: cliPortPrint("GPS Module Disabled\n"); break; case MEDIATEK_3329_BINARY: cliPortPrint("GPS Module set to MEDIATEK3329 (Binary)\n"); break; case MEDIATEK_3329_NMEA: cliPortPrint("GPS Module set to MEDIATEK3329 (NMEA)\n"); break; case UBLOX: cliPortPrint("GPS Module set to UBLOX\n"); break; default: cliPortPrint("Invalid GPS module type. Use 0-3 (NONE, MEDIATEK BINARY, MEDIATEK NMEA, UBLOX\n"); break; } initGPS(); cliBusy = false; cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'R': // Reset to Bootloader cliPortPrint("Entering Bootloader....\n\n"); delay(100); systemReset(true); break; /////////////////////////////// case 'S': // Reset System cliPortPrint("\nSystem Reseting....\n\n"); delay(100); systemReset(false); break; /////////////////////////////// case 'T': // Telemetry CLI telemetryCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'U': // EEPROM CLI eepromCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'V': // Reset EEPROM Parameters cliPortPrint( "\nEEPROM Parameters Reset....\n" ); checkFirstTime(true); cliPortPrint("\nSystem Resetting....\n\n"); delay(100); systemReset(false); break; /////////////////////////////// case 'W': // Write EEPROM Parameters cliPortPrint("\nWriting EEPROM Parameters....\n"); writeEEPROM(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'X': // Environmental Sensor Bus CLI esbCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Y': // ADC CLI adcCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Z': // Not Used computeGeoMagElements(); cliQuery = 'x'; break; /////////////////////////////// case '?': // Command Summary cliBusy = true; cliPortPrint("\n"); cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AP;I;D;N\n"); cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BP;I;D;N\n"); cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CP;I;D;N\n"); cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DP;I;D;N\n"); cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EP;I;D;N\n"); cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FP;I;D;N\n"); cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GP;I;D;N\n"); cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HP;I;D;N\n"); cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IP;I;D;N\n"); cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JP;I;D;N\n"); cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KP;I;D;N\n"); cliPortPrint("'l' Attitudes 'L' Set h PID Data LP;I;D;N\n"); cliPortPrint("'m' Axis PIDs 'M' MAX7456 CLI\n"); cliPortPrint("'n' GPS Data 'N' Mixer CLI\n"); cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n"); cliPortPrint("'p' Camera CLI 'P' Sensor CLI\n"); cliPortPrint("'q' ADC CLI 'Q' GPS Data Selection\n"); cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n"); cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n"); cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n"); cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n"); cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n"); cliPortPrint("'w' Servo PWM Outputs 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Terminate Serial Communication 'X' ESB CLI\n"); cliPortPrint("'y' ESC Calibration/Motor Verification 'Y' ADC CLI\n"); cliPortPrint("'z' ADC Values 'Z' WMM Test\n"); cliPortPrint(" '?' Command Summary\n"); cliPortPrint("\n"); cliQuery = 'x'; cliBusy = false; break; /////////////////////////////// } } }
void gpsCLI() { USART_InitTypeDef USART_InitStructure; uint8_t gpsQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering GPS CLI....\n\n"); while(true) { cliPrint("GPS CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) gpsQuery = cliRead(); cliPrint("\n"); switch(gpsQuery) { /////////////////////////// case 'a': // GPS Installation Data cliPrint("\n"); switch(eepromConfig.gpsType) { /////////////// case NO_GPS: cliPrint("No GPS Installed....\n\n"); break; /////////////// case MEDIATEK_3329_BINARY: cliPrint("MediaTek 3329 GPS installed, Binary Mode....\n\n"); break; /////////////// case MEDIATEK_3329_NMEA: cliPrint("MediaTek 3329 GPS Installed, NMEA Mode....\n\n"); break; /////////////// case UBLOX: cliPrint("UBLOX GPS Installed, Binary Mode....\n\n"); break; /////////////// } cliPrintF("GPS Baud Rate: %6ld\n\n", eepromConfig.gpsBaudRate); validQuery = false; break; /////////////////////////// case 'x': cliPrint("\nExiting GPS CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Set GPS Installed State to False eepromConfig.gpsType = NO_GPS; gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Set GPS Type to MediaTek 3329 Binary eepromConfig.gpsType = MEDIATEK_3329_BINARY; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Set GPS Type to MediaTek 3329 NMEA eepromConfig.gpsType = MEDIATEK_3329_NMEA; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Set GPS Type to UBLOX Binary eepromConfig.gpsType = UBLOX; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'S': // Read GPS Baud Rate eepromConfig.gpsBaudRate = (uint16_t)readFloatCLI(); USART_StructInit(&USART_InitStructure); USART_InitStructure.USART_BaudRate = eepromConfig.gpsBaudRate; USART_Init(USART2, &USART_InitStructure); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Display GPS Installation Data 'A' Set GPS Type to No GPS\n"); cliPrint(" 'B' Set GPS Type to MediaTek 3329 Binary\n"); cliPrint(" 'C' Set GPS Type to MediaTek 3329 NMEA\n"); cliPrint(" 'D' Set GPS Type to UBLOX\n"); cliPrint(" 'S' Set GPS Baud Rate\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit GPS CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
void pathManagerInit(void) { initTimer4(); initLED(0); //Communication with GPS initGPS(); initBatterySensor(); initAirspeedSensor(); initSPI(IC_DMA_PORT, DMA_CLOCK_KHZ, SPI_MODE1, SPI_BYTE, SPI_MASTER); initInterchip(DMA_CHIP_ID_PATH_MANAGER); //Communication with Altimeter if (initAltimeter()){ float initialValue = 0; while (initialValue == 0) initialValue = getAltitude(); calibrateAltimeter(initialValue); } //Initialize Home Location home.altitude = 400; home.latitude = RELATIVE_LATITUDE; home.longitude = RELATIVE_LONGITUDE; home.radius = 1; home.id = -1; //Initialize first path nodes // PathData* node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473662; // node->longitude = -80.540019; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473479; // node->longitude = -80.540601; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473718; // node->longitude = -80.540837; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473946; // node->longitude = -80.540261; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473685; // node->longitude = -80.540073; // node->radius = 5; // appendPathNode(node); #if DEBUG char str[20]; sprintf(str, "PM:%d, AM:%d", sizeof(PMData),sizeof(AMData)); debug(str); #endif }
void main() { auto int chan, ang, rvalue; auto float dist; auto int pwm, r_head, r_head2, head; auto long freq; auto char tmpbuf[128], c; char gpsString[MAX_SENTENCE]; GPSPosition testPoint, testPoint_old; brdInit(); serPORT(BAUDGPS); //serCopen(BAUDGPS); serMode(0); serCrdFlush();// main loop initGPS(); freq = pwm_init(450ul); printf("pick heading: "); r_head = atoi(gets(tmpbuf)); //while(1) // { /*goal.lat_degrees = 43; goal.lat_minutes = 7036; goal.lon_degrees = 72; goal.lon_minutes = 2811; goal.lat_direction = 'N'; goal.lon_direction = 'W'; goal.sog = 0; //goal.tog = */ if ((r_head > 90) || (r_head < -90)) { printf("\nbad heading "); } else { while (1) { costate GPSRead always_on { // wait until gps mode is engaged and a gps string has been recieved waitfor(serCrdUsed() > 0); //printf("gps read: "); // read until finding the beginning of a gps string then wait while(1) { //int test2; c = serCgetc(); if (c == -1) { serCrdFlush(); abort; } else if (c == '$') { waitfor(DelayMs(20)); //should only take 5ms or so at 115200 baud break; } }//end while(1) // now that 20 ms have passed, read in the gps string (it must all // be there by now, since so much time has passed getgps(gpsString); rvalue = gps_get_position(&testPoint,gpsString); if( rvalue != -1) { printGPSPosition(&testPoint); } //printf("gps: %u \n",strlen(test2)); printf("gps: %s ",gpsString); //printGPSPosition(&testPoint); //puts(gpsString); //puts(": end gps \n"); //dist = gps_ground_distance(&testPoint, &testPoint_old); //head = (int)gps_bearing(&testPoint, &testPoint_old, dist); //testPoint_old = testPoint; head = testPoint.tog; // grab current heading r_head2 = r_head-head; pwm = set_angle(0, r_head2); printf("angle: %d, head %d \n",pwm, head); }//end costate } // end while } // end else //} // end first while } // end main
int main () { CHIP_Init(); //Initialize Chip initMCU(); initGPIO(); //Initialize GPIO sdi2c_Init(); //Initialize I2C initTimer(); //initialize timer for timer interuprt /* Setup SysTick Timer for 10 msec interrupts */ if (SysTick_Config(CMU_ClockFreqGet(cmuClock_CORE) / 1000)) { while (1) ; } /*Enable all sensors and GPS */ GPIO_PinOutSet(GPS_PORT, ENABLE); //Enable GPS Sensor board BMP180_GetCalData(); //Enable and get calibration data from BMP180 initLSM303_LowPower(); //Enable Accel in low power mode initL3GD20H_Normal(); //Enable Gyro in normal mode GPIO_PinOutToggle(EXT_LED, YEL_LED); /* Acceleromter Data */ Delay(10); LSM303_GetAccelData(); LSM303_PowerOff(); GPIO_PinOutToggle(EXT_LED, YEL_LED); /*Gyroscope Data */ Delay(10); L3GD20H_GetGyroData(); L3GD20H_PowerOff(); GPIO_PinOutToggle(EXT_LED, YEL_LED); /*Temperature/Barometric Pressure */ Delay(10); BMP180_GetTemp(); BMP180_GetPressure(); BMP180_CalcRealTemperature(); BMP180_CalcRealPressure(); GPIO_PinOutToggle(EXT_LED, YEL_LED); /*Power Sensor*/ INA219_GetVoltage(); GPIO_PinOutClear(EXT_LED, YEL_LED); GPIO_PinOutToggle(EXT_LED, GRE_LED); Delay(100); GPIO_PinOutToggle(EXT_LED, GRE_LED); /* GPS Information */ timeout = 0; //reset timeout initGPS(); //Initialize GPS UART coldrestart(); //Send cold restart command while(fullread == 0) { //while not a full read, do the following if(satfix == 1) { //if there is a satellite fix, then... GPIO_PinOutSet(EXT_LED, YEL_LED); //toggle LED for debug readdata(); //readdata timeout = 0; } if(timeout > 20) { GPIO_PinOutClear(EXT_LED, YEL_LED); GPStimeout(); break; } } GPIO_PinOutSet(EXT_LED, GRE_LED); Delay(500); printGPS(0); //Print GPS to screen for debug concact(); /* Infinite loop */ while(1) { if(GPIO_PinInGet(BUT_PORT, RIGHT_BUT) == 0) { r = r + 1; if(r > 2) { //wrap around to beginning r = 0; } printGPS(r); } if(GPIO_PinInGet(BUT_PORT, LEFT_BUT) == 0) { r = r - 1; if(r < 0) { //wrap around to beginning r = 2; } printSENSORS(); } // if(GPIO_PinInGet(GPS_PORT, FIX) == 1) { // GPIO_PinOutToggle(EXT_LED, RED_LED); // } } }