Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
main.cpp
- Committer:
- shimniok
- Date:
- 2012-06-20
- Revision:
- 0:826c6171fc1b
File content as of revision 0:826c6171fc1b:
/** Code for "Data Bus" UGV entry for Sparkfun AVC 2012 * http://www.bot-thoughts.com/ */ /////////////////////////////////////////////////////////////////////////////////////////////////////// // INCLUDES /////////////////////////////////////////////////////////////////////////////////////////////////////// #include <stdio.h> #include <math.h> #include "mbed.h" #include "globals.h" #include "Config.h" #include "Buttons.h" #include "Menu.h" //#include "lcd.h" #include "SerialGraphicLCD.h" #include "Bargraph.h" #include "GPSStatus.h" #include "logging.h" #include "shell.h" #include "Sensors.h" //#include "DCM.h" //#include "dcm_matrix.h" #include "kalman.h" #include "GPS.h" #include "Camera.h" #include "PinDetect.h" #include "Actuators.h" #include "IncrementalEncoder.h" #include "Steering.h" #include "Schedule.h" #include "GeoPosition.h" #include "Mapping.h" #include "SimpleFilter.h" #include "Beep.h" #include "util.h" #include "MAVlink/include/mavlink_bridge.h" #include "updater.h" #define LCD_FMT "%-20s" // used to fill a single line on the LCD screen /////////////////////////////////////////////////////////////////////////////////////////////////////// // DEFINES /////////////////////////////////////////////////////////////////////////////////////////////////////// #define absf(x) (x *= (x < 0.0) ? -1 : 1) #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position #define UPDATE_PERIOD 0.010 // update period in s #define UPDATE_PERIOD_MS 10 // update period in ms // Driver configuration parameters #define SONARLEFT_CHAN 0 #define SONARRIGHT_CHAN 1 #define IRLEFT_CHAN 2 #define IRRIGHT_CHAN 3 #define TEMP_CHAN 4 #define GYRO_CHAN 5 // Chassis specific parameters #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375m circumference #define WHEELBASE 0.290 #define TRACK 0.280 #define INSTRUMENT_CHECK 0 #define AHRS_VISUALIZATION 1 #define DISPLAY_PANEL 2 /////////////////////////////////////////////////////////////////////////////////////////////////////// // GLOBAL VARIABLES /////////////////////////////////////////////////////////////////////////////////////////////////////// // OUTPUT DigitalOut confStatus(LED1); // Config file status LED DigitalOut logStatus(LED2); // Log file status LED DigitalOut gpsStatus(LED3); // GPS fix status LED DigitalOut ahrsStatus(LED4); // AHRS status LED //DigitalOut sonarStart(p18); // Sends signal to start sonar array pings Beep speaker(p24); // Piezo speaker // INPUT Menu menu; Buttons keypad; // VEHICLE Steering steerCalc(TRACK, WHEELBASE); // steering calculator // COMM Serial pc(USBTX, USBRX); // PC usb communications SerialGraphicLCD lcd(p17, p18, SD_FW); // Graphic LCD with summoningdark firmware //Serial *debug = &pc; // SENSORS Sensors sensors; // Abstraction of sensor drivers //DCM ahrs; // ArduPilot/MatrixPilot AHRS Serial *dev; // For use with bridge GPS gps(p26, p25, VENUS); // gps FILE *camlog; // Camera log // Configuration Config config; // Persistent configuration // Course Waypoints // Sensor Calibration // etc. // Timing Timer timer; // For main loop scheduling Ticker sched; // scheduler for interrupt driven routines // Overall system state (used for logging but also convenient for general use SystemState state[SSBUF]; // system state for logging, FIFO buffer unsigned char inState; // FIFO items enter in here unsigned char outState; // FIFO items leave out here bool ssBufOverrun = false; // GPS Variables unsigned long age = 0; // gps fix age // schedule for LED warning flasher Schedule blink; // Estimation & Navigation Variables GeoPosition dr_here; // Estimated position based on estimated heading GeoPosition gps_here; // current gps position Mapping mapper; /////////////////////////////////////////////////////////////////////////////////////////////////////// // FUNCTION DEFINITIONS /////////////////////////////////////////////////////////////////////////////////////////////////////// void initFlasher(void); void initDR(void); int autonomousMode(void); void mavlinkMode(void); void servoCalibrate(void); void serialBridge(Serial &gps); int instrumentCheck(void); void displayData(int mode); int compassCalibrate(void); int compassSwing(void); int gyroSwing(void); int setBacklight(void); int reverseScreen(void); float gyroRate(unsigned int adc); float sonarDistance(unsigned int adc); float irDistance(unsigned int adc); float getVoltage(void); extern "C" void mbed_reset(); extern unsigned int matrix_error; // If we don't close the log file, when we restart, all the written data // will be lost. So we have to use a button to force mbed to close the // file and preserve the data. // int dummy(void) { return 0; } // TODO: 3 move to GPS module /* GPS serial interrupt handler */ void gpsRecv() { while (gps.serial.readable()) { gpsStatus = !gpsStatus; gps.nmea.encode(gps.serial.getc()); gpsStatus = !gpsStatus; } } int resetMe() { mbed_reset(); return 0; } #define DISPLAY_CLEAR 0x01 #define DISPLAY_SET_POS 0x08 int main() { // Send data back to the PC pc.baud(115200); lcd.baud(115200); lcd.printf("test\n"); // hopefully force 115200 on powerup lcd.clear(); wait(0.3); lcd.printf("Data Bus mAGV V2"); fprintf(stdout, "Data Bus mAGV Control System\n"); fprintf(stdout, "Initialization...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Initializing"); wait(0.5); gps.setUpdateRate(10); // Initialize status LEDs ahrsStatus = 0; gpsStatus = 0; logStatus = 0; confStatus = 0; //ahrs.G_Dt = UPDATE_PERIOD; fprintf(stdout, "Loading configuration...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Load config"); wait(0.5); if (config.load()) // Load various configurable parameters, e.g., waypoints, declination, etc. confStatus = 1; // Something here is jacking up the I2C stuff initSteering(); initThrottle(); // initFlasher(); // Initialize autonomous mode flasher sensors.Compass_Calibrate(config.magOffset, config.magScale); pc.printf("Declination: %.1f\n", config.declination); pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", config.escZero, config.escMax, config.topSpeed, config.turnSpeed, config.speedKp, config.speedKi, config.speedKd); pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n", config.steerZero, config.steerGain, config.steerGainAngle); // Convert lat/lon waypoints to cartesian mapper.init(config.wptCount, config.wpt); for (int w = 0; w < MAXWPT && w < config.wptCount; w++) { mapper.geoToCart(config.wpt[w], &(config.cwpt[w])); pc.printf("Waypoint #%d (%.4f, %.4f) lat: %.6f lon: %.6f\n", w, config.cwpt[w]._x, config.cwpt[w]._y, config.wpt[w].latitude(), config.wpt[w].longitude()); } // TODO: 3 print mag and gyro calibrations lcd.pos(0,1); lcd.printf(LCD_FMT, "GPS configuration "); gps.setType(config.gpsType); gps.setBaud(config.gpsBaud); fprintf(stdout, "GPS config: type=%d baud=%d\n", config.gpsType, config.gpsBaud); lcd.pos(0,1); lcd.printf(LCD_FMT, "Nav configuration "); steerCalc.setIntercept(config.interceptDist); // Setup steering calculator based on intercept distance pc.printf("Intercept distance: %.1f\n", config.interceptDist); pc.printf("Waypoint distance: %.1f\n", config.waypointDist); pc.printf("Brake distance: %.1f\n", config.brakeDist); pc.printf("Min turn radius: %.3f\n", config.minRadius); fprintf(stdout, "Calculating offsets...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Offset calculation "); wait(0.5); // TODO: 3 Really need to give the gyro more time to settle sensors.Calculate_Offsets(); fprintf(stdout, "Starting GPS...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Start GPS "); wait(0.5); // TODO: 3 move this to GPS module gps.serial.attach(gpsRecv, Serial::RxIrq); // TODO: 3 enable and process GSV as bar graph //gps.gsvMessage(false); //gps.gsaMessage(true); fprintf(stdout, "Starting Scheduler...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Start scheduler "); wait(0.5); // Startup sensor/AHRS ticker; update every 10ms = 100hz restartNav(); sched.attach(&update, UPDATE_PERIOD); /* fprintf(stdout, "Starting Camera...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Start Camera "); wait(0.5); cam.start(); */ // Let's try setting serial IRQs lower and see if that alleviates issues with I2C reads and AHRS reads //NVIC_SetPriority(UART0_IRQn, 1); // USB //NVIC_SetPriority(UART1_IRQn, 2); // GPS p25,p26 //NVIC_SetPriority(UART3_IRQn, 3); // LCD p17,p18 // Insert menu system here w/ timeout //bool autoBoot=false; //speaker.beep(3000.0, 0.2); // non-blocking keypad.init(); // Initialize LCD graphics Bargraph::lcd = &lcd; Bargraph v(1, 40, 15, 'V'); v.calibrate(6.3, 8.4); Bargraph a(11, 40, 15, 'A'); a.calibrate(0, 15.0); Bargraph g1(21, 40, 15, 'G'); g1.calibrate(0, 10); Bargraph g2(31, 40, 15, 'H'); g2.calibrate(4.0, 0.8); //GPSStatus g2(21, 12); //GPSStatus::lcd = &lcd; // Setup LCD Input Menu menu.add("Auto mode", &autonomousMode); menu.add("Instruments", &instrumentCheck); menu.add("Calibrate", &compassCalibrate); menu.add("Compass Swing", &compassSwing); menu.add("Gyro Calib", &gyroSwing); //menu.sdd("Reload Config", &loadConfig); menu.add("Backlight", &setBacklight); menu.add("Reverse", &reverseScreen); menu.add("Reset", &resetMe); char cmd; bool printMenu = true; bool printLCDMenu = true; timer.start(); timer.reset(); int thisUpdate = timer.read_ms(); int nextUpdate = thisUpdate; //int hdgUpdate = nextUpdate; while (1) { /* if (timer.read_ms() > hdgUpdate) { fprintf(stdout, "He=%.3f %.5f\n", kfGetX(0), kfGetX(1)); hdgUpdate = timer.read_ms() + 100; }*/ if ((thisUpdate = timer.read_ms()) > nextUpdate) { //fprintf(stdout, "Updating...\n"); v.update(sensors.voltage); a.update(sensors.current); g1.update((float) gps.nmea.sat_count()); g2.update(gps.hdop); lcd.posXY(60, 22); lcd.printf("%.2f", state[inState].rightRanger); lcd.posXY(60, 32); lcd.printf("%.2f", state[inState].leftRanger); lcd.posXY(60, 42); lcd.printf("%5.1f", state[inState].estHeading); lcd.posXY(60, 52); lcd.printf("%.3f", state[inState].gpsCourse); nextUpdate = thisUpdate + 2000; // TODO: 3 address integer overflow // TODO: 3 display scheduler() timing } if (keypad.pressed) { keypad.pressed = false; printLCDMenu = true; //causes issue with servo library //speaker.beep(3000.0, 0.1); // non-blocking switch (keypad.which) { case NEXT_BUTTON: menu.next(); break; case PREV_BUTTON: menu.prev(); break; case SELECT_BUTTON: lcd.pos(0,0); lcd.printf(">>%-14s", menu.getItemName()); menu.select(); printMenu = true; break; default: printLCDMenu = false; break; }//switch keypad.pressed = false; }// if (keypad.pressed) if (printLCDMenu) { lcd.pos(0,0); lcd.printf("< %-14s >", menu.getItemName()); lcd.pos(0,1); lcd.printf(LCD_FMT, "Ready."); lcd.posXY(50, 22); lcd.printf("R"); lcd.rect(58, 20, 98, 30, true); wait(0.01); lcd.posXY(50, 32); lcd.printf("L"); lcd.rect(58, 30, 98, 40, true); wait(0.01); lcd.posXY(50, 42); lcd.printf("H"); lcd.rect(58, 40, 98, 50, true); wait(0.01); lcd.posXY(44,52); lcd.printf("GH"); lcd.rect(58, 50, 98, 60, true); v.init(); a.init(); g1.init(); g2.init(); printLCDMenu = false; } /* if (autoBoot) { autoBoot = false; cmd = 'a'; } else {*/ if (printMenu) { int i=0; fprintf(stdout, "\n==============\nData Bus Menu\n==============\n"); fprintf(stdout, "%d) Autonomous mode\n", i++); fprintf(stdout, "%d) Bridge serial to GPS\n", i++); fprintf(stdout, "%d) Calibrate compass\n", i++); fprintf(stdout, "%d) Swing compass\n", i++); fprintf(stdout, "%d) Gyro calibrate\n", i++); fprintf(stdout, "%d) Instrument check\n", i++); fprintf(stdout, "%d) Display AHRS\n", i++); fprintf(stdout, "%d) Mavlink mode\n", i++); fprintf(stdout, "%d) Shell\n", i++); fprintf(stdout, "R) Reset\n"); fprintf(stdout, "\nSelect from the above: "); fflush(stdout); printMenu = false; } // Basic functional architecture // SENSORS -> FILTERS -> AHRS -> POSITION -> NAVIGATION -> CONTROL | INPUT/OUTPUT | LOGGING // SENSORS (for now) are polled out of AHRS via interrupt every 10ms // // no FILTERing in place right now // if we filter too heavily we get lag. At 30mph = 14m/s a sensor lag // of only 10ms means the estimate is 140cm behind the robot // // POSITION and NAVIGATION should probably always be running // log file can have different entry type per module, to be demultiplexed on the PC // // Autonomous mode engages CONTROL outputs // // I/O mode could be one of: MAVlink, serial bridge (gps), sensor check, shell, log to serial // Or maybe shell should be the main control for different output modes // // LOGGING can be turned on or off, probably best to start with it engaged // and then disable from user panel or when navigation is ended if (pc.readable()) { cmd = pc.getc(); fprintf(stdout, "%c\n", cmd); printMenu = true; printLCDMenu = true; switch (cmd) { case 'R' : resetMe(); break; case '0' : lcd.pos(0,0); lcd.printf(">>%-14s", menu.getItemName(0)); autonomousMode(); break; case '1' : lcd.pos(0,0); lcd.printf(">>%-14s", "Serial bridge"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Standby."); gps.gsvMessage(true); gps.gsaMessage(true); serialBridge(gps.serial); gps.gsvMessage(false); gps.gsaMessage(false); break; case '2' : lcd.pos(0,0); lcd.printf(">>%-14s", menu.getItemName(1)); compassCalibrate(); break; case '3' : lcd.pos(0,0); lcd.printf(">>%-14s", menu.getItemName(2)); compassSwing(); break; case '4' : lcd.pos(0,0); lcd.printf(">>%-14s", menu.getItemName(2)); gyroSwing(); break; case '5' : lcd.pos(0,0); lcd.printf(">>%-14s", "Instruments"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Standby."); displayData(INSTRUMENT_CHECK); break; case '6' : lcd.pos(0,0); lcd.printf(">>%-14s", "AHRS Visual'n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Standby."); displayData(AHRS_VISUALIZATION); break; case '7' : lcd.pos(0,0); lcd.printf(">>%-14s", "Mavlink mode"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Standby."); mavlinkMode(); break; case '8' : lcd.pos(0,0); lcd.printf(">>%-14s", "Shell"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Standby."); shell(); break; default : break; } // switch } // if (pc.readable()) } // while } /////////////////////////////////////////////////////////////////////////////////////////////////////// // INITIALIZATION ROUTINES /////////////////////////////////////////////////////////////////////////////////////////////////////// void initFlasher() { // Set up flasher schedule; 3 flashes every 80ms // for 80ms total, with a 9x80ms period blink.max(9); blink.scale(80); blink.mode(Schedule::repeat); blink.set(0, 1); blink.set(2, 1); blink.set(4, 1); } /////////////////////////////////////////////////////////////////////////////////////////////////////// // OPERATIONAL MODE FUNCTIONS /////////////////////////////////////////////////////////////////////////////////////////////////////// int autonomousMode() { bool goGoGo = false; // signal to start moving bool navDone; // signal that we're done navigating extern int tSensor, tGPS, tAHRS, tLog; // TODO: 3 move to main? // Navigation goGoGo = false; navDone = false; keypad.pressed = false; //bool started = false; // flag to indicate robot has exceeded min speed. if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d wait(0.2); gps.setNmeaMessages(true, true, false, false, true, false); // enable GGA, GSA, RMC gps.serial.attach(gpsRecv, Serial::RxIrq); lcd.pos(0,1); lcd.printf(LCD_FMT, "Select starts."); wait(1.0); timer.reset(); timer.start(); wait(0.1); // Initialize logging buffer // Needs to happen after we've reset the millisecond timer and after // the schedHandler() fires off at least once more with the new time inState = outState = 0; // Tell the navigation / position estimation stuff to reset to starting waypoint restartNav(); // Main loop // while(navDone == false) { ////////////////////////////////////////////////////////////////////////////// // USER INPUT ////////////////////////////////////////////////////////////////////////////// // Button state machine // if we've not started going, button starts us // if we have started going, button stops us // but only if we've released it first // // set throttle only if goGoGo set if (goGoGo) { /** acceleration curve */ /* if (go.ticked(timer.read_ms())) { throttle = go.get() / 1000.0; //fprintf(stdout, "throttle: %0.3f\n", throttle.read()); } */ // TODO: 1 Add additional condition of travel for N meters before // the HALT button is armed if (keypad.pressed == true) { // && started fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> HALT\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "HALT."); navDone = true; goGoGo = false; keypad.pressed = false; endRun(); } } else { if (keypad.pressed == true) { fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "GO GO GO!"); goGoGo = true; keypad.pressed = false; //restartNav(); beginRun(); // Doing this for collecting step response, hopefully an S curve... we'll see. //throttle = config.escMax; // TODO: 2 Improve encapsulation of the scheduler // TODO: 2 can we do something clever with GPS position estimate since we know where we're starting? // E.g. if dist to wpt0 < x then initialize to wpt0 else use gps } } // Are we at the last waypoint? // if (state[inState].nextWaypoint == config.wptCount) { fprintf(stdout, "Arrived at final destination. Done.\n"); //causes issue with servo library //speaker.beep(3000.0, 1.0); // non-blocking lcd.pos(0,1); lcd.printf(LCD_FMT, "Arrived. Done."); navDone = true; endRun(); } ////////////////////////////////////////////////////////////////////////////// // LOGGING ////////////////////////////////////////////////////////////////////////////// // sensor reads are happening in the schedHandler(); // Are there more items to come out of the log buffer? // Since this could take anywhere from a few hundred usec to // 150ms, we run it opportunistically and use a buffer. That way // the sensor updates, calculation, and control can continue to happen if (outState != inState) { logStatus = !logStatus; // log indicator LED //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState); if (ssBufOverrun) { fprintf(stdout, ">>> SystemState Buffer Overrun Condition\n"); ssBufOverrun = false; } // do we need to disable interrupts briefly to prevent a race // condition with schedHandler() ? int out=outState; // in case we're interrupted this 'should' be atomic outState++; // increment outState &= SSBUF; // wrap logData( state[out] ); // log state data to file logStatus = !logStatus; // log indicator LED //fprintf(stdout, "Time Stats\n----------\nSensors: %d\nGPS: %d\nAHRS: %d\nLog: %d\n----------\nTotal: %d", // tSensor, tGPS, tAHRS, tLog, tSensor+tGPS+tAHRS+tLog); } } // while closeLogfile(); logStatus = 0; fprintf(stdout, "Completed, file saved.\n"); wait(2); // wait from last printout lcd.pos(0,1); lcd.printf(LCD_FMT, "Done. Saved."); wait(2); ahrsStatus = 0; gpsStatus = 0; //confStatus = 0; //flasher = 0; gps.gsaMessage(false); gps.gsvMessage(false); return 0; } // autonomousMode /////////////////////////////////////////////////////////////////////////////////////////////////////// // UTILITY FUNCTIONS /////////////////////////////////////////////////////////////////////////////////////////////////////// int compassCalibrate() { bool done=false; int m[3]; FILE *fp; fprintf(stdout, "Entering compass calibration in 2 seconds.\nLaunch _3DScatter Processing app now... type e to exit\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Starting..."); fp = openlog("cal"); wait(2); lcd.pos(0,1); lcd.printf(LCD_FMT, "Select exits"); timer.reset(); timer.start(); while (!done) { if (keypad.pressed) { keypad.pressed = false; done = true; } while (pc.readable()) { if (pc.getc() == 'e') { done = true; break; } } int millis = timer.read_ms(); if ((millis % 100) == 0) { sensors.getRawMag(m); // Correction // Let's see how our ellipsoid looks after scaling and offset /* float mag[3]; mag[0] = ((float) m[0] - M_OFFSET_X) * 0.5 / M_SCALE_X; mag[1] = ((float) m[1] - M_OFFSET_Y) * 0.5 / M_SCALE_Y; mag[2] = ((float) m[2] - M_OFFSET_Z) * 0.5 / M_SCALE_Z; */ bool skipIt = false; for (int i=0; i < 3; i++) { if (abs(m[i]) > 1024) skipIt = true; } if (!skipIt) { fprintf(stdout, "%c%d %d %d \r\n", 0xDE, m[0], m[1], m[2]); fprintf(fp, "%d, %d, %d\n", m[0], m[1], m[2]); } } } if (fp) { fclose(fp); lcd.pos(0,1); lcd.printf(LCD_FMT, "Done. Saved."); wait(2); } return 0; } // Gather gyro data using turntable equipped with dual channel // encoder. Use onboard wheel encoder system. Left channel // is the index (0 degree) mark, while the right channel // is the incremental encoder. Can then compare gyro integrated // heading with machine-reported heading // // Note: some of this code is identical to the compassSwing() code. // int gyroSwing() { FILE *fp; // Timing is pretty critical so just in case, disable serial processing from GPS gps.serial.attach(NULL, Serial::RxIrq); fprintf(stdout, "Entering gyro swing...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Starting..."); wait(2); fp = openlog("gy"); wait(2); lcd.pos(0,1); lcd.printf(LCD_FMT, "Begin. Select exits."); fprintf(stdout, "Begin clockwise rotation, varying rpm... press select to exit\n"); timer.reset(); timer.start(); sensors.rightTotal = 0; // reset total sensors._right.read(); // easiest way to reset the heading counter while (1) { if (keypad.pressed) { keypad.pressed = false; break; } // Print out data // fprintf(stdout, "%d,%d,%d,%d,%d\n", timer.read_ms(), heading, sensors.g[0], sensors.g[1], sensors.g[2]); // sensors.rightTotal gives us each tick of the machine, multiply by 2 for cumulative heading, which is easiest // to compare with cumulative integration of gyro (rather than dealing with 0-360 degree range and modulus and whatnot 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); wait(0.200); } if (fp) { fclose(fp); lcd.pos(0,1); lcd.printf(LCD_FMT, "Done. Saved."); fprintf(stdout, "Data collection complete.\n"); wait(2); } keypad.pressed = false; return 0; } // Swing compass using turntable equipped with dual channel // encoder. Use onboard wheel encoder system. Left channel // is the index (0 degree) mark, while the right channel // is the incremental encoder. // // Note: much of this code is identical to the gyroSwing() code. // int compassSwing() { int revolutions=5; int heading=0; int leftCount = 0; FILE *fp; // left is index track // right is encoder track fprintf(stdout, "Entering compass swing...\n"); lcd.pos(0,1); lcd.printf(LCD_FMT, "Starting..."); wait(2); fp = openlog("sw"); wait(2); lcd.pos(0,1); lcd.printf(LCD_FMT, "Ok. Begin."); fprintf(stdout, "Begin clockwise rotation... exit after %d revolutions\n", revolutions); timer.reset(); timer.start(); // wait for index to change while ((leftCount += sensors._left.read()) < 2) { if (keypad.pressed) { keypad.pressed = false; break; } } fprintf(stdout, ">>>> Index detected. Starting data collection\n"); leftCount = 0; lcd.pos(0,1); lcd.printf("%1d %-14s", revolutions, "revs left"); sensors._right.read(); // easiest way to reset the heading counter while (revolutions > 0) { int encoder; if (keypad.pressed) { keypad.pressed = false; break; } // wait for state change while ((encoder = sensors._right.read()) == 0) { if (keypad.pressed) { keypad.pressed = false; break; } } heading += 2*encoder; // encoder has resolution of 2 degrees if (heading >= 360) heading -= 360; // when index is 1, reset the heading and decrement revolution counter // make sure we don't detect the index mark until after the first several // encoder pulses. Index is active low if ((leftCount += sensors._left.read()) > 1) { // check for error in heading? leftCount = 0; revolutions--; fprintf(stdout, ">>>>> %d left\n", revolutions); // we sense the rising and falling of the index so /2 lcd.pos(0,1); lcd.printf("%1d %-14s", revolutions, "revs left"); } float heading2d = 180 * atan2((float) sensors.mag[1], (float) sensors.mag[0]) / PI; // Print out data //getRawMag(m); fprintf(stdout, "%d %.4f\n", heading, heading2d); // int t1=t.read_us(); if (fp) fprintf(fp, "%d, %d, %.2f, %.4f, %.4f, %.4f\n", timer.read_ms(), heading, heading2d, sensors.mag[0], sensors.mag[1], sensors.mag[2]); // int t2=t.read_us(); // fprintf(stdout, "dt=%d\n", t2-t1); } if (fp) { fclose(fp); lcd.pos(0,1); lcd.printf(LCD_FMT, "Done. Saved."); fprintf(stdout, "Data collection complete.\n"); wait(2); } keypad.pressed = false; return 0; } void servoCalibrate() { } void bridgeRecv() { while (dev && dev->readable()) { pc.putc(dev->getc()); } } void serialBridge(Serial &serial) { char x; int count = 0; bool done=false; fprintf(stdout, "\nEntering serial bridge in 2 seconds, +++ to escape\n\n"); //gps.setNmeaMessages(true, true, true, false, true, false); // enable GGA, GSA, GSV, RMC gps.setNmeaMessages(true, false, false, false, true, false); // enable only GGA, RMC wait(2.0); //dev = &gps; serial.attach(NULL, Serial::RxIrq); while (!done) { if (pc.readable()) { x = pc.getc(); // escape sequence if (x == '+') { if (++count >= 3) done=true; } else { count = 0; } serial.putc(x); } if (serial.readable()) { pc.putc(serial.getc()); } } } /* to be called from panel menu */ int instrumentCheck(void) { displayData(INSTRUMENT_CHECK); return 0; } /* Display data * mode determines the type of data and format * INSTRUMENT_CHECK : display readings of various instruments * AHRS_VISUALIZATION : display data for use by AHRS python visualization script */ void displayData(int mode) { bool done = false; lcd.clear(); // Init GPS gps.setNmeaMessages(true, false, false, false, true, false); // enable GGA, GSA, RMC gps.serial.attach(gpsRecv, Serial::RxIrq); gps.nmea.reset_ready(); keypad.pressed = false; timer.reset(); timer.start(); fprintf(stdout, "press e to exit\n"); while (!done) { int millis = timer.read_ms(); if (keypad.pressed) { keypad.pressed = false; done=true; } while (pc.readable()) { if (pc.getc() == 'e') { done = true; break; } } /* if (mode == AHRS_VISUALIZATION && (millis % 100) == 0) { fprintf(stdout, "!ANG:%.1f,%.1f,%.1f\r\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); } else */ if (mode == INSTRUMENT_CHECK) { if ((millis % 1000) == 0) { fprintf(stdout, "update() time = %.3f msec\n", getUpdateTime() / 1000.0); fprintf(stdout, "Rangers: L=%.2f R=%.2f C=%.2f", sensors.leftRanger, sensors.rightRanger, sensors.centerRanger); fprintf(stdout, "\n"); //fprintf(stdout, "ahrs.MAG_Heading=%4.1f\n", ahrs.MAG_Heading*180/PI); fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]); fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2], sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] )); fprintf(stdout, "g=(%4d, %4d, %4d) %d\n", sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp); fprintf(stdout, "gc=(%.1f, %.1f, %.1f)\n", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); fprintf(stdout, "a=(%5d, %5d, %5d)\n", sensors.a[0], sensors.a[1], sensors.a[2]); fprintf(stdout, "estHdg=%.2f\n", state[inState].estHeading); //fprintf(stdout, "roll=%.2f pitch=%.2f yaw=%.2f\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); fprintf(stdout, "speed: left=%.3f right=%.3f\n", sensors.lrEncSpeed, sensors.rrEncSpeed); fprintf(stdout, "gps=(%.6f, %.6f, %.1f, %.1f, %.1f, %d)\n", gps_here.latitude(), gps_here.longitude(), gps.nmea.f_course(), gps.nmea.f_speed_mps(), gps.hdop, gps.nmea.sat_count()); fprintf(stdout, "v=%.2f a=%.3f\n", sensors.voltage, sensors.current); fprintf(stdout, "\n"); } if ((millis % 3000) == 0) { lcd.pos(0,1); //lcd.printf("H=%4.1f ", ahrs.MAG_Heading*180/PI); //wait(0.1); lcd.pos(0,2); lcd.printf("G=%4.1f,%4.1f,%4.1f ", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); wait(0.1); lcd.pos(0,3); lcd.printf("La=%11.6f HD=%1.1f ", gps_here.latitude(), gps.hdop); wait(0.1); lcd.pos(0,4); lcd.printf("Lo=%11.6f Sat=%-2d ", gps_here.longitude(), gps.nmea.sat_count()); wait(0.1); lcd.pos(0,5); lcd.printf("V=%5.2f A=%5.3f ", sensors.voltage, sensors.current); } } } // while !done // clear input buffer while (pc.readable()) pc.getc(); lcd.clear(); ahrsStatus = 0; gpsStatus = 0; } // TODO: 3 move Mavlink into main (non-interrupt) loop along with logging // possibly also buffered if necessary void mavlinkMode() { uint8_t system_type = MAV_FIXED_WING; uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC; //int count = 0; bool done = false; mavlink_system.sysid = 100; // System ID, 1-255 mavlink_system.compid = 200; // Component/Subsystem ID, 1-255 //mavlink_attitude_t mav_attitude; //mavlink_sys_status_t mav_stat; mavlink_vfr_hud_t mav_hud; //mav_stat.mode = MAV_MODE_MANUAL; //mav_stat.status = MAV_STATE_STANDBY; //mav_stat.vbat = 8400; //mav_stat.battery_remaining = 1000; mav_hud.airspeed = 0.0; mav_hud.groundspeed = 0.0; mav_hud.throttle = 0; fprintf(stdout, "Entering MAVlink mode; reset the MCU to exit\n\n"); gps.gsvMessage(true); gps.gsaMessage(true); gps.serial.attach(gpsRecv, Serial::RxIrq); while (done == false) { if (keypad.pressed == true) { // && started keypad.pressed = false; done = true; } int millis = timer.read_ms(); if ((millis % 1000) == 0) { mav_hud.heading = 0.0; //ahrs.parser.yaw; mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, 0.0, //ToDeg(ahrs.roll), 0.0, //ToDeg(ahrs.pitch), 0.0, //ToDeg(ahrs.yaw), TODO: 3 fix this to use current estimate 0.0, // rollspeed 0.0, // pitchspeed 0.0 // yawspeed ); mav_hud.groundspeed = sensors.encSpeed; mav_hud.groundspeed *= 2.237; // convert to mph //mav_hud.heading = compassHeading(); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, mav_hud.groundspeed, mav_hud.groundspeed, mav_hud.heading, mav_hud.throttle, 0.0, // altitude 0.0 // climb ); mavlink_msg_heartbeat_send(MAVLINK_COMM_0, system_type, autopilot_type); mavlink_msg_sys_status_send(MAVLINK_COMM_0, MAV_MODE_MANUAL, MAV_NAV_GROUNDED, MAV_STATE_STANDBY, 0.0, // load (uint16_t) (sensors.voltage * 1000), 1000, // TODO: 3 fix batt remaining 0 // packet drop ); wait(0.001); } // millis % 1000 if (gps.nmea.rmc_ready() && gps.nmea.gga_ready()) { char gpsdate[32], gpstime[32]; gps.process(gps_here, gpsdate, gpstime); gpsStatus = (gps.hdop > 0.0 && gps.hdop < 3.0) ? 1 : 0; mavlink_msg_gps_raw_send(MAVLINK_COMM_0, millis*1000, 3, gps_here.latitude(), gps_here.longitude(), 0.0, // altitude gps.nmea.f_hdop()*100.0, 0.0, // VDOP mav_hud.groundspeed, mav_hud.heading ); mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.nmea.sat_count(), 0, 0, 0, 0, 0); gps.nmea.reset_ready(); } //gps //mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, mav_attitude.roll, mav_attitude.pitch, mav_attitude.yaw, 0.0, 0.0, 0.0); //mavlink_msg_sys_status_send(MAVLINK_COMM_0, mav_stat.mode, mav_stat.nav_mode, mav_stat.status, mav_stat.load, // mav_stat.vbat, mav_stat.battery_remaining, 0); } gps.serial.attach(NULL, Serial::RxIrq); gps.gsvMessage(false); gps.gsaMessage(false); fprintf(stdout, "\n"); return; } int setBacklight(void) { Menu bmenu; bool done = false; bool printUpdate = false; static int backlight=100; lcd.pos(0,0); lcd.printf(LCD_FMT, ">> Backlight"); while (!done) { if (keypad.pressed) { keypad.pressed = false; printUpdate = true; switch (keypad.which) { case NEXT_BUTTON: backlight+=5; if (backlight > 100) backlight = 100; lcd.backlight(backlight); break; case PREV_BUTTON: backlight-=5; if (backlight < 0) backlight = 0; lcd.backlight(backlight); break; case SELECT_BUTTON: done = true; break; } } if (printUpdate) { printUpdate = false; lcd.pos(0,1); lcd.printf("%3d%%%-16s", backlight, ""); } } return 0; } int reverseScreen(void) { lcd.reverseMode(); return 0; } /////////////////////////////////////////////////////////////////////////////////////////////////////// // ADC CONVERSION FUNCTIONS /////////////////////////////////////////////////////////////////////////////////////////////////////// // returns distance in m for Sharp GP2YOA710K0F // to get m and b, I wrote down volt vs. dist by eyeballin the // datasheet chart plot. Then used Excel to do linear regression // float irDistance(unsigned int adc) { float b = 1.0934; // Intercept from Excel float m = 1.4088; // Slope from Excel return m / (((float) adc) * 4.95/4096 - b); }