Michael Shimniok
Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /** Code for "Data Bus" UGV entry for Sparkfun AVC 2012 00002 * http://www.bot-thoughts.com/ 00003 */ 00004 00005 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00006 // INCLUDES 00007 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00008 00009 #include <stdio.h> 00010 #include <math.h> 00011 #include "mbed.h" 00012 #include "globals.h" 00013 #include "Config.h" 00014 #include "Buttons.h" 00015 #include "Menu.h" 00016 //#include "lcd.h" 00017 #include "SerialGraphicLCD.h" 00018 #include "Bargraph.h" 00019 #include "GPSStatus.h" 00020 #include "logging.h" 00021 #include "shell.h" 00022 #include "Sensors.h" 00023 //#include "DCM.h" 00024 //#include "dcm_matrix.h" 00025 #include "kalman.h" 00026 #include "GPS.h" 00027 #include "Camera.h" 00028 #include "PinDetect.h" 00029 #include "Actuators.h" 00030 #include "IncrementalEncoder.h" 00031 #include "Steering.h" 00032 #include "Schedule.h" 00033 #include "GeoPosition.h" 00034 #include "Mapping.h" 00035 #include "SimpleFilter.h" 00036 #include "Beep.h" 00037 #include "util.h" 00038 #include "MAVlink/include/mavlink_bridge.h" 00039 #include "updater.h" 00040 00041 #define LCD_FMT "%-20s" // used to fill a single line on the LCD screen 00042 00043 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00044 // DEFINES 00045 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 00047 #define absf(x) (x *= (x < 0.0) ? -1 : 1) 00048 00049 #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course 00050 #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position 00051 00052 #define UPDATE_PERIOD 0.010 // update period in s 00053 #define UPDATE_PERIOD_MS 10 // update period in ms 00054 00055 // Driver configuration parameters 00056 #define SONARLEFT_CHAN 0 00057 #define SONARRIGHT_CHAN 1 00058 #define IRLEFT_CHAN 2 00059 #define IRRIGHT_CHAN 3 00060 #define TEMP_CHAN 4 00061 #define GYRO_CHAN 5 00062 00063 // Chassis specific parameters 00064 #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375m circumference 00065 #define WHEELBASE 0.290 00066 #define TRACK 0.280 00067 00068 #define INSTRUMENT_CHECK 0 00069 #define AHRS_VISUALIZATION 1 00070 #define DISPLAY_PANEL 2 00071 00072 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00073 // GLOBAL VARIABLES 00074 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00075 00076 // OUTPUT 00077 DigitalOut confStatus(LED1); // Config file status LED 00078 DigitalOut logStatus(LED2); // Log file status LED 00079 DigitalOut gpsStatus(LED3); // GPS fix status LED 00080 DigitalOut ahrsStatus(LED4); // AHRS status LED 00081 //DigitalOut sonarStart(p18); // Sends signal to start sonar array pings 00082 Beep speaker(p24); // Piezo speaker 00083 00084 // INPUT 00085 Menu menu; 00086 Buttons keypad; 00087 00088 // VEHICLE 00089 Steering steerCalc(TRACK, WHEELBASE); // steering calculator 00090 00091 // COMM 00092 Serial pc(USBTX, USBRX); // PC usb communications 00093 SerialGraphicLCD lcd(p17, p18, SD_FW); // Graphic LCD with summoningdark firmware 00094 //Serial *debug = &pc; 00095 00096 // SENSORS 00097 Sensors sensors; // Abstraction of sensor drivers 00098 //DCM ahrs; // ArduPilot/MatrixPilot AHRS 00099 Serial *dev; // For use with bridge 00100 GPS gps(p26, p25, VENUS); // gps 00101 00102 FILE *camlog; // Camera log 00103 00104 // Configuration 00105 Config config; // Persistent configuration 00106 // Course Waypoints 00107 // Sensor Calibration 00108 // etc. 00109 00110 // Timing 00111 Timer timer; // For main loop scheduling 00112 Ticker sched; // scheduler for interrupt driven routines 00113 00114 // Overall system state (used for logging but also convenient for general use 00115 SystemState state[SSBUF]; // system state for logging, FIFO buffer 00116 unsigned char inState; // FIFO items enter in here 00117 unsigned char outState; // FIFO items leave out here 00118 bool ssBufOverrun = false; 00119 00120 // GPS Variables 00121 unsigned long age = 0; // gps fix age 00122 00123 // schedule for LED warning flasher 00124 Schedule blink; 00125 00126 // Estimation & Navigation Variables 00127 GeoPosition dr_here; // Estimated position based on estimated heading 00128 GeoPosition gps_here; // current gps position 00129 Mapping mapper; 00130 00131 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00132 // FUNCTION DEFINITIONS 00133 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00134 00135 void initFlasher(void); 00136 void initDR(void); 00137 int autonomousMode(void); 00138 void mavlinkMode(void); 00139 void servoCalibrate(void); 00140 void serialBridge(Serial &gps); 00141 int instrumentCheck(void); 00142 void displayData(int mode); 00143 int compassCalibrate(void); 00144 int compassSwing(void); 00145 int gyroSwing(void); 00146 int setBacklight(void); 00147 int reverseScreen(void); 00148 float gyroRate(unsigned int adc); 00149 float sonarDistance(unsigned int adc); 00150 float irDistance(unsigned int adc); 00151 float getVoltage(void); 00152 extern "C" void mbed_reset(); 00153 00154 extern unsigned int matrix_error; 00155 00156 // If we don't close the log file, when we restart, all the written data 00157 // will be lost. So we have to use a button to force mbed to close the 00158 // file and preserve the data. 00159 // 00160 00161 int dummy(void) 00162 { 00163 return 0; 00164 } 00165 00166 00167 // TODO: 3 move to GPS module 00168 /* GPS serial interrupt handler 00169 */ 00170 void gpsRecv() { 00171 while (gps.serial.readable()) { 00172 gpsStatus = !gpsStatus; 00173 gps.nmea.encode(gps.serial.getc()); 00174 gpsStatus = !gpsStatus; 00175 } 00176 } 00177 00178 00179 int resetMe() 00180 { 00181 mbed_reset(); 00182 00183 return 0; 00184 } 00185 00186 00187 #define DISPLAY_CLEAR 0x01 00188 #define DISPLAY_SET_POS 0x08 00189 00190 00191 int main() 00192 { 00193 // Send data back to the PC 00194 pc.baud(115200); 00195 lcd.baud(115200); 00196 lcd.printf("test\n"); // hopefully force 115200 on powerup 00197 lcd.clear(); 00198 wait(0.3); 00199 lcd.printf("Data Bus mAGV V2"); 00200 00201 fprintf(stdout, "Data Bus mAGV Control System\n"); 00202 00203 fprintf(stdout, "Initialization...\n"); 00204 lcd.pos(0,1); 00205 lcd.printf(LCD_FMT, "Initializing"); 00206 wait(0.5); 00207 00208 gps.setUpdateRate(10); 00209 00210 // Initialize status LEDs 00211 ahrsStatus = 0; 00212 gpsStatus = 0; 00213 logStatus = 0; 00214 confStatus = 0; 00215 00216 //ahrs.G_Dt = UPDATE_PERIOD; 00217 00218 fprintf(stdout, "Loading configuration...\n"); 00219 lcd.pos(0,1); 00220 lcd.printf(LCD_FMT, "Load config"); 00221 wait(0.5); 00222 if (config.load()) // Load various configurable parameters, e.g., waypoints, declination, etc. 00223 confStatus = 1; 00224 00225 // Something here is jacking up the I2C stuff 00226 initSteering(); 00227 initThrottle(); 00228 // initFlasher(); // Initialize autonomous mode flasher 00229 00230 sensors.Compass_Calibrate(config.magOffset, config.magScale); 00231 pc.printf("Declination: %.1f\n", config.declination); 00232 pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", 00233 config.escZero, config.escMax, config.topSpeed, config.turnSpeed, 00234 config.speedKp, config.speedKi, config.speedKd); 00235 pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n", config.steerZero, config.steerGain, config.steerGainAngle); 00236 00237 // Convert lat/lon waypoints to cartesian 00238 mapper.init(config.wptCount, config.wpt); 00239 for (int w = 0; w < MAXWPT && w < config.wptCount; w++) { 00240 mapper.geoToCart(config.wpt[w], &(config.cwpt[w])); 00241 pc.printf("Waypoint #%d (%.4f, %.4f) lat: %.6f lon: %.6f\n", 00242 w, config.cwpt[w]._x, config.cwpt[w]._y, config.wpt[w].latitude(), config.wpt[w].longitude()); 00243 } 00244 00245 // TODO: 3 print mag and gyro calibrations 00246 00247 lcd.pos(0,1); 00248 lcd.printf(LCD_FMT, "GPS configuration "); 00249 gps.setType(config.gpsType); 00250 gps.setBaud(config.gpsBaud); 00251 fprintf(stdout, "GPS config: type=%d baud=%d\n", config.gpsType, config.gpsBaud); 00252 00253 lcd.pos(0,1); 00254 lcd.printf(LCD_FMT, "Nav configuration "); 00255 steerCalc.setIntercept(config.interceptDist); // Setup steering calculator based on intercept distance 00256 pc.printf("Intercept distance: %.1f\n", config.interceptDist); 00257 pc.printf("Waypoint distance: %.1f\n", config.waypointDist); 00258 pc.printf("Brake distance: %.1f\n", config.brakeDist); 00259 pc.printf("Min turn radius: %.3f\n", config.minRadius); 00260 00261 fprintf(stdout, "Calculating offsets...\n"); 00262 lcd.pos(0,1); 00263 lcd.printf(LCD_FMT, "Offset calculation "); 00264 wait(0.5); 00265 // TODO: 3 Really need to give the gyro more time to settle 00266 sensors.Calculate_Offsets(); 00267 00268 fprintf(stdout, "Starting GPS...\n"); 00269 lcd.pos(0,1); 00270 lcd.printf(LCD_FMT, "Start GPS "); 00271 wait(0.5); 00272 // TODO: 3 move this to GPS module 00273 gps.serial.attach(gpsRecv, Serial::RxIrq); 00274 // TODO: 3 enable and process GSV as bar graph 00275 //gps.gsvMessage(false); 00276 //gps.gsaMessage(true); 00277 00278 fprintf(stdout, "Starting Scheduler...\n"); 00279 lcd.pos(0,1); 00280 lcd.printf(LCD_FMT, "Start scheduler "); 00281 wait(0.5); 00282 // Startup sensor/AHRS ticker; update every 10ms = 100hz 00283 restartNav(); 00284 sched.attach(&update, UPDATE_PERIOD); 00285 00286 /* 00287 fprintf(stdout, "Starting Camera...\n"); 00288 lcd.pos(0,1); 00289 lcd.printf(LCD_FMT, "Start Camera "); 00290 wait(0.5); 00291 cam.start(); 00292 */ 00293 00294 // Let's try setting serial IRQs lower and see if that alleviates issues with I2C reads and AHRS reads 00295 //NVIC_SetPriority(UART0_IRQn, 1); // USB 00296 //NVIC_SetPriority(UART1_IRQn, 2); // GPS p25,p26 00297 //NVIC_SetPriority(UART3_IRQn, 3); // LCD p17,p18 00298 00299 // Insert menu system here w/ timeout 00300 //bool autoBoot=false; 00301 00302 //speaker.beep(3000.0, 0.2); // non-blocking 00303 00304 keypad.init(); 00305 00306 // Initialize LCD graphics 00307 Bargraph::lcd = &lcd; 00308 Bargraph v(1, 40, 15, 'V'); 00309 v.calibrate(6.3, 8.4); 00310 Bargraph a(11, 40, 15, 'A'); 00311 a.calibrate(0, 15.0); 00312 Bargraph g1(21, 40, 15, 'G'); 00313 g1.calibrate(0, 10); 00314 Bargraph g2(31, 40, 15, 'H'); 00315 g2.calibrate(4.0, 0.8); 00316 //GPSStatus g2(21, 12); 00317 //GPSStatus::lcd = &lcd; 00318 00319 // Setup LCD Input Menu 00320 menu.add("Auto mode", &autonomousMode); 00321 menu.add("Instruments", &instrumentCheck); 00322 menu.add("Calibrate", &compassCalibrate); 00323 menu.add("Compass Swing", &compassSwing); 00324 menu.add("Gyro Calib", &gyroSwing); 00325 //menu.sdd("Reload Config", &loadConfig); 00326 menu.add("Backlight", &setBacklight); 00327 menu.add("Reverse", &reverseScreen); 00328 menu.add("Reset", &resetMe); 00329 00330 char cmd; 00331 bool printMenu = true; 00332 bool printLCDMenu = true; 00333 00334 timer.start(); 00335 timer.reset(); 00336 00337 int thisUpdate = timer.read_ms(); 00338 int nextUpdate = thisUpdate; 00339 //int hdgUpdate = nextUpdate; 00340 00341 while (1) { 00342 00343 /* 00344 if (timer.read_ms() > hdgUpdate) { 00345 fprintf(stdout, "He=%.3f %.5f\n", kfGetX(0), kfGetX(1)); 00346 hdgUpdate = timer.read_ms() + 100; 00347 }*/ 00348 00349 if ((thisUpdate = timer.read_ms()) > nextUpdate) { 00350 //fprintf(stdout, "Updating...\n"); 00351 v.update(sensors.voltage); 00352 a.update(sensors.current); 00353 g1.update((float) gps.nmea.sat_count()); 00354 g2.update(gps.hdop); 00355 lcd.posXY(60, 22); 00356 lcd.printf("%.2f", state[inState].rightRanger); 00357 lcd.posXY(60, 32); 00358 lcd.printf("%.2f", state[inState].leftRanger); 00359 lcd.posXY(60, 42); 00360 lcd.printf("%5.1f", state[inState].estHeading); 00361 lcd.posXY(60, 52); 00362 lcd.printf("%.3f", state[inState].gpsCourse); 00363 nextUpdate = thisUpdate + 2000; 00364 // TODO: 3 address integer overflow 00365 // TODO: 3 display scheduler() timing 00366 } 00367 00368 if (keypad.pressed) { 00369 keypad.pressed = false; 00370 printLCDMenu = true; 00371 //causes issue with servo library 00372 //speaker.beep(3000.0, 0.1); // non-blocking 00373 switch (keypad.which) { 00374 case NEXT_BUTTON: 00375 menu.next(); 00376 break; 00377 case PREV_BUTTON: 00378 menu.prev(); 00379 break; 00380 case SELECT_BUTTON: 00381 lcd.pos(0,0); 00382 lcd.printf(">>%-14s", menu.getItemName()); 00383 menu.select(); 00384 printMenu = true; 00385 break; 00386 default: 00387 printLCDMenu = false; 00388 break; 00389 }//switch 00390 keypad.pressed = false; 00391 }// if (keypad.pressed) 00392 00393 00394 if (printLCDMenu) { 00395 lcd.pos(0,0); 00396 lcd.printf("< %-14s >", menu.getItemName()); 00397 lcd.pos(0,1); 00398 lcd.printf(LCD_FMT, "Ready."); 00399 lcd.posXY(50, 22); 00400 lcd.printf("R"); 00401 lcd.rect(58, 20, 98, 30, true); 00402 wait(0.01); 00403 lcd.posXY(50, 32); 00404 lcd.printf("L"); 00405 lcd.rect(58, 30, 98, 40, true); 00406 wait(0.01); 00407 lcd.posXY(50, 42); 00408 lcd.printf("H"); 00409 lcd.rect(58, 40, 98, 50, true); 00410 wait(0.01); 00411 lcd.posXY(44,52); 00412 lcd.printf("GH"); 00413 lcd.rect(58, 50, 98, 60, true); 00414 v.init(); 00415 a.init(); 00416 g1.init(); 00417 g2.init(); 00418 printLCDMenu = false; 00419 } 00420 00421 /* if (autoBoot) { 00422 autoBoot = false; 00423 cmd = 'a'; 00424 } else {*/ 00425 00426 if (printMenu) { 00427 int i=0; 00428 fprintf(stdout, "\n==============\nData Bus Menu\n==============\n"); 00429 fprintf(stdout, "%d) Autonomous mode\n", i++); 00430 fprintf(stdout, "%d) Bridge serial to GPS\n", i++); 00431 fprintf(stdout, "%d) Calibrate compass\n", i++); 00432 fprintf(stdout, "%d) Swing compass\n", i++); 00433 fprintf(stdout, "%d) Gyro calibrate\n", i++); 00434 fprintf(stdout, "%d) Instrument check\n", i++); 00435 fprintf(stdout, "%d) Display AHRS\n", i++); 00436 fprintf(stdout, "%d) Mavlink mode\n", i++); 00437 fprintf(stdout, "%d) Shell\n", i++); 00438 fprintf(stdout, "R) Reset\n"); 00439 fprintf(stdout, "\nSelect from the above: "); 00440 fflush(stdout); 00441 printMenu = false; 00442 } 00443 00444 00445 // Basic functional architecture 00446 // SENSORS -> FILTERS -> AHRS -> POSITION -> NAVIGATION -> CONTROL | INPUT/OUTPUT | LOGGING 00447 // SENSORS (for now) are polled out of AHRS via interrupt every 10ms 00448 // 00449 // no FILTERing in place right now 00450 // if we filter too heavily we get lag. At 30mph = 14m/s a sensor lag 00451 // of only 10ms means the estimate is 140cm behind the robot 00452 // 00453 // POSITION and NAVIGATION should probably always be running 00454 // log file can have different entry type per module, to be demultiplexed on the PC 00455 // 00456 // Autonomous mode engages CONTROL outputs 00457 // 00458 // I/O mode could be one of: MAVlink, serial bridge (gps), sensor check, shell, log to serial 00459 // Or maybe shell should be the main control for different output modes 00460 // 00461 // LOGGING can be turned on or off, probably best to start with it engaged 00462 // and then disable from user panel or when navigation is ended 00463 00464 if (pc.readable()) { 00465 cmd = pc.getc(); 00466 fprintf(stdout, "%c\n", cmd); 00467 printMenu = true; 00468 printLCDMenu = true; 00469 00470 switch (cmd) { 00471 case 'R' : 00472 resetMe(); 00473 break; 00474 case '0' : 00475 lcd.pos(0,0); 00476 lcd.printf(">>%-14s", menu.getItemName(0)); 00477 autonomousMode(); 00478 break; 00479 case '1' : 00480 lcd.pos(0,0); 00481 lcd.printf(">>%-14s", "Serial bridge"); 00482 lcd.pos(0,1); 00483 lcd.printf(LCD_FMT, "Standby."); 00484 gps.gsvMessage(true); 00485 gps.gsaMessage(true); 00486 serialBridge(gps.serial); 00487 gps.gsvMessage(false); 00488 gps.gsaMessage(false); 00489 break; 00490 case '2' : 00491 lcd.pos(0,0); 00492 lcd.printf(">>%-14s", menu.getItemName(1)); 00493 compassCalibrate(); 00494 break; 00495 case '3' : 00496 lcd.pos(0,0); 00497 lcd.printf(">>%-14s", menu.getItemName(2)); 00498 compassSwing(); 00499 break; 00500 case '4' : 00501 lcd.pos(0,0); 00502 lcd.printf(">>%-14s", menu.getItemName(2)); 00503 gyroSwing(); 00504 break; 00505 case '5' : 00506 lcd.pos(0,0); 00507 lcd.printf(">>%-14s", "Instruments"); 00508 lcd.pos(0,1); 00509 lcd.printf(LCD_FMT, "Standby."); 00510 displayData(INSTRUMENT_CHECK); 00511 break; 00512 case '6' : 00513 lcd.pos(0,0); 00514 lcd.printf(">>%-14s", "AHRS Visual'n"); 00515 lcd.pos(0,1); 00516 lcd.printf(LCD_FMT, "Standby."); 00517 displayData(AHRS_VISUALIZATION); 00518 break; 00519 case '7' : 00520 lcd.pos(0,0); 00521 lcd.printf(">>%-14s", "Mavlink mode"); 00522 lcd.pos(0,1); 00523 lcd.printf(LCD_FMT, "Standby."); 00524 mavlinkMode(); 00525 break; 00526 case '8' : 00527 lcd.pos(0,0); 00528 lcd.printf(">>%-14s", "Shell"); 00529 lcd.pos(0,1); 00530 lcd.printf(LCD_FMT, "Standby."); 00531 shell(); 00532 break; 00533 default : 00534 break; 00535 } // switch 00536 00537 } // if (pc.readable()) 00538 00539 } // while 00540 00541 } 00542 00543 00544 00545 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00546 // INITIALIZATION ROUTINES 00547 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00548 00549 00550 void initFlasher() 00551 { 00552 // Set up flasher schedule; 3 flashes every 80ms 00553 // for 80ms total, with a 9x80ms period 00554 blink.max(9); 00555 blink.scale(80); 00556 blink.mode(Schedule::repeat); 00557 blink.set(0, 1); blink.set(2, 1); blink.set(4, 1); 00558 } 00559 00560 00561 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00562 // OPERATIONAL MODE FUNCTIONS 00563 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00564 00565 int autonomousMode() 00566 { 00567 bool goGoGo = false; // signal to start moving 00568 bool navDone; // signal that we're done navigating 00569 extern int tSensor, tGPS, tAHRS, tLog; 00570 00571 // TODO: 3 move to main? 00572 // Navigation 00573 00574 goGoGo = false; 00575 navDone = false; 00576 keypad.pressed = false; 00577 //bool started = false; // flag to indicate robot has exceeded min speed. 00578 00579 if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d 00580 wait(0.2); 00581 00582 gps.setNmeaMessages(true, true, false, false, true, false); // enable GGA, GSA, RMC 00583 gps.serial.attach(gpsRecv, Serial::RxIrq); 00584 00585 lcd.pos(0,1); 00586 lcd.printf(LCD_FMT, "Select starts."); 00587 wait(1.0); 00588 00589 timer.reset(); 00590 timer.start(); 00591 wait(0.1); 00592 00593 // Initialize logging buffer 00594 // Needs to happen after we've reset the millisecond timer and after 00595 // the schedHandler() fires off at least once more with the new time 00596 inState = outState = 0; 00597 00598 // Tell the navigation / position estimation stuff to reset to starting waypoint 00599 restartNav(); 00600 00601 // Main loop 00602 // 00603 while(navDone == false) { 00604 00605 ////////////////////////////////////////////////////////////////////////////// 00606 // USER INPUT 00607 ////////////////////////////////////////////////////////////////////////////// 00608 00609 // Button state machine 00610 // if we've not started going, button starts us 00611 // if we have started going, button stops us 00612 // but only if we've released it first 00613 // 00614 // set throttle only if goGoGo set 00615 if (goGoGo) { 00616 /** acceleration curve */ 00617 /* 00618 if (go.ticked(timer.read_ms())) { 00619 throttle = go.get() / 1000.0; 00620 //fprintf(stdout, "throttle: %0.3f\n", throttle.read()); 00621 } 00622 */ 00623 00624 // TODO: 1 Add additional condition of travel for N meters before 00625 // the HALT button is armed 00626 00627 if (keypad.pressed == true) { // && started 00628 fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> HALT\n"); 00629 lcd.pos(0,1); 00630 lcd.printf(LCD_FMT, "HALT."); 00631 navDone = true; 00632 goGoGo = false; 00633 keypad.pressed = false; 00634 endRun(); 00635 } 00636 } else { 00637 if (keypad.pressed == true) { 00638 fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n"); 00639 lcd.pos(0,1); 00640 lcd.printf(LCD_FMT, "GO GO GO!"); 00641 goGoGo = true; 00642 keypad.pressed = false; 00643 //restartNav(); 00644 beginRun(); 00645 // Doing this for collecting step response, hopefully an S curve... we'll see. 00646 //throttle = config.escMax; 00647 // TODO: 2 Improve encapsulation of the scheduler 00648 // TODO: 2 can we do something clever with GPS position estimate since we know where we're starting? 00649 // E.g. if dist to wpt0 < x then initialize to wpt0 else use gps 00650 } 00651 } 00652 00653 // Are we at the last waypoint? 00654 // 00655 if (state[inState].nextWaypoint == config.wptCount) { 00656 fprintf(stdout, "Arrived at final destination. Done.\n"); 00657 //causes issue with servo library 00658 //speaker.beep(3000.0, 1.0); // non-blocking 00659 lcd.pos(0,1); 00660 lcd.printf(LCD_FMT, "Arrived. Done."); 00661 navDone = true; 00662 endRun(); 00663 } 00664 00665 ////////////////////////////////////////////////////////////////////////////// 00666 // LOGGING 00667 ////////////////////////////////////////////////////////////////////////////// 00668 // sensor reads are happening in the schedHandler(); 00669 // Are there more items to come out of the log buffer? 00670 // Since this could take anywhere from a few hundred usec to 00671 // 150ms, we run it opportunistically and use a buffer. That way 00672 // the sensor updates, calculation, and control can continue to happen 00673 if (outState != inState) { 00674 logStatus = !logStatus; // log indicator LED 00675 //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState); 00676 if (ssBufOverrun) { 00677 fprintf(stdout, ">>> SystemState Buffer Overrun Condition\n"); 00678 ssBufOverrun = false; 00679 } 00680 // do we need to disable interrupts briefly to prevent a race 00681 // condition with schedHandler() ? 00682 int out=outState; // in case we're interrupted this 'should' be atomic 00683 outState++; // increment 00684 outState &= SSBUF; // wrap 00685 logData( state[out] ); // log state data to file 00686 logStatus = !logStatus; // log indicator LED 00687 00688 //fprintf(stdout, "Time Stats\n----------\nSensors: %d\nGPS: %d\nAHRS: %d\nLog: %d\n----------\nTotal: %d", 00689 // tSensor, tGPS, tAHRS, tLog, tSensor+tGPS+tAHRS+tLog); 00690 } 00691 00692 } // while 00693 00694 closeLogfile(); 00695 logStatus = 0; 00696 fprintf(stdout, "Completed, file saved.\n"); 00697 wait(2); // wait from last printout 00698 lcd.pos(0,1); 00699 lcd.printf(LCD_FMT, "Done. Saved."); 00700 wait(2); 00701 00702 ahrsStatus = 0; 00703 gpsStatus = 0; 00704 //confStatus = 0; 00705 //flasher = 0; 00706 00707 gps.gsaMessage(false); 00708 gps.gsvMessage(false); 00709 00710 return 0; 00711 } // autonomousMode 00712 00713 00714 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00715 // UTILITY FUNCTIONS 00716 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00717 00718 00719 int compassCalibrate() 00720 { 00721 bool done=false; 00722 int m[3]; 00723 FILE *fp; 00724 00725 fprintf(stdout, "Entering compass calibration in 2 seconds.\nLaunch _3DScatter Processing app now... type e to exit\n"); 00726 lcd.pos(0,1); 00727 00728 lcd.printf(LCD_FMT, "Starting..."); 00729 00730 fp = openlog("cal"); 00731 00732 wait(2); 00733 lcd.pos(0,1); 00734 lcd.printf(LCD_FMT, "Select exits"); 00735 timer.reset(); 00736 timer.start(); 00737 while (!done) { 00738 00739 if (keypad.pressed) { 00740 keypad.pressed = false; 00741 done = true; 00742 } 00743 00744 while (pc.readable()) { 00745 if (pc.getc() == 'e') { 00746 done = true; 00747 break; 00748 } 00749 } 00750 int millis = timer.read_ms(); 00751 if ((millis % 100) == 0) { 00752 sensors.getRawMag(m); 00753 00754 // Correction 00755 // Let's see how our ellipsoid looks after scaling and offset 00756 /* 00757 float mag[3]; 00758 mag[0] = ((float) m[0] - M_OFFSET_X) * 0.5 / M_SCALE_X; 00759 mag[1] = ((float) m[1] - M_OFFSET_Y) * 0.5 / M_SCALE_Y; 00760 mag[2] = ((float) m[2] - M_OFFSET_Z) * 0.5 / M_SCALE_Z; 00761 */ 00762 00763 bool skipIt = false; 00764 for (int i=0; i < 3; i++) { 00765 if (abs(m[i]) > 1024) skipIt = true; 00766 } 00767 if (!skipIt) { 00768 fprintf(stdout, "%c%d %d %d \r\n", 0xDE, m[0], m[1], m[2]); 00769 fprintf(fp, "%d, %d, %d\n", m[0], m[1], m[2]); 00770 } 00771 } 00772 } 00773 if (fp) { 00774 fclose(fp); 00775 lcd.pos(0,1); 00776 lcd.printf(LCD_FMT, "Done. Saved."); 00777 wait(2); 00778 } 00779 00780 return 0; 00781 } 00782 00783 // Gather gyro data using turntable equipped with dual channel 00784 // encoder. Use onboard wheel encoder system. Left channel 00785 // is the index (0 degree) mark, while the right channel 00786 // is the incremental encoder. Can then compare gyro integrated 00787 // heading with machine-reported heading 00788 // 00789 // Note: some of this code is identical to the compassSwing() code. 00790 // 00791 int gyroSwing() 00792 { 00793 FILE *fp; 00794 00795 // Timing is pretty critical so just in case, disable serial processing from GPS 00796 gps.serial.attach(NULL, Serial::RxIrq); 00797 00798 fprintf(stdout, "Entering gyro swing...\n"); 00799 lcd.pos(0,1); 00800 lcd.printf(LCD_FMT, "Starting..."); 00801 wait(2); 00802 fp = openlog("gy"); 00803 wait(2); 00804 lcd.pos(0,1); 00805 lcd.printf(LCD_FMT, "Begin. Select exits."); 00806 00807 fprintf(stdout, "Begin clockwise rotation, varying rpm... press select to exit\n"); 00808 00809 timer.reset(); 00810 timer.start(); 00811 00812 sensors.rightTotal = 0; // reset total 00813 sensors._right.read(); // easiest way to reset the heading counter 00814 00815 while (1) { 00816 if (keypad.pressed) { 00817 keypad.pressed = false; 00818 break; 00819 } 00820 00821 // Print out data 00822 // fprintf(stdout, "%d,%d,%d,%d,%d\n", timer.read_ms(), heading, sensors.g[0], sensors.g[1], sensors.g[2]); 00823 // sensors.rightTotal gives us each tick of the machine, multiply by 2 for cumulative heading, which is easiest 00824 // to compare with cumulative integration of gyro (rather than dealing with 0-360 degree range and modulus and whatnot 00825 if (fp) fprintf(fp, "%d,%d,%d,%d,%d,%d\n", timer.read_ms(), 2*sensors.rightTotal, sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp); 00826 wait(0.200); 00827 } 00828 if (fp) { 00829 fclose(fp); 00830 lcd.pos(0,1); 00831 lcd.printf(LCD_FMT, "Done. Saved."); 00832 fprintf(stdout, "Data collection complete.\n"); 00833 wait(2); 00834 } 00835 00836 keypad.pressed = false; 00837 00838 return 0; 00839 } 00840 00841 00842 // Swing compass using turntable equipped with dual channel 00843 // encoder. Use onboard wheel encoder system. Left channel 00844 // is the index (0 degree) mark, while the right channel 00845 // is the incremental encoder. 00846 // 00847 // Note: much of this code is identical to the gyroSwing() code. 00848 // 00849 int compassSwing() 00850 { 00851 int revolutions=5; 00852 int heading=0; 00853 int leftCount = 0; 00854 FILE *fp; 00855 // left is index track 00856 // right is encoder track 00857 00858 fprintf(stdout, "Entering compass swing...\n"); 00859 lcd.pos(0,1); 00860 lcd.printf(LCD_FMT, "Starting..."); 00861 wait(2); 00862 fp = openlog("sw"); 00863 wait(2); 00864 lcd.pos(0,1); 00865 lcd.printf(LCD_FMT, "Ok. Begin."); 00866 00867 fprintf(stdout, "Begin clockwise rotation... exit after %d revolutions\n", revolutions); 00868 00869 timer.reset(); 00870 timer.start(); 00871 00872 // wait for index to change 00873 while ((leftCount += sensors._left.read()) < 2) { 00874 if (keypad.pressed) { 00875 keypad.pressed = false; 00876 break; 00877 } 00878 } 00879 fprintf(stdout, ">>>> Index detected. Starting data collection\n"); 00880 leftCount = 0; 00881 lcd.pos(0,1); 00882 lcd.printf("%1d %-14s", revolutions, "revs left"); 00883 00884 sensors._right.read(); // easiest way to reset the heading counter 00885 00886 while (revolutions > 0) { 00887 int encoder; 00888 00889 if (keypad.pressed) { 00890 keypad.pressed = false; 00891 break; 00892 } 00893 00894 // wait for state change 00895 while ((encoder = sensors._right.read()) == 0) { 00896 if (keypad.pressed) { 00897 keypad.pressed = false; 00898 break; 00899 } 00900 } 00901 heading += 2*encoder; // encoder has resolution of 2 degrees 00902 if (heading >= 360) heading -= 360; 00903 00904 // when index is 1, reset the heading and decrement revolution counter 00905 // make sure we don't detect the index mark until after the first several 00906 // encoder pulses. Index is active low 00907 if ((leftCount += sensors._left.read()) > 1) { 00908 // check for error in heading? 00909 leftCount = 0; 00910 revolutions--; 00911 fprintf(stdout, ">>>>> %d left\n", revolutions); // we sense the rising and falling of the index so /2 00912 lcd.pos(0,1); 00913 lcd.printf("%1d %-14s", revolutions, "revs left"); 00914 } 00915 00916 float heading2d = 180 * atan2((float) sensors.mag[1], (float) sensors.mag[0]) / PI; 00917 // Print out data 00918 //getRawMag(m); 00919 fprintf(stdout, "%d %.4f\n", heading, heading2d); 00920 00921 // int t1=t.read_us(); 00922 if (fp) fprintf(fp, "%d, %d, %.2f, %.4f, %.4f, %.4f\n", 00923 timer.read_ms(), heading, heading2d, sensors.mag[0], sensors.mag[1], sensors.mag[2]); 00924 // int t2=t.read_us(); 00925 // fprintf(stdout, "dt=%d\n", t2-t1); 00926 } 00927 if (fp) { 00928 fclose(fp); 00929 lcd.pos(0,1); 00930 lcd.printf(LCD_FMT, "Done. Saved."); 00931 fprintf(stdout, "Data collection complete.\n"); 00932 wait(2); 00933 } 00934 00935 keypad.pressed = false; 00936 00937 return 0; 00938 } 00939 00940 void servoCalibrate() 00941 { 00942 } 00943 00944 void bridgeRecv() 00945 { 00946 while (dev && dev->readable()) { 00947 pc.putc(dev->getc()); 00948 } 00949 } 00950 00951 void serialBridge(Serial &serial) 00952 { 00953 char x; 00954 int count = 0; 00955 bool done=false; 00956 00957 fprintf(stdout, "\nEntering serial bridge in 2 seconds, +++ to escape\n\n"); 00958 //gps.setNmeaMessages(true, true, true, false, true, false); // enable GGA, GSA, GSV, RMC 00959 gps.setNmeaMessages(true, false, false, false, true, false); // enable only GGA, RMC 00960 wait(2.0); 00961 //dev = &gps; 00962 serial.attach(NULL, Serial::RxIrq); 00963 while (!done) { 00964 if (pc.readable()) { 00965 x = pc.getc(); 00966 // escape sequence 00967 if (x == '+') { 00968 if (++count >= 3) done=true; 00969 } else { 00970 count = 0; 00971 } 00972 serial.putc(x); 00973 } 00974 if (serial.readable()) { 00975 pc.putc(serial.getc()); 00976 } 00977 } 00978 } 00979 00980 /* to be called from panel menu 00981 */ 00982 int instrumentCheck(void) { 00983 displayData(INSTRUMENT_CHECK); 00984 return 0; 00985 } 00986 00987 /* Display data 00988 * mode determines the type of data and format 00989 * INSTRUMENT_CHECK : display readings of various instruments 00990 * AHRS_VISUALIZATION : display data for use by AHRS python visualization script 00991 */ 00992 00993 void displayData(int mode) 00994 { 00995 bool done = false; 00996 00997 lcd.clear(); 00998 00999 // Init GPS 01000 gps.setNmeaMessages(true, false, false, false, true, false); // enable GGA, GSA, RMC 01001 gps.serial.attach(gpsRecv, Serial::RxIrq); 01002 gps.nmea.reset_ready(); 01003 01004 keypad.pressed = false; 01005 01006 timer.reset(); 01007 timer.start(); 01008 01009 fprintf(stdout, "press e to exit\n"); 01010 while (!done) { 01011 int millis = timer.read_ms(); 01012 01013 if (keypad.pressed) { 01014 keypad.pressed = false; 01015 done=true; 01016 } 01017 01018 while (pc.readable()) { 01019 if (pc.getc() == 'e') { 01020 done = true; 01021 break; 01022 } 01023 } 01024 01025 /* 01026 if (mode == AHRS_VISUALIZATION && (millis % 100) == 0) { 01027 01028 fprintf(stdout, "!ANG:%.1f,%.1f,%.1f\r\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); 01029 01030 } else */ 01031 01032 if (mode == INSTRUMENT_CHECK) { 01033 01034 if ((millis % 1000) == 0) { 01035 01036 fprintf(stdout, "update() time = %.3f msec\n", getUpdateTime() / 1000.0); 01037 fprintf(stdout, "Rangers: L=%.2f R=%.2f C=%.2f", sensors.leftRanger, sensors.rightRanger, sensors.centerRanger); 01038 fprintf(stdout, "\n"); 01039 //fprintf(stdout, "ahrs.MAG_Heading=%4.1f\n", ahrs.MAG_Heading*180/PI); 01040 fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]); 01041 fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2], 01042 sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] )); 01043 fprintf(stdout, "g=(%4d, %4d, %4d) %d\n", sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp); 01044 fprintf(stdout, "gc=(%.1f, %.1f, %.1f)\n", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); 01045 fprintf(stdout, "a=(%5d, %5d, %5d)\n", sensors.a[0], sensors.a[1], sensors.a[2]); 01046 fprintf(stdout, "estHdg=%.2f\n", state[inState].estHeading); 01047 //fprintf(stdout, "roll=%.2f pitch=%.2f yaw=%.2f\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); 01048 fprintf(stdout, "speed: left=%.3f right=%.3f\n", sensors.lrEncSpeed, sensors.rrEncSpeed); 01049 fprintf(stdout, "gps=(%.6f, %.6f, %.1f, %.1f, %.1f, %d)\n", gps_here.latitude(), gps_here.longitude(), 01050 gps.nmea.f_course(), gps.nmea.f_speed_mps(), gps.hdop, gps.nmea.sat_count()); 01051 fprintf(stdout, "v=%.2f a=%.3f\n", sensors.voltage, sensors.current); 01052 fprintf(stdout, "\n"); 01053 01054 } 01055 01056 if ((millis % 3000) == 0) { 01057 01058 lcd.pos(0,1); 01059 //lcd.printf("H=%4.1f ", ahrs.MAG_Heading*180/PI); 01060 //wait(0.1); 01061 lcd.pos(0,2); 01062 lcd.printf("G=%4.1f,%4.1f,%4.1f ", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); 01063 wait(0.1); 01064 lcd.pos(0,3); 01065 lcd.printf("La=%11.6f HD=%1.1f ", gps_here.latitude(), gps.hdop); 01066 wait(0.1); 01067 lcd.pos(0,4); 01068 lcd.printf("Lo=%11.6f Sat=%-2d ", gps_here.longitude(), gps.nmea.sat_count()); 01069 wait(0.1); 01070 lcd.pos(0,5); 01071 lcd.printf("V=%5.2f A=%5.3f ", sensors.voltage, sensors.current); 01072 01073 } 01074 } 01075 01076 } // while !done 01077 // clear input buffer 01078 while (pc.readable()) pc.getc(); 01079 lcd.clear(); 01080 ahrsStatus = 0; 01081 gpsStatus = 0; 01082 } 01083 01084 01085 // TODO: 3 move Mavlink into main (non-interrupt) loop along with logging 01086 // possibly also buffered if necessary 01087 01088 void mavlinkMode() { 01089 uint8_t system_type = MAV_FIXED_WING; 01090 uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC; 01091 //int count = 0; 01092 bool done = false; 01093 01094 mavlink_system.sysid = 100; // System ID, 1-255 01095 mavlink_system.compid = 200; // Component/Subsystem ID, 1-255 01096 01097 //mavlink_attitude_t mav_attitude; 01098 //mavlink_sys_status_t mav_stat; 01099 mavlink_vfr_hud_t mav_hud; 01100 01101 //mav_stat.mode = MAV_MODE_MANUAL; 01102 //mav_stat.status = MAV_STATE_STANDBY; 01103 //mav_stat.vbat = 8400; 01104 //mav_stat.battery_remaining = 1000; 01105 01106 mav_hud.airspeed = 0.0; 01107 mav_hud.groundspeed = 0.0; 01108 mav_hud.throttle = 0; 01109 01110 fprintf(stdout, "Entering MAVlink mode; reset the MCU to exit\n\n"); 01111 01112 gps.gsvMessage(true); 01113 gps.gsaMessage(true); 01114 gps.serial.attach(gpsRecv, Serial::RxIrq); 01115 01116 while (done == false) { 01117 01118 if (keypad.pressed == true) { // && started 01119 keypad.pressed = false; 01120 done = true; 01121 } 01122 01123 int millis = timer.read_ms(); 01124 01125 if ((millis % 1000) == 0) { 01126 01127 mav_hud.heading = 0.0; //ahrs.parser.yaw; 01128 01129 mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, 01130 0.0, //ToDeg(ahrs.roll), 01131 0.0, //ToDeg(ahrs.pitch), 01132 0.0, //ToDeg(ahrs.yaw), TODO: 3 fix this to use current estimate 01133 0.0, // rollspeed 01134 0.0, // pitchspeed 01135 0.0 // yawspeed 01136 ); 01137 01138 mav_hud.groundspeed = sensors.encSpeed; 01139 mav_hud.groundspeed *= 2.237; // convert to mph 01140 //mav_hud.heading = compassHeading(); 01141 01142 mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, 01143 mav_hud.groundspeed, 01144 mav_hud.groundspeed, 01145 mav_hud.heading, 01146 mav_hud.throttle, 01147 0.0, // altitude 01148 0.0 // climb 01149 ); 01150 01151 mavlink_msg_heartbeat_send(MAVLINK_COMM_0, system_type, autopilot_type); 01152 mavlink_msg_sys_status_send(MAVLINK_COMM_0, 01153 MAV_MODE_MANUAL, 01154 MAV_NAV_GROUNDED, 01155 MAV_STATE_STANDBY, 01156 0.0, // load 01157 (uint16_t) (sensors.voltage * 1000), 01158 1000, // TODO: 3 fix batt remaining 01159 0 // packet drop 01160 ); 01161 01162 wait(0.001); 01163 } // millis % 1000 01164 01165 if (gps.nmea.rmc_ready() && gps.nmea.gga_ready()) { 01166 char gpsdate[32], gpstime[32]; 01167 01168 gps.process(gps_here, gpsdate, gpstime); 01169 gpsStatus = (gps.hdop > 0.0 && gps.hdop < 3.0) ? 1 : 0; 01170 01171 mavlink_msg_gps_raw_send(MAVLINK_COMM_0, millis*1000, 3, 01172 gps_here.latitude(), 01173 gps_here.longitude(), 01174 0.0, // altitude 01175 gps.nmea.f_hdop()*100.0, 01176 0.0, // VDOP 01177 mav_hud.groundspeed, 01178 mav_hud.heading 01179 ); 01180 01181 mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.nmea.sat_count(), 0, 0, 0, 0, 0); 01182 01183 gps.nmea.reset_ready(); 01184 01185 } //gps 01186 01187 //mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, mav_attitude.roll, mav_attitude.pitch, mav_attitude.yaw, 0.0, 0.0, 0.0); 01188 //mavlink_msg_sys_status_send(MAVLINK_COMM_0, mav_stat.mode, mav_stat.nav_mode, mav_stat.status, mav_stat.load, 01189 // mav_stat.vbat, mav_stat.battery_remaining, 0); 01190 } 01191 01192 gps.serial.attach(NULL, Serial::RxIrq); 01193 gps.gsvMessage(false); 01194 gps.gsaMessage(false); 01195 fprintf(stdout, "\n"); 01196 01197 return; 01198 } 01199 01200 01201 int setBacklight(void) { 01202 Menu bmenu; 01203 bool done = false; 01204 bool printUpdate = false; 01205 static int backlight=100; 01206 01207 lcd.pos(0,0); 01208 lcd.printf(LCD_FMT, ">> Backlight"); 01209 01210 while (!done) { 01211 if (keypad.pressed) { 01212 keypad.pressed = false; 01213 printUpdate = true; 01214 switch (keypad.which) { 01215 case NEXT_BUTTON: 01216 backlight+=5; 01217 if (backlight > 100) backlight = 100; 01218 lcd.backlight(backlight); 01219 break; 01220 case PREV_BUTTON: 01221 backlight-=5; 01222 if (backlight < 0) backlight = 0; 01223 lcd.backlight(backlight); 01224 break; 01225 case SELECT_BUTTON: 01226 done = true; 01227 break; 01228 } 01229 } 01230 if (printUpdate) { 01231 printUpdate = false; 01232 lcd.pos(0,1); 01233 lcd.printf("%3d%%%-16s", backlight, ""); 01234 } 01235 } 01236 01237 return 0; 01238 } 01239 01240 int reverseScreen(void) { 01241 lcd.reverseMode(); 01242 01243 return 0; 01244 } 01245 01246 /////////////////////////////////////////////////////////////////////////////////////////////////////// 01247 // ADC CONVERSION FUNCTIONS 01248 /////////////////////////////////////////////////////////////////////////////////////////////////////// 01249 01250 // returns distance in m for Sharp GP2YOA710K0F 01251 // to get m and b, I wrote down volt vs. dist by eyeballin the 01252 // datasheet chart plot. Then used Excel to do linear regression 01253 // 01254 float irDistance(unsigned int adc) 01255 { 01256 float b = 1.0934; // Intercept from Excel 01257 float m = 1.4088; // Slope from Excel 01258 01259 return m / (((float) adc) * 4.95/4096 - b); 01260 }
Generated on Tue Jul 12 2022 14:09:25 by
1.7.2