Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Diff: main.cpp
- Revision:
- 0:826c6171fc1b
diff -r 000000000000 -r 826c6171fc1b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 20 14:57:48 2012 +0000 @@ -0,0 +1,1260 @@ +/** 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); +}