Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: main.cpp
- Revision:
- 17:08f6ee55abbe
- Parent:
- 5:e301811727f7
- Child:
- 20:1c2067937065
--- a/main.cpp Thu Nov 29 17:13:50 2018 +0000 +++ b/main.cpp Thu Nov 29 17:17:52 2018 +0000 @@ -1,4 +1,4 @@ -/** Code for "Data Bus" UGV entry for Sparkfun AVC 2013 +/** Code for "Data Bus" UGV entry for Sparkfun AVC 2014 * http://www.bot-thoughts.com/ */ @@ -6,9 +6,10 @@ // INCLUDES /////////////////////////////////////////////////////////////////////////////////////////////////////// -#include <stdio.h> +#include "mbed.h" #include <math.h> -#include "mbed.h" +#include <stdint.h> +#include "devices.h" #include "globals.h" #include "Config.h" #include "Buttons.h" @@ -16,14 +17,14 @@ #include "Menu.h" #include "GPSStatus.h" #include "logging.h" +#include "Telemetry.h" +#include "SystemState.h" #include "shell.h" +#include "Steering.h" #include "Sensors.h" #include "kalman.h" -#include "Venus638flpx.h" #include "Ublox6.h" -#include "Camera.h" -#include "PinDetect.h" -#include "Actuators.h" +#include "PinDetect.h" // TODO 4 this should be broken into .h, .cpp #include "IncrementalEncoder.h" #include "Steering.h" #include "Schedule.h" @@ -32,7 +33,6 @@ #include "SimpleFilter.h" #include "Beep.h" #include "util.h" -#include "MAVlink/include/mavlink_bridge.h" #include "updater.h" /////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -52,12 +52,6 @@ #define TEMP_CHAN 4 #define GYRO_CHAN 5 -// Chassis specific parameters -// TODO 1 put WHEEL_CIRC, WHEELBASE, and TRACK in config.txt -#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 @@ -70,22 +64,20 @@ 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 +DigitalOut updaterStatus(LED4); // update loop status LED +//DigitalOut sonarStart(p18); // Sends signal to start sonar array pings Display display; // UI display -Beep speaker(p24); // Piezo speaker +//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; +SerialGraphicLCD lcd(LCDTX, LCDRX, SD_FW); // Graphic LCD with summoningdark firmware +Serial tel(TELEMTX, TELEMRX); // UART for telemetry +Telemetry telem(tel); // Setup telemetry system // SENSORS Sensors sensors; // Abstraction of sensor drivers @@ -94,22 +86,12 @@ // MISC FILE *camlog; // Camera log - -// Configuration -Config config; // Persistent configuration - // Course Waypoints - // Sensor Calibration - // etc. +Filesystem fs; // Set up filesystems +Config config; // Configuration utility // Timing Timer timer; // For main loop scheduling -// 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 @@ -124,27 +106,14 @@ // 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); +float irDistance(const unsigned int adc); 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. @@ -166,14 +135,21 @@ int main() { + //checkit(__FILE__, __LINE__); + //xTaskCreate( shell, (const signed char * ) "shell", 128, NULL, (tskIDLE_PRIORITY+3), NULL ); + //checkit(__FILE__, __LINE__); + //vTaskStartScheduler(); // should never get past this line. + //while(1); + // Let's try setting priorities... - NVIC_SetPriority(TIMER3_IRQn, 0); // updater running off Ticker - NVIC_SetPriority(DMA_IRQn, 0); - NVIC_SetPriority(EINT0_IRQn, 0); // wheel encoders - NVIC_SetPriority(EINT1_IRQn, 0); // wheel encoders - NVIC_SetPriority(EINT2_IRQn, 0); // wheel encoders - NVIC_SetPriority(EINT3_IRQn, 0); // wheel encoders - NVIC_SetPriority(UART0_IRQn, 5); // USB + //NVIC_SetPriority(DMA_IRQn, 0); + NVIC_SetPriority(TIMER3_IRQn, 2); // updater running off Ticker, must be highest priority!! + NVIC_SetPriority(EINT0_IRQn, 5); // wheel encoders + NVIC_SetPriority(EINT1_IRQn, 5); // wheel encoders + NVIC_SetPriority(EINT2_IRQn, 5); // wheel encoders + NVIC_SetPriority(EINT3_IRQn, 5); // wheel encoders + NVIC_SetPriority(SPI_IRQn, 7); // uSD card, logging + NVIC_SetPriority(UART0_IRQn, 10); // USB NVIC_SetPriority(UART1_IRQn, 10); NVIC_SetPriority(UART2_IRQn, 10); NVIC_SetPriority(UART3_IRQn, 10); @@ -181,142 +157,217 @@ NVIC_SetPriority(I2C1_IRQn, 10); // sensors? NVIC_SetPriority(I2C2_IRQn, 10); // sensors? NVIC_SetPriority(ADC_IRQn, 10); // Voltage/current - NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?) - NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?) - NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?) - NVIC_SetPriority(SPI_IRQn, 10); // uSD card, logging + NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?) + NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?) + NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?) // Something here is jacking up the I2C stuff // Also when initializing with ESC powered, it causes motor to run which // totally jacks up everything (noise?) initSteering(); initThrottle(); - // initFlasher(); // Initialize autonomous mode flasher - - // Send data back to the PC - pc.baud(115200); - fprintf(stdout, "Data Bus 2013\n"); - while (pc.readable()) pc.getc(); // flush buffer on reset display.init(); - display.status("Data Bus 2013"); - - fprintf(stdout, "Initializing...\n"); + display.status("Data Bus 2014"); + + // Send data back to the PC + pc.baud(115200); + fputs("Data Bus 2014\n", stdout); + fflush(stdin); + + fputs("Initializing...\n", stdout); display.status("Initializing"); - wait(0.2); // Initialize status LEDs - ahrsStatus = 0; + updaterStatus = 0; gpsStatus = 0; logStatus = 0; confStatus = 0; - //ahrs.G_Dt = UPDATE_PERIOD; + if (!fifo_init()) { + error("\n\n%% Error initializing SystemState fifo %%\n"); + } - fprintf(stdout, "Loading configuration...\n"); + fputs("Loading configuration...\n", stdout); display.status("Load config"); - wait(0.2); - if (config.load()) // Load various configurable parameters, e.g., waypoints, declination, etc. + if (config.load("/etc/config.txt")) // Load various configurable parameters, e.g., waypoints, declination, etc. confStatus = 1; - sensors.Compass_Calibrate(config.magOffset, config.magScale); + initThrottle(); + //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); + pc.puts("Speed: escZero="); + pc.puts(cvftos(config.escZero, 3)); + pc.puts(" escMin="); + pc.puts(cvftos(config.escMin, 3)); + pc.puts(" escMax="); + pc.puts(cvftos(config.escMax, 3)); + pc.puts(" top="); + pc.puts(cvftos(config.topSpeed, 1)); + pc.puts(" turn="); + pc.puts(cvftos(config.turnSpeed, 1)); + pc.puts(" Kp="); + pc.puts(cvftos(config.speedKp, 4)); + pc.puts(" Ki="); + pc.puts(cvftos(config.speedKi, 4)); + pc.puts(" Kd="); + pc.puts(cvftos(config.speedKd, 4)); + pc.puts("\n"); + + pc.puts("Steering: steerZero="); + pc.puts(cvftos(config.steerZero, 2)); + pc.puts(" steerScale="); + pc.puts(cvftos(config.steerScale, 1)); + pc.puts("\n"); + steering.setScale(config.steerScale); // Convert lat/lon waypoints to cartesian mapper.init(config.wptCount, config.wpt); - for (int w = 0; w < MAXWPT && w < config.wptCount; w++) { + for (unsigned 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()); + pc.puts("Waypoint #"); + pc.puts(cvntos(w)); + pc.puts(" ("); + pc.puts(cvftos(config.cwpt[w].x, 4)); + pc.puts(", "); + pc.puts(cvftos(config.cwpt[w].y, 4)); + pc.puts(") lat: "); + pc.puts(cvftos(config.wpt[w].latitude(), 6)); + pc.puts(" lon: "); + pc.puts(cvftos(config.wpt[w].longitude(), 6)); + pc.puts(", topspeed: "); + pc.puts(cvftos(config.topSpeed + config.wptTopSpeedAdj[w], 1)); + pc.puts(", turnspeed: "); + pc.puts(cvftos(config.turnSpeed + config.wptTurnSpeedAdj[w], 1)); + pc.puts("\n"); } // TODO 3 print mag and gyro calibrations // TODO 3 remove GPS configuration, all config will be in object itself I think + display.status("Vehicle config "); + pc.puts("Wheelbase: "); + pc.puts(cvftos(config.wheelbase, 3)); + pc.puts("\n"); + pc.puts("Track Width: "); + pc.puts(cvftos(config.track, 3)); + pc.puts("\n"); + steering.setWheelbase(config.wheelbase); + steering.setTrack(config.track); + + display.status("Encoder config "); + pc.puts("Tire Circumference: "); + pc.puts(cvftos(config.tireCirc, 5)); + pc.puts("\n"); + pc.puts("Ticks per revolution: "); + pc.puts(cvftos(config.encStripes, 5)); + pc.puts("\n"); + sensors.configureEncoders(config.tireCirc, config.encStripes); + display.status("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); + pc.puts("Intercept distance: "); + pc.puts(cvftos(config.intercept, 1)); + pc.puts("\n"); + steering.setIntercept(config.intercept); + pc.puts("Waypoint distance: "); + pc.puts(cvftos(config.waypointDist, 1)); + pc.puts("\n"); + pc.puts("Brake distance: "); + pc.puts(cvftos(config.brakeDist, 1)); + pc.puts("\n"); + pc.puts("Min turn radius: "); + pc.puts(cvftos(config.minRadius, 3)); + pc.puts("\n"); - fprintf(stdout, "Calculating offsets...\n"); + display.status("Gyro config "); + pc.puts("Gyro scale: "); + pc.puts(cvftos(config.gyroScale, 5)); + pc.puts("\n"); + sensors.setGyroScale(config.gyroScale); + + display.status("GPS configuration "); + pc.puts("GPS valid speed: "); + pc.puts(cvftos(config.gpsValidSpeed,1)); + pc.puts("\n"); + + pc.puts("Gyro config "); + pc.puts("\n"); + pc.puts("Gyro scale: "); + pc.puts(cvftos(config.gyroScale, 5)); + pc.puts("\n"); + sensors.setGyroScale(config.gyroScale); + + pc.puts("Calculating offsets...\n"); display.status("Offset calculation "); wait(0.2); // TODO 3 Really need to give the gyro more time to settle sensors.gps.disable(); - sensors.Calculate_Offsets(); + // TODO 2 sensors.Calculate_Offsets(); - fprintf(stdout, "Starting GPS...\n"); + pc.puts("Starting GPS...\n"); display.status("Start GPS "); // TODO 3: would be nice not to have to pad at this level wait(0.2); sensors.gps.setUpdateRate(10); - sensors.gps.enable(); + sensors.gps.enable(); - fprintf(stdout, "Starting Scheduler...\n"); + pc.puts("Starting Scheduler...\n"); display.status("Start scheduler "); wait(0.2); // Startup sensor/AHRS ticker; update every UPDATE_PERIOD restartNav(); startUpdater(); -/* - fprintf(stdout, "Starting Camera...\n"); - display.status("Start Camera "); - wait(0.5); - cam.start(); -*/ + pc.puts("Starting keypad...\n"); keypad.init(); + pc.puts("Adding menu items...\n"); + // 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; - + + pc.puts("Starting main timer...\n"); + timer.start(); timer.reset(); int thisUpdate = timer.read_ms(); - int nextUpdate = thisUpdate; - //int hdgUpdate = nextUpdate; + int nextDisplayUpdate = thisUpdate; + int nextWaypointUpdate = thisUpdate; + char cmd; + bool printMenu = true; + bool printLCDMenu = true; + + pc.puts("Timer done, enter loop...\n"); 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) { + thisUpdate = timer.read_ms(); + if (thisUpdate > nextDisplayUpdate) { // Pulling out current state so we get the most current - SystemState s = state[inState]; + SystemState *s = fifo_first(); + // TODO 3 fix this so gps is already in state // Now populate in the current GPS data - s.gpsHDOP = sensors.gps.hdop(); - s.gpsSats = sensors.gps.sat_count(); + s->gpsHDOP = sensors.gps.hdop(); + s->gpsSats = sensors.gps.sat_count(); + + telem.sendPacket(s); display.update(s); - nextUpdate = thisUpdate + 2000; - // TODO move this statistic into display class - //fprintf(stdout, "update time: %d\n", getUpdateTime()); + nextDisplayUpdate = thisUpdate + 200; } - + + // every so often, send the currently configured waypoints + if (thisUpdate > nextWaypointUpdate) { + telem.sendPacket(config.cwpt, config.wptCount); + nextWaypointUpdate = thisUpdate + 10000; + // TODO 2: make this a request/response, Telemetry has to receive packets, decode, etc. + } + if (keypad.pressed) { keypad.pressed = false; printLCDMenu = true; @@ -347,48 +398,23 @@ printLCDMenu = false; } - + // TODO 3 move to UI area 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); + fputs("\n==============\nData Bus Menu\n==============\n", stdout); + fputs("0) Autonomous mode\n", stdout); + fputs("1) Bridge serial to GPS\n", stdout); + fputs("2) Gyro calibrate\n", stdout); + fputs("3) Shell\n", stdout); + fputs("R) Reset\n", stdout); + fputs("\nSelect from the above: ", stdout); + 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); + cmd = fgetc(stdin); + fputc(cmd, stdout); + fputc('\n', stdout); printMenu = true; printLCDMenu = true; @@ -408,49 +434,23 @@ sensors.gps.disableVerbose(); break; case '2' : - display.select(menu.getItemName(1)); - compassCalibrate(); - break; - case '3' : - display.select(menu.getItemName(2)); - compassSwing(); - break; - case '4' : + display.select("Gyro Calib"); display.select(menu.getItemName(2)); gyroSwing(); break; - case '5' : - display.select("Instruments"); - display.status("Standby."); - displayData(INSTRUMENT_CHECK); - break; - case '6' : - display.select("AHRS Visual'n"); - display.status("Standby."); - displayData(AHRS_VISUALIZATION); - break; - case '7' : - display.select("Mavlink mode"); - display.status("Standby."); - mavlinkMode(); - break; - case '8' : + case '3' : display.select("Shell"); display.status("Standby."); - shell(); + shell(0); break; - case '9' : - display.select("Serial bridge 2"); - display.status("Standby."); - //gps2.enableVerbose(); - //serialBridge( *(gps2.getSerial()) ); - //gps2.disableVerbose(); default : break; } // switch } // if (pc.readable()) + wait(0.1); + } // while } @@ -458,30 +458,15 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////// -// 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; + bool goGoGo = false; // signal to start moving + bool navDone; // signal that we're done navigating + int nextTelemUpdate; // keeps track of teletry update periods + //extern int tSensor, tGPS, tAHRS, tLog; sensors.gps.reset_available(); @@ -493,28 +478,29 @@ 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 + if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d wait(0.2); sensors.gps.disableVerbose(); sensors.gps.enable(); //gps2.enable(); + fputs("Press select button to start.\n", stdout); display.status("Select starts."); wait(1.0); timer.reset(); timer.start(); + nextTelemUpdate = timer.read_ms(); wait(0.1); // Tell the navigation / position estimation stuff to reset to starting waypoint // Disable 05/27/2013 to try and fix initial heading estimate //restartNav(); - + // Main loop // while(navDone == false) { - ////////////////////////////////////////////////////////////////////////////// // USER INPUT ////////////////////////////////////////////////////////////////////////////// @@ -526,11 +512,10 @@ // // set throttle only if goGoGo set if (goGoGo) { - // TODO: 1 Add additional condition of travel for N meters before - // the HALT button is armed + // TODO: 2 Add additional condition of travel for N meters before the HALT button is armed if (keypad.pressed == true) { // && started - fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> HALT\n"); + fputs(">>>>>>>>>>>>>>>>>>>>>>> HALT\n", stdout); display.status("HALT."); navDone = true; goGoGo = false; @@ -539,7 +524,7 @@ } } else { if (keypad.pressed == true) { - fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n"); + fputs(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n", stdout); display.status("GO GO GO!"); goGoGo = true; keypad.pressed = false; @@ -549,14 +534,23 @@ // Are we at the last waypoint? // - if (state[inState].nextWaypoint == config.wptCount) { - fprintf(stdout, "Arrived at final destination.\n"); + if (fifo_first()->nextWaypoint == config.wptCount) { + fputs("Arrived at final destination.\n", stdout); display.status("Arrived at end."); navDone = true; endRun(); } ////////////////////////////////////////////////////////////////////////////// + // TELEMETRY + ////////////////////////////////////////////////////////////////////////////// + if (timer.read_ms() > nextTelemUpdate) { + SystemState *s = fifo_first(); + telem.sendPacket(s); // TODO 4 run this out of timer interrupt + nextTelemUpdate += 200; // TODO 3 increase update speed + } + + ////////////////////////////////////////////////////////////////////////////// // LOGGING ////////////////////////////////////////////////////////////////////////////// // sensor reads are happening in the schedHandler(); @@ -564,24 +558,10 @@ // 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) { + if (fifo_available()) { logStatus = !logStatus; // log indicator LED - //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState); - - if (ssBufOverrun) { // set in update() - 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 + logData( fifo_pull() ); // 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 @@ -591,7 +571,7 @@ display.status("Completed. Saved."); wait(2.0); - ahrsStatus = 0; + updaterStatus = 0; gpsStatus = 0; //confStatus = 0; //flasher = 0; @@ -606,116 +586,80 @@ // 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"); - display.status("Starting..."); - - fp = openlog("cal"); - - wait(2); - display.status("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); - display.status("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; + int now; + int next; + int g[3]; + int leftTotal=0; + int rightTotal=0; // Timing is pretty critical so just in case, disable serial processing from GPS sensors.gps.disable(); + stopUpdater(); - fprintf(stdout, "Entering gyro swing...\n"); + fputs("Starting gyro swing...\n", stdout); display.status("Starting..."); - wait(2); - fp = openlog("gy"); - wait(2); - display.status("Begin. Select exits."); +// fp = openlog("gy"); + fp = stdout; - fprintf(stdout, "Begin clockwise rotation, varying rpm... press select to exit\n"); + display.status("Rotate clockwise."); + fputs("Begin clockwise rotation, varying rpm\n", stdout); + wait(1); + + display.status("Select exits."); + fputs("Press select to exit\n", stdout); + wait(1); + timer.reset(); timer.start(); - sensors.rightTotal = 0; // reset total + next = now = timer.read_ms(); + sensors._right.read(); // easiest way to reset the heading counter + sensors._left.read(); while (1) { + now = timer.read_ms(); + 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 (now >= next) { + leftTotal += sensors._left.read(); + rightTotal += sensors._right.read(); + sensors._gyro.read(g); + fputs(cvitos(now), fp); + fputs(" ", fp); + fputs(cvntos(leftTotal), fp); + fputs(" ", fp); + fputs(cvntos(rightTotal), fp); + fputs(" ", fp); + fputs(cvitos(g[_z_]), fp); + fputs("\n", fp); + next = now + 50; + } } - if (fp) { + if (fp && fp != stdout) { fclose(fp); display.status("Done. Saved."); - fprintf(stdout, "Data collection complete.\n"); + fputs("Data collection complete.\n", stdout); wait(2); } + + sensors.gps.enable(); + restartNav(); + startUpdater(); keypad.pressed = false; @@ -723,125 +667,30 @@ } -// 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"); - display.status("Starting..."); - wait(2); - fp = openlog("sw"); - wait(2); - display.status("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; - // TODO: how to parameterize status? - 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); - display.status("Done. Saved."); - fprintf(stdout, "Data collection complete.\n"); - wait(2); - } - - keypad.pressed = false; - - return 0; -} - -void servoCalibrate() -{ -} void bridgeRecv() { +#if 0 while (dev && dev->readable()) { pc.putc(dev->getc()); } +#endif } + void serialBridge(Serial &serial) { +#if 0 char x; int count = 0; bool done=false; - fprintf(stdout, "\nEntering serial bridge in 2 seconds, +++ to escape\n\n"); + fputs("\nEntering serial bridge in 2 seconds, +++ to escape\n\n", stdout); sensors.gps.enableVerbose(); wait(2.0); //dev = &gps; sensors.gps.disable(); - serial.baud(4800); + serial.baud(38400); while (!done) { if (pc.readable()) { x = pc.getc(); @@ -854,288 +703,13 @@ } } if (serial.readable()) { - pc.putc(serial.getc()); + fputc(serial.getc(), stdout); } } -} - -/* 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 - sensors.gps.disableVerbose(); - sensors.gps.enable(); - sensors.gps.reset_available(); - - // Init 2nd GPS - //gps2.enable(); - //gps2.reset_available(); - - 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 lagHdg=%.2f\n", state[inState].estHeading, state[inState].estLagHeading); - //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) %02x\n", - sensors.gps.latitude(), sensors.gps.longitude(), sensors.gps.heading_deg(), - sensors.gps.speed_mps(), sensors.gps.hdop(), sensors.gps.sat_count(), - (unsigned char) sensors.gps.getAvailable() ); - fprintf(stdout, "brg=%6.2f d=%8.4f sa=%6.2f\n", - state[inState].bearing, state[inState].distance, state[inState].steerAngle); - /* - fprintf(stdout, "gps2=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", - gps2.latitude(), gps2.longitude(), gps2.heading_deg(), gps2.speed_mps(), gps2.hdop(), gps2.sat_count(), - (unsigned char) gps2.getAvailable() ); - */ - 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 ", sensors.gps.latitude(), sensors.gps.hdop()); - wait(0.1); - lcd.pos(0,4); - lcd.printf("Lo=%11.6f Sat=%-2d ", sensors.gps.longitude(), sensors.gps.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; +#endif } -// 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"); - - wait(5.0); - - //gps.gsvMessage(true); - //gps.gsaMessage(true); - //gps.serial.attach(gpsRecv, Serial::RxIrq); - - timer.start(); - timer.reset(); - - while (done == false) { - - if (keypad.pressed == true) { // && started - keypad.pressed = false; - done = true; - } - - int millis = timer.read_ms(); - - if ((millis % 1000) == 0) { - SystemState s = state[outState]; - /* - s.millis, - s.current, s.voltage, - s.g[0], s.g[1], s.g[2], - s.gTemp, - s.a[0], s.a[1], s.a[2], - s.m[0], s.m[1], s.m[2], - s.gHeading, //s.cHeading, - //s.roll, s.pitch, s.yaw, - s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed*0.44704, s.gpsHDOP, s.gpsSats, // convert gps speed to m/s - s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, - s.estHeading, s.estLatitude, s.estLongitude, - // s.estNorthing, s.estEasting, - s.estX, s.estY, - s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle, - s.leftRanger, s.rightRanger, s.centerRanger, - s.crossTrackErr - */ - - float groundspeed = (s.lrEncSpeed + s.rrEncSpeed)/2.0; - //mav_hud.groundspeed *= 2.237; // convert to mph - //mav_hud.heading = compassHeading(); - - 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), - s.estHeading, - 0.0, // rollspeed - 0.0, // pitchspeed - 0.0 // yawspeed - ); - - - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, - groundspeed, - groundspeed, - s.estHeading, - 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 - ); - - - mavlink_msg_gps_raw_send(MAVLINK_COMM_0, millis*1000, 3, - sensors.gps.latitude(), - sensors.gps.longitude(), - 0.0, // altitude - sensors.gps.hdop()*100.0, - 0.0, // VDOP - groundspeed, - s.estHeading - ); - - mavlink_msg_gps_status_send(MAVLINK_COMM_0, sensors.gps.sat_count(), 0, 0, 0, 0, 0); - - wait(0.001); - } // millis % 1000 - - /* - if (gps.nmea.rmc_ready() &&sensors.gps.nmea.gga_ready()) { - char gpsdate[32], gpstime[32]; - - sensors.gps.process(gps_here, gpsdate, gpstime); - gpsStatus = (gps.hdop > 0.0 && sensors.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 - sensors.gps.nmea.f_hdop()*100.0, - 0.0, // VDOP - mav_hud.groundspeed, - mav_hud.heading - ); - - mavlink_msg_gps_status_send(MAVLINK_COMM_0, sensors.gps.nmea.sat_count(), 0, 0, 0, 0, 0); - - sensors.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; -} - -// TODO: move to display int setBacklight(void) { Menu bmenu; bool done = false; @@ -1167,7 +741,7 @@ if (printUpdate) { printUpdate = false; lcd.pos(0,1); - lcd.printf("%3d%%%-16s", backlight, ""); + // TODO 3 lcd.printf("%3d%%%-16s", backlight, ""); } } @@ -1188,7 +762,7 @@ // 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 irDistance(const unsigned int adc) { float b = 1.0934; // Intercept from Excel float m = 1.4088; // Slope from Excel