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
main.cpp@2:fbc6e3cf3ed8, 2013-06-06 (annotated)
- Committer:
- shimniok
- Date:
- Thu Jun 06 13:40:23 2013 +0000
- Revision:
- 2:fbc6e3cf3ed8
- Parent:
- 1:cb84b477886c
- Child:
- 3:42f3821c4e54
Sort-of working version, still some errors with estimation. Added clamp() function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 1:cb84b477886c | 1 | /** Code for "Data Bus" UGV entry for Sparkfun AVC 2013 |
shimniok | 0:a6a169de725f | 2 | * http://www.bot-thoughts.com/ |
shimniok | 0:a6a169de725f | 3 | */ |
shimniok | 0:a6a169de725f | 4 | |
shimniok | 0:a6a169de725f | 5 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 6 | // INCLUDES |
shimniok | 0:a6a169de725f | 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 8 | |
shimniok | 0:a6a169de725f | 9 | #include <stdio.h> |
shimniok | 0:a6a169de725f | 10 | #include <math.h> |
shimniok | 0:a6a169de725f | 11 | #include "mbed.h" |
shimniok | 0:a6a169de725f | 12 | #include "globals.h" |
shimniok | 0:a6a169de725f | 13 | #include "Config.h" |
shimniok | 0:a6a169de725f | 14 | #include "Buttons.h" |
shimniok | 0:a6a169de725f | 15 | #include "Display.h" |
shimniok | 0:a6a169de725f | 16 | #include "Menu.h" |
shimniok | 0:a6a169de725f | 17 | #include "GPSStatus.h" |
shimniok | 0:a6a169de725f | 18 | #include "logging.h" |
shimniok | 0:a6a169de725f | 19 | #include "shell.h" |
shimniok | 0:a6a169de725f | 20 | #include "Sensors.h" |
shimniok | 0:a6a169de725f | 21 | //#include "DCM.h" |
shimniok | 0:a6a169de725f | 22 | //#include "dcm_matrix.h" |
shimniok | 0:a6a169de725f | 23 | #include "kalman.h" |
shimniok | 0:a6a169de725f | 24 | #include "Venus638flpx.h" |
shimniok | 0:a6a169de725f | 25 | #include "Ublox6.h" |
shimniok | 0:a6a169de725f | 26 | #include "Camera.h" |
shimniok | 0:a6a169de725f | 27 | #include "PinDetect.h" |
shimniok | 0:a6a169de725f | 28 | #include "Actuators.h" |
shimniok | 0:a6a169de725f | 29 | #include "IncrementalEncoder.h" |
shimniok | 0:a6a169de725f | 30 | #include "Steering.h" |
shimniok | 0:a6a169de725f | 31 | #include "Schedule.h" |
shimniok | 0:a6a169de725f | 32 | #include "GeoPosition.h" |
shimniok | 0:a6a169de725f | 33 | #include "Mapping.h" |
shimniok | 0:a6a169de725f | 34 | #include "SimpleFilter.h" |
shimniok | 0:a6a169de725f | 35 | #include "Beep.h" |
shimniok | 0:a6a169de725f | 36 | #include "util.h" |
shimniok | 0:a6a169de725f | 37 | #include "MAVlink/include/mavlink_bridge.h" |
shimniok | 0:a6a169de725f | 38 | #include "updater.h" |
shimniok | 0:a6a169de725f | 39 | |
shimniok | 0:a6a169de725f | 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 41 | // DEFINES |
shimniok | 0:a6a169de725f | 42 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 43 | |
shimniok | 0:a6a169de725f | 44 | #define absf(x) (x *= (x < 0.0) ? -1 : 1) |
shimniok | 0:a6a169de725f | 45 | |
shimniok | 0:a6a169de725f | 46 | #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course |
shimniok | 0:a6a169de725f | 47 | #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position |
shimniok | 0:a6a169de725f | 48 | |
shimniok | 0:a6a169de725f | 49 | // Driver configuration parameters |
shimniok | 0:a6a169de725f | 50 | #define SONARLEFT_CHAN 0 |
shimniok | 0:a6a169de725f | 51 | #define SONARRIGHT_CHAN 1 |
shimniok | 0:a6a169de725f | 52 | #define IRLEFT_CHAN 2 |
shimniok | 0:a6a169de725f | 53 | #define IRRIGHT_CHAN 3 |
shimniok | 0:a6a169de725f | 54 | #define TEMP_CHAN 4 |
shimniok | 0:a6a169de725f | 55 | #define GYRO_CHAN 5 |
shimniok | 0:a6a169de725f | 56 | |
shimniok | 0:a6a169de725f | 57 | // Chassis specific parameters |
shimniok | 2:fbc6e3cf3ed8 | 58 | // TODO 1 put WHEEL_CIRC, WHEELBASE, and TRACK in config.txt |
shimniok | 0:a6a169de725f | 59 | #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375m circumference |
shimniok | 0:a6a169de725f | 60 | #define WHEELBASE 0.290 |
shimniok | 0:a6a169de725f | 61 | #define TRACK 0.280 |
shimniok | 0:a6a169de725f | 62 | |
shimniok | 0:a6a169de725f | 63 | #define INSTRUMENT_CHECK 0 |
shimniok | 0:a6a169de725f | 64 | #define AHRS_VISUALIZATION 1 |
shimniok | 0:a6a169de725f | 65 | #define DISPLAY_PANEL 2 |
shimniok | 0:a6a169de725f | 66 | |
shimniok | 0:a6a169de725f | 67 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 68 | // GLOBAL VARIABLES |
shimniok | 0:a6a169de725f | 69 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 70 | |
shimniok | 0:a6a169de725f | 71 | // OUTPUT |
shimniok | 0:a6a169de725f | 72 | DigitalOut confStatus(LED1); // Config file status LED |
shimniok | 0:a6a169de725f | 73 | DigitalOut logStatus(LED2); // Log file status LED |
shimniok | 0:a6a169de725f | 74 | DigitalOut gpsStatus(LED3); // GPS fix status LED |
shimniok | 0:a6a169de725f | 75 | DigitalOut ahrsStatus(LED4); // AHRS status LED |
shimniok | 0:a6a169de725f | 76 | //DigitalOut sonarStart(p18); // Sends signal to start sonar array pings |
shimniok | 0:a6a169de725f | 77 | Display display; // UI display |
shimniok | 0:a6a169de725f | 78 | Beep speaker(p24); // Piezo speaker |
shimniok | 0:a6a169de725f | 79 | |
shimniok | 0:a6a169de725f | 80 | // INPUT |
shimniok | 0:a6a169de725f | 81 | Menu menu; |
shimniok | 0:a6a169de725f | 82 | Buttons keypad; |
shimniok | 0:a6a169de725f | 83 | |
shimniok | 0:a6a169de725f | 84 | // VEHICLE |
shimniok | 0:a6a169de725f | 85 | Steering steerCalc(TRACK, WHEELBASE); // steering calculator |
shimniok | 0:a6a169de725f | 86 | |
shimniok | 0:a6a169de725f | 87 | // COMM |
shimniok | 0:a6a169de725f | 88 | Serial pc(USBTX, USBRX); // PC usb communications |
shimniok | 0:a6a169de725f | 89 | SerialGraphicLCD lcd(p17, p18, SD_FW); // Graphic LCD with summoningdark firmware |
shimniok | 0:a6a169de725f | 90 | //Serial *debug = &pc; |
shimniok | 0:a6a169de725f | 91 | |
shimniok | 0:a6a169de725f | 92 | // SENSORS |
shimniok | 0:a6a169de725f | 93 | Sensors sensors; // Abstraction of sensor drivers |
shimniok | 0:a6a169de725f | 94 | //DCM ahrs; // ArduPilot/MatrixPilot AHRS |
shimniok | 0:a6a169de725f | 95 | Serial *dev; // For use with bridge |
shimniok | 0:a6a169de725f | 96 | |
shimniok | 0:a6a169de725f | 97 | // MISC |
shimniok | 0:a6a169de725f | 98 | FILE *camlog; // Camera log |
shimniok | 0:a6a169de725f | 99 | |
shimniok | 0:a6a169de725f | 100 | // Configuration |
shimniok | 0:a6a169de725f | 101 | Config config; // Persistent configuration |
shimniok | 0:a6a169de725f | 102 | // Course Waypoints |
shimniok | 0:a6a169de725f | 103 | // Sensor Calibration |
shimniok | 0:a6a169de725f | 104 | // etc. |
shimniok | 0:a6a169de725f | 105 | |
shimniok | 0:a6a169de725f | 106 | // Timing |
shimniok | 0:a6a169de725f | 107 | Timer timer; // For main loop scheduling |
shimniok | 0:a6a169de725f | 108 | |
shimniok | 0:a6a169de725f | 109 | // Overall system state (used for logging but also convenient for general use |
shimniok | 0:a6a169de725f | 110 | SystemState state[SSBUF]; // system state for logging, FIFO buffer |
shimniok | 0:a6a169de725f | 111 | unsigned char inState; // FIFO items enter in here |
shimniok | 0:a6a169de725f | 112 | unsigned char outState; // FIFO items leave out here |
shimniok | 0:a6a169de725f | 113 | bool ssBufOverrun = false; |
shimniok | 0:a6a169de725f | 114 | |
shimniok | 0:a6a169de725f | 115 | // GPS Variables |
shimniok | 0:a6a169de725f | 116 | unsigned long age = 0; // gps fix age |
shimniok | 0:a6a169de725f | 117 | |
shimniok | 0:a6a169de725f | 118 | // schedule for LED warning flasher |
shimniok | 0:a6a169de725f | 119 | Schedule blink; |
shimniok | 0:a6a169de725f | 120 | |
shimniok | 0:a6a169de725f | 121 | // Estimation & Navigation Variables |
shimniok | 0:a6a169de725f | 122 | GeoPosition dr_here; // Estimated position based on estimated heading |
shimniok | 0:a6a169de725f | 123 | Mapping mapper; |
shimniok | 0:a6a169de725f | 124 | |
shimniok | 0:a6a169de725f | 125 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 126 | // FUNCTION DEFINITIONS |
shimniok | 0:a6a169de725f | 127 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 128 | |
shimniok | 0:a6a169de725f | 129 | void initFlasher(void); |
shimniok | 0:a6a169de725f | 130 | void initDR(void); |
shimniok | 0:a6a169de725f | 131 | int autonomousMode(void); |
shimniok | 0:a6a169de725f | 132 | void mavlinkMode(void); |
shimniok | 0:a6a169de725f | 133 | void servoCalibrate(void); |
shimniok | 0:a6a169de725f | 134 | void serialBridge(Serial &gps); |
shimniok | 0:a6a169de725f | 135 | int instrumentCheck(void); |
shimniok | 0:a6a169de725f | 136 | void displayData(int mode); |
shimniok | 0:a6a169de725f | 137 | int compassCalibrate(void); |
shimniok | 0:a6a169de725f | 138 | int compassSwing(void); |
shimniok | 0:a6a169de725f | 139 | int gyroSwing(void); |
shimniok | 0:a6a169de725f | 140 | int setBacklight(void); |
shimniok | 0:a6a169de725f | 141 | int reverseScreen(void); |
shimniok | 0:a6a169de725f | 142 | float gyroRate(unsigned int adc); |
shimniok | 0:a6a169de725f | 143 | float sonarDistance(unsigned int adc); |
shimniok | 0:a6a169de725f | 144 | float irDistance(unsigned int adc); |
shimniok | 0:a6a169de725f | 145 | float getVoltage(void); |
shimniok | 0:a6a169de725f | 146 | extern "C" void mbed_reset(); |
shimniok | 0:a6a169de725f | 147 | |
shimniok | 0:a6a169de725f | 148 | extern unsigned int matrix_error; |
shimniok | 0:a6a169de725f | 149 | |
shimniok | 0:a6a169de725f | 150 | // If we don't close the log file, when we restart, all the written data |
shimniok | 0:a6a169de725f | 151 | // will be lost. So we have to use a button to force mbed to close the |
shimniok | 0:a6a169de725f | 152 | // file and preserve the data. |
shimniok | 0:a6a169de725f | 153 | // |
shimniok | 0:a6a169de725f | 154 | |
shimniok | 0:a6a169de725f | 155 | int dummy(void) |
shimniok | 0:a6a169de725f | 156 | { |
shimniok | 0:a6a169de725f | 157 | return 0; |
shimniok | 0:a6a169de725f | 158 | } |
shimniok | 0:a6a169de725f | 159 | |
shimniok | 0:a6a169de725f | 160 | |
shimniok | 0:a6a169de725f | 161 | int resetMe() |
shimniok | 0:a6a169de725f | 162 | { |
shimniok | 0:a6a169de725f | 163 | mbed_reset(); |
shimniok | 0:a6a169de725f | 164 | |
shimniok | 0:a6a169de725f | 165 | return 0; |
shimniok | 0:a6a169de725f | 166 | } |
shimniok | 0:a6a169de725f | 167 | |
shimniok | 0:a6a169de725f | 168 | |
shimniok | 0:a6a169de725f | 169 | int main() |
shimniok | 0:a6a169de725f | 170 | { |
shimniok | 0:a6a169de725f | 171 | // Let's try setting priorities... |
shimniok | 0:a6a169de725f | 172 | NVIC_SetPriority(TIMER3_IRQn, 0); // updater running off Ticker |
shimniok | 2:fbc6e3cf3ed8 | 173 | NVIC_SetPriority(DMA_IRQn, 0); |
shimniok | 2:fbc6e3cf3ed8 | 174 | NVIC_SetPriority(EINT0_IRQn, 0); // wheel encoders |
shimniok | 2:fbc6e3cf3ed8 | 175 | NVIC_SetPriority(EINT1_IRQn, 0); // wheel encoders |
shimniok | 2:fbc6e3cf3ed8 | 176 | NVIC_SetPriority(EINT2_IRQn, 0); // wheel encoders |
shimniok | 2:fbc6e3cf3ed8 | 177 | NVIC_SetPriority(EINT3_IRQn, 0); // wheel encoders |
shimniok | 2:fbc6e3cf3ed8 | 178 | NVIC_SetPriority(UART0_IRQn, 5); // USB |
shimniok | 2:fbc6e3cf3ed8 | 179 | NVIC_SetPriority(UART1_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 180 | NVIC_SetPriority(UART2_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 181 | NVIC_SetPriority(UART3_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 182 | NVIC_SetPriority(I2C0_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 183 | NVIC_SetPriority(I2C1_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 184 | NVIC_SetPriority(I2C2_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 185 | NVIC_SetPriority(ADC_IRQn, 10); // Voltage/current |
shimniok | 2:fbc6e3cf3ed8 | 186 | NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?) |
shimniok | 2:fbc6e3cf3ed8 | 187 | NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?) |
shimniok | 2:fbc6e3cf3ed8 | 188 | NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?) |
shimniok | 2:fbc6e3cf3ed8 | 189 | NVIC_SetPriority(SPI_IRQn, 10); // uSD card, logging |
shimniok | 0:a6a169de725f | 190 | |
shimniok | 2:fbc6e3cf3ed8 | 191 | // Something here is jacking up the I2C stuff |
shimniok | 2:fbc6e3cf3ed8 | 192 | // Also when initializing with ESC powered, it causes motor to run which |
shimniok | 2:fbc6e3cf3ed8 | 193 | // totally jacks up everything (noise?) |
shimniok | 2:fbc6e3cf3ed8 | 194 | initSteering(); |
shimniok | 2:fbc6e3cf3ed8 | 195 | initThrottle(); |
shimniok | 2:fbc6e3cf3ed8 | 196 | // initFlasher(); // Initialize autonomous mode flasher |
shimniok | 2:fbc6e3cf3ed8 | 197 | |
shimniok | 0:a6a169de725f | 198 | // Send data back to the PC |
shimniok | 0:a6a169de725f | 199 | pc.baud(115200); |
shimniok | 1:cb84b477886c | 200 | fprintf(stdout, "Data Bus 2013\n"); |
shimniok | 0:a6a169de725f | 201 | |
shimniok | 0:a6a169de725f | 202 | display.init(); |
shimniok | 1:cb84b477886c | 203 | display.status("Data Bus 2013"); |
shimniok | 0:a6a169de725f | 204 | |
shimniok | 1:cb84b477886c | 205 | fprintf(stdout, "Initializing...\n"); |
shimniok | 0:a6a169de725f | 206 | display.status("Initializing"); |
shimniok | 0:a6a169de725f | 207 | wait(0.2); |
shimniok | 0:a6a169de725f | 208 | |
shimniok | 0:a6a169de725f | 209 | // Initialize status LEDs |
shimniok | 0:a6a169de725f | 210 | ahrsStatus = 0; |
shimniok | 0:a6a169de725f | 211 | gpsStatus = 0; |
shimniok | 0:a6a169de725f | 212 | logStatus = 0; |
shimniok | 0:a6a169de725f | 213 | confStatus = 0; |
shimniok | 0:a6a169de725f | 214 | |
shimniok | 0:a6a169de725f | 215 | //ahrs.G_Dt = UPDATE_PERIOD; |
shimniok | 0:a6a169de725f | 216 | |
shimniok | 0:a6a169de725f | 217 | fprintf(stdout, "Loading configuration...\n"); |
shimniok | 0:a6a169de725f | 218 | display.status("Load config"); |
shimniok | 0:a6a169de725f | 219 | wait(0.2); |
shimniok | 0:a6a169de725f | 220 | if (config.load()) // Load various configurable parameters, e.g., waypoints, declination, etc. |
shimniok | 0:a6a169de725f | 221 | confStatus = 1; |
shimniok | 1:cb84b477886c | 222 | |
shimniok | 0:a6a169de725f | 223 | sensors.Compass_Calibrate(config.magOffset, config.magScale); |
shimniok | 1:cb84b477886c | 224 | //pc.printf("Declination: %.1f\n", config.declination); |
shimniok | 0:a6a169de725f | 225 | pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", |
shimniok | 0:a6a169de725f | 226 | config.escZero, config.escMax, config.topSpeed, config.turnSpeed, |
shimniok | 0:a6a169de725f | 227 | config.speedKp, config.speedKi, config.speedKd); |
shimniok | 1:cb84b477886c | 228 | pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n", |
shimniok | 1:cb84b477886c | 229 | config.steerZero, config.steerGain, config.steerGainAngle); |
shimniok | 0:a6a169de725f | 230 | |
shimniok | 0:a6a169de725f | 231 | // Convert lat/lon waypoints to cartesian |
shimniok | 0:a6a169de725f | 232 | mapper.init(config.wptCount, config.wpt); |
shimniok | 0:a6a169de725f | 233 | for (int w = 0; w < MAXWPT && w < config.wptCount; w++) { |
shimniok | 0:a6a169de725f | 234 | mapper.geoToCart(config.wpt[w], &(config.cwpt[w])); |
shimniok | 0:a6a169de725f | 235 | pc.printf("Waypoint #%d (%.4f, %.4f) lat: %.6f lon: %.6f\n", |
shimniok | 0:a6a169de725f | 236 | w, config.cwpt[w]._x, config.cwpt[w]._y, config.wpt[w].latitude(), config.wpt[w].longitude()); |
shimniok | 0:a6a169de725f | 237 | } |
shimniok | 0:a6a169de725f | 238 | |
shimniok | 1:cb84b477886c | 239 | // TODO 3 print mag and gyro calibrations |
shimniok | 0:a6a169de725f | 240 | |
shimniok | 1:cb84b477886c | 241 | // TODO 3 remove GPS configuration, all config will be in object itself I think |
shimniok | 0:a6a169de725f | 242 | |
shimniok | 0:a6a169de725f | 243 | display.status("Nav configuration "); |
shimniok | 0:a6a169de725f | 244 | steerCalc.setIntercept(config.interceptDist); // Setup steering calculator based on intercept distance |
shimniok | 0:a6a169de725f | 245 | pc.printf("Intercept distance: %.1f\n", config.interceptDist); |
shimniok | 0:a6a169de725f | 246 | pc.printf("Waypoint distance: %.1f\n", config.waypointDist); |
shimniok | 0:a6a169de725f | 247 | pc.printf("Brake distance: %.1f\n", config.brakeDist); |
shimniok | 0:a6a169de725f | 248 | pc.printf("Min turn radius: %.3f\n", config.minRadius); |
shimniok | 0:a6a169de725f | 249 | |
shimniok | 0:a6a169de725f | 250 | fprintf(stdout, "Calculating offsets...\n"); |
shimniok | 0:a6a169de725f | 251 | display.status("Offset calculation "); |
shimniok | 0:a6a169de725f | 252 | wait(0.2); |
shimniok | 1:cb84b477886c | 253 | // TODO 3 Really need to give the gyro more time to settle |
shimniok | 0:a6a169de725f | 254 | sensors.gps.disable(); |
shimniok | 0:a6a169de725f | 255 | sensors.Calculate_Offsets(); |
shimniok | 0:a6a169de725f | 256 | |
shimniok | 0:a6a169de725f | 257 | fprintf(stdout, "Starting GPS...\n"); |
shimniok | 0:a6a169de725f | 258 | display.status("Start GPS "); // TODO 3: would be nice not to have to pad at this level |
shimniok | 0:a6a169de725f | 259 | wait(0.2); |
shimniok | 0:a6a169de725f | 260 | sensors.gps.setUpdateRate(10); |
shimniok | 0:a6a169de725f | 261 | sensors.gps.enable(); |
shimniok | 0:a6a169de725f | 262 | |
shimniok | 0:a6a169de725f | 263 | fprintf(stdout, "Starting Scheduler...\n"); |
shimniok | 0:a6a169de725f | 264 | display.status("Start scheduler "); |
shimniok | 0:a6a169de725f | 265 | wait(0.2); |
shimniok | 0:a6a169de725f | 266 | // Startup sensor/AHRS ticker; update every UPDATE_PERIOD |
shimniok | 0:a6a169de725f | 267 | restartNav(); |
shimniok | 0:a6a169de725f | 268 | startUpdater(); |
shimniok | 0:a6a169de725f | 269 | |
shimniok | 0:a6a169de725f | 270 | /* |
shimniok | 0:a6a169de725f | 271 | fprintf(stdout, "Starting Camera...\n"); |
shimniok | 0:a6a169de725f | 272 | display.status("Start Camera "); |
shimniok | 0:a6a169de725f | 273 | wait(0.5); |
shimniok | 0:a6a169de725f | 274 | cam.start(); |
shimniok | 0:a6a169de725f | 275 | */ |
shimniok | 0:a6a169de725f | 276 | |
shimniok | 0:a6a169de725f | 277 | keypad.init(); |
shimniok | 0:a6a169de725f | 278 | |
shimniok | 0:a6a169de725f | 279 | // Setup LCD Input Menu |
shimniok | 0:a6a169de725f | 280 | menu.add("Auto mode", &autonomousMode); |
shimniok | 0:a6a169de725f | 281 | menu.add("Instruments", &instrumentCheck); |
shimniok | 0:a6a169de725f | 282 | menu.add("Calibrate", &compassCalibrate); |
shimniok | 0:a6a169de725f | 283 | menu.add("Compass Swing", &compassSwing); |
shimniok | 0:a6a169de725f | 284 | menu.add("Gyro Calib", &gyroSwing); |
shimniok | 0:a6a169de725f | 285 | //menu.sdd("Reload Config", &loadConfig); |
shimniok | 0:a6a169de725f | 286 | menu.add("Backlight", &setBacklight); |
shimniok | 0:a6a169de725f | 287 | menu.add("Reverse", &reverseScreen); |
shimniok | 0:a6a169de725f | 288 | menu.add("Reset", &resetMe); |
shimniok | 0:a6a169de725f | 289 | |
shimniok | 0:a6a169de725f | 290 | char cmd; |
shimniok | 0:a6a169de725f | 291 | bool printMenu = true; |
shimniok | 0:a6a169de725f | 292 | bool printLCDMenu = true; |
shimniok | 0:a6a169de725f | 293 | |
shimniok | 0:a6a169de725f | 294 | timer.start(); |
shimniok | 0:a6a169de725f | 295 | timer.reset(); |
shimniok | 0:a6a169de725f | 296 | |
shimniok | 0:a6a169de725f | 297 | int thisUpdate = timer.read_ms(); |
shimniok | 0:a6a169de725f | 298 | int nextUpdate = thisUpdate; |
shimniok | 0:a6a169de725f | 299 | //int hdgUpdate = nextUpdate; |
shimniok | 0:a6a169de725f | 300 | |
shimniok | 0:a6a169de725f | 301 | while (1) { |
shimniok | 0:a6a169de725f | 302 | |
shimniok | 0:a6a169de725f | 303 | /* |
shimniok | 0:a6a169de725f | 304 | if (timer.read_ms() > hdgUpdate) { |
shimniok | 0:a6a169de725f | 305 | fprintf(stdout, "He=%.3f %.5f\n", kfGetX(0), kfGetX(1)); |
shimniok | 0:a6a169de725f | 306 | hdgUpdate = timer.read_ms() + 100; |
shimniok | 0:a6a169de725f | 307 | }*/ |
shimniok | 0:a6a169de725f | 308 | |
shimniok | 0:a6a169de725f | 309 | if ((thisUpdate = timer.read_ms()) > nextUpdate) { |
shimniok | 1:cb84b477886c | 310 | // Pulling out current state so we get the most current |
shimniok | 0:a6a169de725f | 311 | SystemState s = state[inState]; |
shimniok | 1:cb84b477886c | 312 | // Now populate in the current GPS data |
shimniok | 0:a6a169de725f | 313 | s.gpsHDOP = sensors.gps.hdop(); |
shimniok | 0:a6a169de725f | 314 | s.gpsSats = sensors.gps.sat_count(); |
shimniok | 0:a6a169de725f | 315 | display.update(s); |
shimniok | 0:a6a169de725f | 316 | nextUpdate = thisUpdate + 2000; |
shimniok | 1:cb84b477886c | 317 | // TODO move this statistic into display class |
shimniok | 2:fbc6e3cf3ed8 | 318 | //fprintf(stdout, "update time: %d\n", getUpdateTime()); |
shimniok | 0:a6a169de725f | 319 | } |
shimniok | 0:a6a169de725f | 320 | |
shimniok | 0:a6a169de725f | 321 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 322 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 323 | printLCDMenu = true; |
shimniok | 0:a6a169de725f | 324 | switch (keypad.which) { |
shimniok | 0:a6a169de725f | 325 | case NEXT_BUTTON: |
shimniok | 0:a6a169de725f | 326 | menu.next(); |
shimniok | 0:a6a169de725f | 327 | break; |
shimniok | 0:a6a169de725f | 328 | case PREV_BUTTON: |
shimniok | 0:a6a169de725f | 329 | menu.prev(); |
shimniok | 0:a6a169de725f | 330 | break; |
shimniok | 0:a6a169de725f | 331 | case SELECT_BUTTON: |
shimniok | 0:a6a169de725f | 332 | display.select(menu.getItemName()); |
shimniok | 0:a6a169de725f | 333 | menu.select(); |
shimniok | 0:a6a169de725f | 334 | printMenu = true; |
shimniok | 0:a6a169de725f | 335 | break; |
shimniok | 0:a6a169de725f | 336 | default: |
shimniok | 0:a6a169de725f | 337 | printLCDMenu = false; |
shimniok | 0:a6a169de725f | 338 | break; |
shimniok | 0:a6a169de725f | 339 | }//switch |
shimniok | 0:a6a169de725f | 340 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 341 | }// if (keypad.pressed) |
shimniok | 0:a6a169de725f | 342 | |
shimniok | 0:a6a169de725f | 343 | |
shimniok | 0:a6a169de725f | 344 | if (printLCDMenu) { |
shimniok | 0:a6a169de725f | 345 | display.menu( menu.getItemName() ); |
shimniok | 0:a6a169de725f | 346 | display.status("Ready."); |
shimniok | 0:a6a169de725f | 347 | display.redraw(); |
shimniok | 0:a6a169de725f | 348 | printLCDMenu = false; |
shimniok | 0:a6a169de725f | 349 | } |
shimniok | 0:a6a169de725f | 350 | |
shimniok | 0:a6a169de725f | 351 | |
shimniok | 0:a6a169de725f | 352 | if (printMenu) { |
shimniok | 0:a6a169de725f | 353 | int i=0; |
shimniok | 0:a6a169de725f | 354 | fprintf(stdout, "\n==============\nData Bus Menu\n==============\n"); |
shimniok | 0:a6a169de725f | 355 | fprintf(stdout, "%d) Autonomous mode\n", i++); |
shimniok | 0:a6a169de725f | 356 | fprintf(stdout, "%d) Bridge serial to GPS\n", i++); |
shimniok | 0:a6a169de725f | 357 | fprintf(stdout, "%d) Calibrate compass\n", i++); |
shimniok | 0:a6a169de725f | 358 | fprintf(stdout, "%d) Swing compass\n", i++); |
shimniok | 0:a6a169de725f | 359 | fprintf(stdout, "%d) Gyro calibrate\n", i++); |
shimniok | 0:a6a169de725f | 360 | fprintf(stdout, "%d) Instrument check\n", i++); |
shimniok | 0:a6a169de725f | 361 | fprintf(stdout, "%d) Display AHRS\n", i++); |
shimniok | 0:a6a169de725f | 362 | fprintf(stdout, "%d) Mavlink mode\n", i++); |
shimniok | 0:a6a169de725f | 363 | fprintf(stdout, "%d) Shell\n", i++); |
shimniok | 0:a6a169de725f | 364 | fprintf(stdout, "R) Reset\n"); |
shimniok | 0:a6a169de725f | 365 | fprintf(stdout, "\nSelect from the above: "); |
shimniok | 2:fbc6e3cf3ed8 | 366 | //fflush(stdout); |
shimniok | 0:a6a169de725f | 367 | printMenu = false; |
shimniok | 0:a6a169de725f | 368 | } |
shimniok | 0:a6a169de725f | 369 | |
shimniok | 0:a6a169de725f | 370 | |
shimniok | 0:a6a169de725f | 371 | // Basic functional architecture |
shimniok | 0:a6a169de725f | 372 | // SENSORS -> FILTERS -> AHRS -> POSITION -> NAVIGATION -> CONTROL | INPUT/OUTPUT | LOGGING |
shimniok | 0:a6a169de725f | 373 | // SENSORS (for now) are polled out of AHRS via interrupt every 10ms |
shimniok | 0:a6a169de725f | 374 | // |
shimniok | 0:a6a169de725f | 375 | // no FILTERing in place right now |
shimniok | 0:a6a169de725f | 376 | // if we filter too heavily we get lag. At 30mph = 14m/s a sensor lag |
shimniok | 0:a6a169de725f | 377 | // of only 10ms means the estimate is 140cm behind the robot |
shimniok | 0:a6a169de725f | 378 | // |
shimniok | 0:a6a169de725f | 379 | // POSITION and NAVIGATION should probably always be running |
shimniok | 0:a6a169de725f | 380 | // log file can have different entry type per module, to be demultiplexed on the PC |
shimniok | 0:a6a169de725f | 381 | // |
shimniok | 0:a6a169de725f | 382 | // Autonomous mode engages CONTROL outputs |
shimniok | 0:a6a169de725f | 383 | // |
shimniok | 0:a6a169de725f | 384 | // I/O mode could be one of: MAVlink, serial bridge (gps), sensor check, shell, log to serial |
shimniok | 0:a6a169de725f | 385 | // Or maybe shell should be the main control for different output modes |
shimniok | 0:a6a169de725f | 386 | // |
shimniok | 0:a6a169de725f | 387 | // LOGGING can be turned on or off, probably best to start with it engaged |
shimniok | 0:a6a169de725f | 388 | // and then disable from user panel or when navigation is ended |
shimniok | 0:a6a169de725f | 389 | |
shimniok | 0:a6a169de725f | 390 | if (pc.readable()) { |
shimniok | 0:a6a169de725f | 391 | cmd = pc.getc(); |
shimniok | 0:a6a169de725f | 392 | fprintf(stdout, "%c\n", cmd); |
shimniok | 0:a6a169de725f | 393 | printMenu = true; |
shimniok | 0:a6a169de725f | 394 | printLCDMenu = true; |
shimniok | 0:a6a169de725f | 395 | |
shimniok | 0:a6a169de725f | 396 | switch (cmd) { |
shimniok | 0:a6a169de725f | 397 | case 'R' : |
shimniok | 0:a6a169de725f | 398 | resetMe(); |
shimniok | 0:a6a169de725f | 399 | break; |
shimniok | 0:a6a169de725f | 400 | case '0' : |
shimniok | 0:a6a169de725f | 401 | display.select(menu.getItemName(0)); |
shimniok | 0:a6a169de725f | 402 | autonomousMode(); |
shimniok | 0:a6a169de725f | 403 | break; |
shimniok | 0:a6a169de725f | 404 | case '1' : |
shimniok | 0:a6a169de725f | 405 | display.select("Serial bridge"); |
shimniok | 0:a6a169de725f | 406 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 407 | sensors.gps.enableVerbose(); |
shimniok | 0:a6a169de725f | 408 | serialBridge( *(sensors.gps.getSerial()) ); |
shimniok | 0:a6a169de725f | 409 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 410 | break; |
shimniok | 0:a6a169de725f | 411 | case '2' : |
shimniok | 0:a6a169de725f | 412 | display.select(menu.getItemName(1)); |
shimniok | 0:a6a169de725f | 413 | compassCalibrate(); |
shimniok | 0:a6a169de725f | 414 | break; |
shimniok | 0:a6a169de725f | 415 | case '3' : |
shimniok | 0:a6a169de725f | 416 | display.select(menu.getItemName(2)); |
shimniok | 0:a6a169de725f | 417 | compassSwing(); |
shimniok | 0:a6a169de725f | 418 | break; |
shimniok | 0:a6a169de725f | 419 | case '4' : |
shimniok | 0:a6a169de725f | 420 | display.select(menu.getItemName(2)); |
shimniok | 0:a6a169de725f | 421 | gyroSwing(); |
shimniok | 0:a6a169de725f | 422 | break; |
shimniok | 0:a6a169de725f | 423 | case '5' : |
shimniok | 0:a6a169de725f | 424 | display.select("Instruments"); |
shimniok | 0:a6a169de725f | 425 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 426 | displayData(INSTRUMENT_CHECK); |
shimniok | 0:a6a169de725f | 427 | break; |
shimniok | 0:a6a169de725f | 428 | case '6' : |
shimniok | 0:a6a169de725f | 429 | display.select("AHRS Visual'n"); |
shimniok | 0:a6a169de725f | 430 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 431 | displayData(AHRS_VISUALIZATION); |
shimniok | 0:a6a169de725f | 432 | break; |
shimniok | 0:a6a169de725f | 433 | case '7' : |
shimniok | 0:a6a169de725f | 434 | display.select("Mavlink mode"); |
shimniok | 0:a6a169de725f | 435 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 436 | mavlinkMode(); |
shimniok | 0:a6a169de725f | 437 | break; |
shimniok | 0:a6a169de725f | 438 | case '8' : |
shimniok | 0:a6a169de725f | 439 | display.select("Shell"); |
shimniok | 0:a6a169de725f | 440 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 441 | shell(); |
shimniok | 0:a6a169de725f | 442 | break; |
shimniok | 0:a6a169de725f | 443 | case '9' : |
shimniok | 0:a6a169de725f | 444 | display.select("Serial bridge 2"); |
shimniok | 0:a6a169de725f | 445 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 446 | //gps2.enableVerbose(); |
shimniok | 0:a6a169de725f | 447 | //serialBridge( *(gps2.getSerial()) ); |
shimniok | 0:a6a169de725f | 448 | //gps2.disableVerbose(); |
shimniok | 0:a6a169de725f | 449 | default : |
shimniok | 0:a6a169de725f | 450 | break; |
shimniok | 0:a6a169de725f | 451 | } // switch |
shimniok | 0:a6a169de725f | 452 | |
shimniok | 0:a6a169de725f | 453 | } // if (pc.readable()) |
shimniok | 0:a6a169de725f | 454 | |
shimniok | 0:a6a169de725f | 455 | } // while |
shimniok | 0:a6a169de725f | 456 | |
shimniok | 0:a6a169de725f | 457 | } |
shimniok | 0:a6a169de725f | 458 | |
shimniok | 0:a6a169de725f | 459 | |
shimniok | 0:a6a169de725f | 460 | |
shimniok | 0:a6a169de725f | 461 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 462 | // INITIALIZATION ROUTINES |
shimniok | 0:a6a169de725f | 463 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 464 | |
shimniok | 0:a6a169de725f | 465 | |
shimniok | 0:a6a169de725f | 466 | void initFlasher() |
shimniok | 0:a6a169de725f | 467 | { |
shimniok | 0:a6a169de725f | 468 | // Set up flasher schedule; 3 flashes every 80ms |
shimniok | 0:a6a169de725f | 469 | // for 80ms total, with a 9x80ms period |
shimniok | 0:a6a169de725f | 470 | blink.max(9); |
shimniok | 0:a6a169de725f | 471 | blink.scale(80); |
shimniok | 0:a6a169de725f | 472 | blink.mode(Schedule::repeat); |
shimniok | 0:a6a169de725f | 473 | blink.set(0, 1); blink.set(2, 1); blink.set(4, 1); |
shimniok | 0:a6a169de725f | 474 | } |
shimniok | 0:a6a169de725f | 475 | |
shimniok | 0:a6a169de725f | 476 | |
shimniok | 0:a6a169de725f | 477 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 478 | // OPERATIONAL MODE FUNCTIONS |
shimniok | 0:a6a169de725f | 479 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 480 | |
shimniok | 0:a6a169de725f | 481 | int autonomousMode() |
shimniok | 0:a6a169de725f | 482 | { |
shimniok | 0:a6a169de725f | 483 | bool goGoGo = false; // signal to start moving |
shimniok | 0:a6a169de725f | 484 | bool navDone; // signal that we're done navigating |
shimniok | 0:a6a169de725f | 485 | extern int tSensor, tGPS, tAHRS, tLog; |
shimniok | 0:a6a169de725f | 486 | |
shimniok | 0:a6a169de725f | 487 | sensors.gps.reset_available(); |
shimniok | 0:a6a169de725f | 488 | |
shimniok | 0:a6a169de725f | 489 | // TODO: 3 move to main? |
shimniok | 0:a6a169de725f | 490 | // Navigation |
shimniok | 0:a6a169de725f | 491 | |
shimniok | 0:a6a169de725f | 492 | goGoGo = false; |
shimniok | 0:a6a169de725f | 493 | navDone = false; |
shimniok | 0:a6a169de725f | 494 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 495 | //bool started = false; // flag to indicate robot has exceeded min speed. |
shimniok | 0:a6a169de725f | 496 | |
shimniok | 0:a6a169de725f | 497 | if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d |
shimniok | 0:a6a169de725f | 498 | wait(0.2); |
shimniok | 0:a6a169de725f | 499 | |
shimniok | 0:a6a169de725f | 500 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 501 | sensors.gps.enable(); |
shimniok | 0:a6a169de725f | 502 | //gps2.enable(); |
shimniok | 0:a6a169de725f | 503 | |
shimniok | 0:a6a169de725f | 504 | display.status("Select starts."); |
shimniok | 0:a6a169de725f | 505 | wait(1.0); |
shimniok | 0:a6a169de725f | 506 | |
shimniok | 0:a6a169de725f | 507 | timer.reset(); |
shimniok | 0:a6a169de725f | 508 | timer.start(); |
shimniok | 0:a6a169de725f | 509 | wait(0.1); |
shimniok | 0:a6a169de725f | 510 | |
shimniok | 0:a6a169de725f | 511 | // Tell the navigation / position estimation stuff to reset to starting waypoint |
shimniok | 1:cb84b477886c | 512 | // Disable 05/27/2013 to try and fix initial heading estimate |
shimniok | 1:cb84b477886c | 513 | //restartNav(); |
shimniok | 0:a6a169de725f | 514 | |
shimniok | 0:a6a169de725f | 515 | // Main loop |
shimniok | 0:a6a169de725f | 516 | // |
shimniok | 0:a6a169de725f | 517 | while(navDone == false) { |
shimniok | 0:a6a169de725f | 518 | |
shimniok | 0:a6a169de725f | 519 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 520 | // USER INPUT |
shimniok | 0:a6a169de725f | 521 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 522 | |
shimniok | 0:a6a169de725f | 523 | // Button state machine |
shimniok | 0:a6a169de725f | 524 | // if we've not started going, button starts us |
shimniok | 0:a6a169de725f | 525 | // if we have started going, button stops us |
shimniok | 0:a6a169de725f | 526 | // but only if we've released it first |
shimniok | 0:a6a169de725f | 527 | // |
shimniok | 0:a6a169de725f | 528 | // set throttle only if goGoGo set |
shimniok | 0:a6a169de725f | 529 | if (goGoGo) { |
shimniok | 0:a6a169de725f | 530 | // TODO: 1 Add additional condition of travel for N meters before |
shimniok | 0:a6a169de725f | 531 | // the HALT button is armed |
shimniok | 0:a6a169de725f | 532 | |
shimniok | 0:a6a169de725f | 533 | if (keypad.pressed == true) { // && started |
shimniok | 0:a6a169de725f | 534 | fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> HALT\n"); |
shimniok | 0:a6a169de725f | 535 | display.status("HALT."); |
shimniok | 0:a6a169de725f | 536 | navDone = true; |
shimniok | 0:a6a169de725f | 537 | goGoGo = false; |
shimniok | 0:a6a169de725f | 538 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 539 | endRun(); |
shimniok | 0:a6a169de725f | 540 | } |
shimniok | 0:a6a169de725f | 541 | } else { |
shimniok | 0:a6a169de725f | 542 | if (keypad.pressed == true) { |
shimniok | 0:a6a169de725f | 543 | fprintf(stdout, ">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n"); |
shimniok | 0:a6a169de725f | 544 | display.status("GO GO GO!"); |
shimniok | 0:a6a169de725f | 545 | goGoGo = true; |
shimniok | 0:a6a169de725f | 546 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 547 | beginRun(); |
shimniok | 0:a6a169de725f | 548 | } |
shimniok | 0:a6a169de725f | 549 | } |
shimniok | 0:a6a169de725f | 550 | |
shimniok | 0:a6a169de725f | 551 | // Are we at the last waypoint? |
shimniok | 0:a6a169de725f | 552 | // |
shimniok | 0:a6a169de725f | 553 | if (state[inState].nextWaypoint == config.wptCount) { |
shimniok | 1:cb84b477886c | 554 | fprintf(stdout, "Arrived at final destination.\n"); |
shimniok | 1:cb84b477886c | 555 | display.status("Arrived at end."); |
shimniok | 0:a6a169de725f | 556 | navDone = true; |
shimniok | 0:a6a169de725f | 557 | endRun(); |
shimniok | 0:a6a169de725f | 558 | } |
shimniok | 0:a6a169de725f | 559 | |
shimniok | 0:a6a169de725f | 560 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 561 | // LOGGING |
shimniok | 0:a6a169de725f | 562 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 563 | // sensor reads are happening in the schedHandler(); |
shimniok | 1:cb84b477886c | 564 | // Are there more items to come out of the log fifo? |
shimniok | 0:a6a169de725f | 565 | // Since this could take anywhere from a few hundred usec to |
shimniok | 0:a6a169de725f | 566 | // 150ms, we run it opportunistically and use a buffer. That way |
shimniok | 0:a6a169de725f | 567 | // the sensor updates, calculation, and control can continue to happen |
shimniok | 0:a6a169de725f | 568 | if (outState != inState) { |
shimniok | 0:a6a169de725f | 569 | logStatus = !logStatus; // log indicator LED |
shimniok | 0:a6a169de725f | 570 | //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState); |
shimniok | 1:cb84b477886c | 571 | |
shimniok | 1:cb84b477886c | 572 | if (ssBufOverrun) { // set in update() |
shimniok | 0:a6a169de725f | 573 | fprintf(stdout, ">>> SystemState Buffer Overrun Condition\n"); |
shimniok | 0:a6a169de725f | 574 | ssBufOverrun = false; |
shimniok | 0:a6a169de725f | 575 | } |
shimniok | 0:a6a169de725f | 576 | // do we need to disable interrupts briefly to prevent a race |
shimniok | 0:a6a169de725f | 577 | // condition with schedHandler() ? |
shimniok | 0:a6a169de725f | 578 | int out=outState; // in case we're interrupted this 'should' be atomic |
shimniok | 0:a6a169de725f | 579 | outState++; // increment |
shimniok | 0:a6a169de725f | 580 | outState &= SSBUF; // wrap |
shimniok | 0:a6a169de725f | 581 | logData( state[out] ); // log state data to file |
shimniok | 0:a6a169de725f | 582 | logStatus = !logStatus; // log indicator LED |
shimniok | 0:a6a169de725f | 583 | |
shimniok | 0:a6a169de725f | 584 | //fprintf(stdout, "Time Stats\n----------\nSensors: %d\nGPS: %d\nAHRS: %d\nLog: %d\n----------\nTotal: %d", |
shimniok | 0:a6a169de725f | 585 | // tSensor, tGPS, tAHRS, tLog, tSensor+tGPS+tAHRS+tLog); |
shimniok | 0:a6a169de725f | 586 | } |
shimniok | 0:a6a169de725f | 587 | |
shimniok | 0:a6a169de725f | 588 | } // while |
shimniok | 0:a6a169de725f | 589 | closeLogfile(); |
shimniok | 1:cb84b477886c | 590 | wait(2.0); |
shimniok | 0:a6a169de725f | 591 | logStatus = 0; |
shimniok | 1:cb84b477886c | 592 | display.status("Completed. Saved."); |
shimniok | 1:cb84b477886c | 593 | wait(2.0); |
shimniok | 0:a6a169de725f | 594 | |
shimniok | 0:a6a169de725f | 595 | ahrsStatus = 0; |
shimniok | 0:a6a169de725f | 596 | gpsStatus = 0; |
shimniok | 0:a6a169de725f | 597 | //confStatus = 0; |
shimniok | 0:a6a169de725f | 598 | //flasher = 0; |
shimniok | 0:a6a169de725f | 599 | |
shimniok | 0:a6a169de725f | 600 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 601 | |
shimniok | 0:a6a169de725f | 602 | return 0; |
shimniok | 0:a6a169de725f | 603 | } // autonomousMode |
shimniok | 0:a6a169de725f | 604 | |
shimniok | 0:a6a169de725f | 605 | |
shimniok | 0:a6a169de725f | 606 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 607 | // UTILITY FUNCTIONS |
shimniok | 0:a6a169de725f | 608 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 609 | |
shimniok | 0:a6a169de725f | 610 | |
shimniok | 0:a6a169de725f | 611 | int compassCalibrate() |
shimniok | 0:a6a169de725f | 612 | { |
shimniok | 0:a6a169de725f | 613 | bool done=false; |
shimniok | 0:a6a169de725f | 614 | int m[3]; |
shimniok | 0:a6a169de725f | 615 | FILE *fp; |
shimniok | 0:a6a169de725f | 616 | |
shimniok | 0:a6a169de725f | 617 | fprintf(stdout, "Entering compass calibration in 2 seconds.\nLaunch _3DScatter Processing app now... type e to exit\n"); |
shimniok | 0:a6a169de725f | 618 | display.status("Starting..."); |
shimniok | 0:a6a169de725f | 619 | |
shimniok | 0:a6a169de725f | 620 | fp = openlog("cal"); |
shimniok | 0:a6a169de725f | 621 | |
shimniok | 0:a6a169de725f | 622 | wait(2); |
shimniok | 0:a6a169de725f | 623 | display.status("Select exits"); |
shimniok | 0:a6a169de725f | 624 | timer.reset(); |
shimniok | 0:a6a169de725f | 625 | timer.start(); |
shimniok | 0:a6a169de725f | 626 | while (!done) { |
shimniok | 0:a6a169de725f | 627 | |
shimniok | 0:a6a169de725f | 628 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 629 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 630 | done = true; |
shimniok | 0:a6a169de725f | 631 | } |
shimniok | 0:a6a169de725f | 632 | |
shimniok | 0:a6a169de725f | 633 | while (pc.readable()) { |
shimniok | 0:a6a169de725f | 634 | if (pc.getc() == 'e') { |
shimniok | 0:a6a169de725f | 635 | done = true; |
shimniok | 0:a6a169de725f | 636 | break; |
shimniok | 0:a6a169de725f | 637 | } |
shimniok | 0:a6a169de725f | 638 | } |
shimniok | 0:a6a169de725f | 639 | int millis = timer.read_ms(); |
shimniok | 0:a6a169de725f | 640 | if ((millis % 100) == 0) { |
shimniok | 0:a6a169de725f | 641 | sensors.getRawMag(m); |
shimniok | 0:a6a169de725f | 642 | |
shimniok | 0:a6a169de725f | 643 | // Correction |
shimniok | 0:a6a169de725f | 644 | // Let's see how our ellipsoid looks after scaling and offset |
shimniok | 0:a6a169de725f | 645 | /* |
shimniok | 0:a6a169de725f | 646 | float mag[3]; |
shimniok | 0:a6a169de725f | 647 | mag[0] = ((float) m[0] - M_OFFSET_X) * 0.5 / M_SCALE_X; |
shimniok | 0:a6a169de725f | 648 | mag[1] = ((float) m[1] - M_OFFSET_Y) * 0.5 / M_SCALE_Y; |
shimniok | 0:a6a169de725f | 649 | mag[2] = ((float) m[2] - M_OFFSET_Z) * 0.5 / M_SCALE_Z; |
shimniok | 0:a6a169de725f | 650 | */ |
shimniok | 0:a6a169de725f | 651 | |
shimniok | 0:a6a169de725f | 652 | bool skipIt = false; |
shimniok | 0:a6a169de725f | 653 | for (int i=0; i < 3; i++) { |
shimniok | 0:a6a169de725f | 654 | if (abs(m[i]) > 1024) skipIt = true; |
shimniok | 0:a6a169de725f | 655 | } |
shimniok | 0:a6a169de725f | 656 | if (!skipIt) { |
shimniok | 0:a6a169de725f | 657 | fprintf(stdout, "%c%d %d %d \r\n", 0xDE, m[0], m[1], m[2]); |
shimniok | 0:a6a169de725f | 658 | fprintf(fp, "%d, %d, %d\n", m[0], m[1], m[2]); |
shimniok | 0:a6a169de725f | 659 | } |
shimniok | 0:a6a169de725f | 660 | } |
shimniok | 0:a6a169de725f | 661 | } |
shimniok | 0:a6a169de725f | 662 | if (fp) { |
shimniok | 0:a6a169de725f | 663 | fclose(fp); |
shimniok | 0:a6a169de725f | 664 | display.status("Done. Saved."); |
shimniok | 0:a6a169de725f | 665 | wait(2); |
shimniok | 0:a6a169de725f | 666 | } |
shimniok | 0:a6a169de725f | 667 | |
shimniok | 0:a6a169de725f | 668 | return 0; |
shimniok | 0:a6a169de725f | 669 | } |
shimniok | 0:a6a169de725f | 670 | |
shimniok | 0:a6a169de725f | 671 | // Gather gyro data using turntable equipped with dual channel |
shimniok | 0:a6a169de725f | 672 | // encoder. Use onboard wheel encoder system. Left channel |
shimniok | 0:a6a169de725f | 673 | // is the index (0 degree) mark, while the right channel |
shimniok | 0:a6a169de725f | 674 | // is the incremental encoder. Can then compare gyro integrated |
shimniok | 0:a6a169de725f | 675 | // heading with machine-reported heading |
shimniok | 0:a6a169de725f | 676 | // |
shimniok | 0:a6a169de725f | 677 | // Note: some of this code is identical to the compassSwing() code. |
shimniok | 0:a6a169de725f | 678 | // |
shimniok | 0:a6a169de725f | 679 | int gyroSwing() |
shimniok | 0:a6a169de725f | 680 | { |
shimniok | 0:a6a169de725f | 681 | FILE *fp; |
shimniok | 0:a6a169de725f | 682 | |
shimniok | 0:a6a169de725f | 683 | // Timing is pretty critical so just in case, disable serial processing from GPS |
shimniok | 0:a6a169de725f | 684 | sensors.gps.disable(); |
shimniok | 0:a6a169de725f | 685 | |
shimniok | 0:a6a169de725f | 686 | fprintf(stdout, "Entering gyro swing...\n"); |
shimniok | 0:a6a169de725f | 687 | display.status("Starting..."); |
shimniok | 0:a6a169de725f | 688 | wait(2); |
shimniok | 0:a6a169de725f | 689 | fp = openlog("gy"); |
shimniok | 0:a6a169de725f | 690 | wait(2); |
shimniok | 0:a6a169de725f | 691 | display.status("Begin. Select exits."); |
shimniok | 0:a6a169de725f | 692 | |
shimniok | 0:a6a169de725f | 693 | fprintf(stdout, "Begin clockwise rotation, varying rpm... press select to exit\n"); |
shimniok | 0:a6a169de725f | 694 | |
shimniok | 0:a6a169de725f | 695 | timer.reset(); |
shimniok | 0:a6a169de725f | 696 | timer.start(); |
shimniok | 0:a6a169de725f | 697 | |
shimniok | 0:a6a169de725f | 698 | sensors.rightTotal = 0; // reset total |
shimniok | 0:a6a169de725f | 699 | sensors._right.read(); // easiest way to reset the heading counter |
shimniok | 0:a6a169de725f | 700 | |
shimniok | 0:a6a169de725f | 701 | while (1) { |
shimniok | 0:a6a169de725f | 702 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 703 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 704 | break; |
shimniok | 0:a6a169de725f | 705 | } |
shimniok | 0:a6a169de725f | 706 | |
shimniok | 0:a6a169de725f | 707 | // Print out data |
shimniok | 0:a6a169de725f | 708 | // fprintf(stdout, "%d,%d,%d,%d,%d\n", timer.read_ms(), heading, sensors.g[0], sensors.g[1], sensors.g[2]); |
shimniok | 0:a6a169de725f | 709 | // sensors.rightTotal gives us each tick of the machine, multiply by 2 for cumulative heading, which is easiest |
shimniok | 0:a6a169de725f | 710 | // to compare with cumulative integration of gyro (rather than dealing with 0-360 degree range and modulus and whatnot |
shimniok | 0:a6a169de725f | 711 | 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); |
shimniok | 0:a6a169de725f | 712 | wait(0.200); |
shimniok | 0:a6a169de725f | 713 | } |
shimniok | 0:a6a169de725f | 714 | if (fp) { |
shimniok | 0:a6a169de725f | 715 | fclose(fp); |
shimniok | 0:a6a169de725f | 716 | display.status("Done. Saved."); |
shimniok | 0:a6a169de725f | 717 | fprintf(stdout, "Data collection complete.\n"); |
shimniok | 0:a6a169de725f | 718 | wait(2); |
shimniok | 0:a6a169de725f | 719 | } |
shimniok | 0:a6a169de725f | 720 | |
shimniok | 0:a6a169de725f | 721 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 722 | |
shimniok | 0:a6a169de725f | 723 | return 0; |
shimniok | 0:a6a169de725f | 724 | } |
shimniok | 0:a6a169de725f | 725 | |
shimniok | 0:a6a169de725f | 726 | |
shimniok | 0:a6a169de725f | 727 | // Swing compass using turntable equipped with dual channel |
shimniok | 0:a6a169de725f | 728 | // encoder. Use onboard wheel encoder system. Left channel |
shimniok | 0:a6a169de725f | 729 | // is the index (0 degree) mark, while the right channel |
shimniok | 0:a6a169de725f | 730 | // is the incremental encoder. |
shimniok | 0:a6a169de725f | 731 | // |
shimniok | 0:a6a169de725f | 732 | // Note: much of this code is identical to the gyroSwing() code. |
shimniok | 0:a6a169de725f | 733 | // |
shimniok | 0:a6a169de725f | 734 | int compassSwing() |
shimniok | 0:a6a169de725f | 735 | { |
shimniok | 0:a6a169de725f | 736 | int revolutions=5; |
shimniok | 0:a6a169de725f | 737 | int heading=0; |
shimniok | 0:a6a169de725f | 738 | int leftCount = 0; |
shimniok | 0:a6a169de725f | 739 | FILE *fp; |
shimniok | 0:a6a169de725f | 740 | // left is index track |
shimniok | 0:a6a169de725f | 741 | // right is encoder track |
shimniok | 0:a6a169de725f | 742 | |
shimniok | 0:a6a169de725f | 743 | fprintf(stdout, "Entering compass swing...\n"); |
shimniok | 0:a6a169de725f | 744 | display.status("Starting..."); |
shimniok | 0:a6a169de725f | 745 | wait(2); |
shimniok | 0:a6a169de725f | 746 | fp = openlog("sw"); |
shimniok | 0:a6a169de725f | 747 | wait(2); |
shimniok | 0:a6a169de725f | 748 | display.status("Ok. Begin."); |
shimniok | 0:a6a169de725f | 749 | |
shimniok | 0:a6a169de725f | 750 | fprintf(stdout, "Begin clockwise rotation... exit after %d revolutions\n", revolutions); |
shimniok | 0:a6a169de725f | 751 | |
shimniok | 0:a6a169de725f | 752 | timer.reset(); |
shimniok | 0:a6a169de725f | 753 | timer.start(); |
shimniok | 0:a6a169de725f | 754 | |
shimniok | 0:a6a169de725f | 755 | // wait for index to change |
shimniok | 0:a6a169de725f | 756 | while ((leftCount += sensors._left.read()) < 2) { |
shimniok | 0:a6a169de725f | 757 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 758 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 759 | break; |
shimniok | 0:a6a169de725f | 760 | } |
shimniok | 0:a6a169de725f | 761 | } |
shimniok | 0:a6a169de725f | 762 | fprintf(stdout, ">>>> Index detected. Starting data collection\n"); |
shimniok | 0:a6a169de725f | 763 | leftCount = 0; |
shimniok | 0:a6a169de725f | 764 | // TODO: how to parameterize status? |
shimniok | 0:a6a169de725f | 765 | lcd.pos(0,1); |
shimniok | 0:a6a169de725f | 766 | lcd.printf("%1d %-14s", revolutions, "revs left"); |
shimniok | 0:a6a169de725f | 767 | |
shimniok | 0:a6a169de725f | 768 | sensors._right.read(); // easiest way to reset the heading counter |
shimniok | 0:a6a169de725f | 769 | |
shimniok | 0:a6a169de725f | 770 | while (revolutions > 0) { |
shimniok | 0:a6a169de725f | 771 | int encoder; |
shimniok | 0:a6a169de725f | 772 | |
shimniok | 0:a6a169de725f | 773 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 774 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 775 | break; |
shimniok | 0:a6a169de725f | 776 | } |
shimniok | 0:a6a169de725f | 777 | |
shimniok | 0:a6a169de725f | 778 | // wait for state change |
shimniok | 0:a6a169de725f | 779 | while ((encoder = sensors._right.read()) == 0) { |
shimniok | 0:a6a169de725f | 780 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 781 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 782 | break; |
shimniok | 0:a6a169de725f | 783 | } |
shimniok | 0:a6a169de725f | 784 | } |
shimniok | 0:a6a169de725f | 785 | heading += 2*encoder; // encoder has resolution of 2 degrees |
shimniok | 0:a6a169de725f | 786 | if (heading >= 360) heading -= 360; |
shimniok | 0:a6a169de725f | 787 | |
shimniok | 0:a6a169de725f | 788 | // when index is 1, reset the heading and decrement revolution counter |
shimniok | 0:a6a169de725f | 789 | // make sure we don't detect the index mark until after the first several |
shimniok | 0:a6a169de725f | 790 | // encoder pulses. Index is active low |
shimniok | 0:a6a169de725f | 791 | if ((leftCount += sensors._left.read()) > 1) { |
shimniok | 0:a6a169de725f | 792 | // check for error in heading? |
shimniok | 0:a6a169de725f | 793 | leftCount = 0; |
shimniok | 0:a6a169de725f | 794 | revolutions--; |
shimniok | 0:a6a169de725f | 795 | fprintf(stdout, ">>>>> %d left\n", revolutions); // we sense the rising and falling of the index so /2 |
shimniok | 0:a6a169de725f | 796 | lcd.pos(0,1); |
shimniok | 0:a6a169de725f | 797 | lcd.printf("%1d %-14s", revolutions, "revs left"); |
shimniok | 0:a6a169de725f | 798 | } |
shimniok | 0:a6a169de725f | 799 | |
shimniok | 0:a6a169de725f | 800 | float heading2d = 180 * atan2((float) sensors.mag[1], (float) sensors.mag[0]) / PI; |
shimniok | 0:a6a169de725f | 801 | // Print out data |
shimniok | 0:a6a169de725f | 802 | //getRawMag(m); |
shimniok | 0:a6a169de725f | 803 | fprintf(stdout, "%d %.4f\n", heading, heading2d); |
shimniok | 0:a6a169de725f | 804 | |
shimniok | 0:a6a169de725f | 805 | // int t1=t.read_us(); |
shimniok | 0:a6a169de725f | 806 | if (fp) fprintf(fp, "%d, %d, %.2f, %.4f, %.4f, %.4f\n", |
shimniok | 0:a6a169de725f | 807 | timer.read_ms(), heading, heading2d, sensors.mag[0], sensors.mag[1], sensors.mag[2]); |
shimniok | 0:a6a169de725f | 808 | // int t2=t.read_us(); |
shimniok | 0:a6a169de725f | 809 | // fprintf(stdout, "dt=%d\n", t2-t1); |
shimniok | 0:a6a169de725f | 810 | } |
shimniok | 0:a6a169de725f | 811 | if (fp) { |
shimniok | 0:a6a169de725f | 812 | fclose(fp); |
shimniok | 0:a6a169de725f | 813 | display.status("Done. Saved."); |
shimniok | 0:a6a169de725f | 814 | fprintf(stdout, "Data collection complete.\n"); |
shimniok | 0:a6a169de725f | 815 | wait(2); |
shimniok | 0:a6a169de725f | 816 | } |
shimniok | 0:a6a169de725f | 817 | |
shimniok | 0:a6a169de725f | 818 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 819 | |
shimniok | 0:a6a169de725f | 820 | return 0; |
shimniok | 0:a6a169de725f | 821 | } |
shimniok | 0:a6a169de725f | 822 | |
shimniok | 0:a6a169de725f | 823 | void servoCalibrate() |
shimniok | 0:a6a169de725f | 824 | { |
shimniok | 0:a6a169de725f | 825 | } |
shimniok | 0:a6a169de725f | 826 | |
shimniok | 0:a6a169de725f | 827 | void bridgeRecv() |
shimniok | 0:a6a169de725f | 828 | { |
shimniok | 0:a6a169de725f | 829 | while (dev && dev->readable()) { |
shimniok | 0:a6a169de725f | 830 | pc.putc(dev->getc()); |
shimniok | 0:a6a169de725f | 831 | } |
shimniok | 0:a6a169de725f | 832 | } |
shimniok | 0:a6a169de725f | 833 | |
shimniok | 0:a6a169de725f | 834 | void serialBridge(Serial &serial) |
shimniok | 0:a6a169de725f | 835 | { |
shimniok | 0:a6a169de725f | 836 | char x; |
shimniok | 0:a6a169de725f | 837 | int count = 0; |
shimniok | 0:a6a169de725f | 838 | bool done=false; |
shimniok | 0:a6a169de725f | 839 | |
shimniok | 0:a6a169de725f | 840 | fprintf(stdout, "\nEntering serial bridge in 2 seconds, +++ to escape\n\n"); |
shimniok | 0:a6a169de725f | 841 | sensors.gps.enableVerbose(); |
shimniok | 0:a6a169de725f | 842 | wait(2.0); |
shimniok | 0:a6a169de725f | 843 | //dev = &gps; |
shimniok | 2:fbc6e3cf3ed8 | 844 | sensors.gps.disable(); |
shimniok | 2:fbc6e3cf3ed8 | 845 | serial.baud(4800); |
shimniok | 0:a6a169de725f | 846 | while (!done) { |
shimniok | 0:a6a169de725f | 847 | if (pc.readable()) { |
shimniok | 0:a6a169de725f | 848 | x = pc.getc(); |
shimniok | 0:a6a169de725f | 849 | serial.putc(x); |
shimniok | 0:a6a169de725f | 850 | // escape sequence |
shimniok | 0:a6a169de725f | 851 | if (x == '+') { |
shimniok | 0:a6a169de725f | 852 | if (++count >= 3) done=true; |
shimniok | 0:a6a169de725f | 853 | } else { |
shimniok | 0:a6a169de725f | 854 | count = 0; |
shimniok | 0:a6a169de725f | 855 | } |
shimniok | 0:a6a169de725f | 856 | } |
shimniok | 0:a6a169de725f | 857 | if (serial.readable()) { |
shimniok | 0:a6a169de725f | 858 | pc.putc(serial.getc()); |
shimniok | 0:a6a169de725f | 859 | } |
shimniok | 0:a6a169de725f | 860 | } |
shimniok | 0:a6a169de725f | 861 | } |
shimniok | 0:a6a169de725f | 862 | |
shimniok | 0:a6a169de725f | 863 | /* to be called from panel menu |
shimniok | 0:a6a169de725f | 864 | */ |
shimniok | 0:a6a169de725f | 865 | int instrumentCheck(void) { |
shimniok | 0:a6a169de725f | 866 | displayData(INSTRUMENT_CHECK); |
shimniok | 0:a6a169de725f | 867 | return 0; |
shimniok | 0:a6a169de725f | 868 | } |
shimniok | 0:a6a169de725f | 869 | |
shimniok | 0:a6a169de725f | 870 | /* Display data |
shimniok | 0:a6a169de725f | 871 | * mode determines the type of data and format |
shimniok | 0:a6a169de725f | 872 | * INSTRUMENT_CHECK : display readings of various instruments |
shimniok | 0:a6a169de725f | 873 | * AHRS_VISUALIZATION : display data for use by AHRS python visualization script |
shimniok | 0:a6a169de725f | 874 | */ |
shimniok | 0:a6a169de725f | 875 | |
shimniok | 0:a6a169de725f | 876 | void displayData(int mode) |
shimniok | 0:a6a169de725f | 877 | { |
shimniok | 0:a6a169de725f | 878 | bool done = false; |
shimniok | 0:a6a169de725f | 879 | |
shimniok | 0:a6a169de725f | 880 | lcd.clear(); |
shimniok | 0:a6a169de725f | 881 | |
shimniok | 0:a6a169de725f | 882 | // Init GPS |
shimniok | 0:a6a169de725f | 883 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 884 | sensors.gps.enable(); |
shimniok | 0:a6a169de725f | 885 | sensors.gps.reset_available(); |
shimniok | 0:a6a169de725f | 886 | |
shimniok | 0:a6a169de725f | 887 | // Init 2nd GPS |
shimniok | 0:a6a169de725f | 888 | //gps2.enable(); |
shimniok | 0:a6a169de725f | 889 | //gps2.reset_available(); |
shimniok | 0:a6a169de725f | 890 | |
shimniok | 0:a6a169de725f | 891 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 892 | |
shimniok | 0:a6a169de725f | 893 | timer.reset(); |
shimniok | 0:a6a169de725f | 894 | timer.start(); |
shimniok | 0:a6a169de725f | 895 | |
shimniok | 0:a6a169de725f | 896 | fprintf(stdout, "press e to exit\n"); |
shimniok | 0:a6a169de725f | 897 | while (!done) { |
shimniok | 0:a6a169de725f | 898 | int millis = timer.read_ms(); |
shimniok | 0:a6a169de725f | 899 | |
shimniok | 0:a6a169de725f | 900 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 901 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 902 | done=true; |
shimniok | 0:a6a169de725f | 903 | } |
shimniok | 0:a6a169de725f | 904 | |
shimniok | 0:a6a169de725f | 905 | while (pc.readable()) { |
shimniok | 0:a6a169de725f | 906 | if (pc.getc() == 'e') { |
shimniok | 0:a6a169de725f | 907 | done = true; |
shimniok | 0:a6a169de725f | 908 | break; |
shimniok | 0:a6a169de725f | 909 | } |
shimniok | 0:a6a169de725f | 910 | } |
shimniok | 0:a6a169de725f | 911 | |
shimniok | 0:a6a169de725f | 912 | /* |
shimniok | 0:a6a169de725f | 913 | if (mode == AHRS_VISUALIZATION && (millis % 100) == 0) { |
shimniok | 0:a6a169de725f | 914 | |
shimniok | 0:a6a169de725f | 915 | fprintf(stdout, "!ANG:%.1f,%.1f,%.1f\r\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); |
shimniok | 0:a6a169de725f | 916 | |
shimniok | 0:a6a169de725f | 917 | } else */ |
shimniok | 0:a6a169de725f | 918 | |
shimniok | 0:a6a169de725f | 919 | if (mode == INSTRUMENT_CHECK) { |
shimniok | 0:a6a169de725f | 920 | |
shimniok | 0:a6a169de725f | 921 | if ((millis % 1000) == 0) { |
shimniok | 0:a6a169de725f | 922 | |
shimniok | 0:a6a169de725f | 923 | fprintf(stdout, "update() time = %.3f msec\n", getUpdateTime() / 1000.0); |
shimniok | 0:a6a169de725f | 924 | fprintf(stdout, "Rangers: L=%.2f R=%.2f C=%.2f", sensors.leftRanger, sensors.rightRanger, sensors.centerRanger); |
shimniok | 0:a6a169de725f | 925 | fprintf(stdout, "\n"); |
shimniok | 0:a6a169de725f | 926 | //fprintf(stdout, "ahrs.MAG_Heading=%4.1f\n", ahrs.MAG_Heading*180/PI); |
shimniok | 1:cb84b477886c | 927 | //fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]); |
shimniok | 1:cb84b477886c | 928 | //fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2], |
shimniok | 1:cb84b477886c | 929 | // sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] )); |
shimniok | 0:a6a169de725f | 930 | fprintf(stdout, "g=(%4d, %4d, %4d) %d\n", sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp); |
shimniok | 0:a6a169de725f | 931 | fprintf(stdout, "gc=(%.1f, %.1f, %.1f)\n", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); |
shimniok | 0:a6a169de725f | 932 | fprintf(stdout, "a=(%5d, %5d, %5d)\n", sensors.a[0], sensors.a[1], sensors.a[2]); |
shimniok | 1:cb84b477886c | 933 | fprintf(stdout, "estHdg=%.2f lagHdg=%.2f\n", state[inState].estHeading, state[inState].estLagHeading); |
shimniok | 0:a6a169de725f | 934 | //fprintf(stdout, "roll=%.2f pitch=%.2f yaw=%.2f\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw)); |
shimniok | 0:a6a169de725f | 935 | fprintf(stdout, "speed: left=%.3f right=%.3f\n", sensors.lrEncSpeed, sensors.rrEncSpeed); |
shimniok | 0:a6a169de725f | 936 | fprintf(stdout, "gps=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", |
shimniok | 0:a6a169de725f | 937 | sensors.gps.latitude(), sensors.gps.longitude(), sensors.gps.heading_deg(), |
shimniok | 0:a6a169de725f | 938 | sensors.gps.speed_mps(), sensors.gps.hdop(), sensors.gps.sat_count(), |
shimniok | 0:a6a169de725f | 939 | (unsigned char) sensors.gps.getAvailable() ); |
shimniok | 2:fbc6e3cf3ed8 | 940 | fprintf(stdout, "brg=%6.2f d=%8.4f sa=%6.2f\n", |
shimniok | 2:fbc6e3cf3ed8 | 941 | state[inState].bearing, state[inState].distance, state[inState].steerAngle); |
shimniok | 0:a6a169de725f | 942 | /* |
shimniok | 0:a6a169de725f | 943 | fprintf(stdout, "gps2=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", |
shimniok | 0:a6a169de725f | 944 | gps2.latitude(), gps2.longitude(), gps2.heading_deg(), gps2.speed_mps(), gps2.hdop(), gps2.sat_count(), |
shimniok | 0:a6a169de725f | 945 | (unsigned char) gps2.getAvailable() ); |
shimniok | 0:a6a169de725f | 946 | */ |
shimniok | 0:a6a169de725f | 947 | fprintf(stdout, "v=%.2f a=%.3f\n", sensors.voltage, sensors.current); |
shimniok | 0:a6a169de725f | 948 | fprintf(stdout, "\n"); |
shimniok | 0:a6a169de725f | 949 | |
shimniok | 0:a6a169de725f | 950 | } |
shimniok | 0:a6a169de725f | 951 | |
shimniok | 0:a6a169de725f | 952 | if ((millis % 3000) == 0) { |
shimniok | 0:a6a169de725f | 953 | |
shimniok | 0:a6a169de725f | 954 | lcd.pos(0,1); |
shimniok | 0:a6a169de725f | 955 | //lcd.printf("H=%4.1f ", ahrs.MAG_Heading*180/PI); |
shimniok | 0:a6a169de725f | 956 | //wait(0.1); |
shimniok | 0:a6a169de725f | 957 | lcd.pos(0,2); |
shimniok | 0:a6a169de725f | 958 | lcd.printf("G=%4.1f,%4.1f,%4.1f ", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]); |
shimniok | 0:a6a169de725f | 959 | wait(0.1); |
shimniok | 0:a6a169de725f | 960 | lcd.pos(0,3); |
shimniok | 0:a6a169de725f | 961 | lcd.printf("La=%11.6f HD=%1.1f ", sensors.gps.latitude(), sensors.gps.hdop()); |
shimniok | 0:a6a169de725f | 962 | wait(0.1); |
shimniok | 0:a6a169de725f | 963 | lcd.pos(0,4); |
shimniok | 0:a6a169de725f | 964 | lcd.printf("Lo=%11.6f Sat=%-2d ", sensors.gps.longitude(), sensors.gps.sat_count()); |
shimniok | 0:a6a169de725f | 965 | wait(0.1); |
shimniok | 0:a6a169de725f | 966 | lcd.pos(0,5); |
shimniok | 0:a6a169de725f | 967 | lcd.printf("V=%5.2f A=%5.3f ", sensors.voltage, sensors.current); |
shimniok | 0:a6a169de725f | 968 | |
shimniok | 0:a6a169de725f | 969 | } |
shimniok | 0:a6a169de725f | 970 | } |
shimniok | 0:a6a169de725f | 971 | |
shimniok | 0:a6a169de725f | 972 | } // while !done |
shimniok | 0:a6a169de725f | 973 | // clear input buffer |
shimniok | 0:a6a169de725f | 974 | while (pc.readable()) pc.getc(); |
shimniok | 0:a6a169de725f | 975 | lcd.clear(); |
shimniok | 0:a6a169de725f | 976 | ahrsStatus = 0; |
shimniok | 0:a6a169de725f | 977 | gpsStatus = 0; |
shimniok | 0:a6a169de725f | 978 | } |
shimniok | 0:a6a169de725f | 979 | |
shimniok | 0:a6a169de725f | 980 | |
shimniok | 0:a6a169de725f | 981 | // TODO: 3 move Mavlink into main (non-interrupt) loop along with logging |
shimniok | 0:a6a169de725f | 982 | // possibly also buffered if necessary |
shimniok | 0:a6a169de725f | 983 | |
shimniok | 0:a6a169de725f | 984 | void mavlinkMode() { |
shimniok | 0:a6a169de725f | 985 | uint8_t system_type = MAV_FIXED_WING; |
shimniok | 0:a6a169de725f | 986 | uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC; |
shimniok | 0:a6a169de725f | 987 | //int count = 0; |
shimniok | 0:a6a169de725f | 988 | bool done = false; |
shimniok | 0:a6a169de725f | 989 | |
shimniok | 0:a6a169de725f | 990 | mavlink_system.sysid = 100; // System ID, 1-255 |
shimniok | 0:a6a169de725f | 991 | mavlink_system.compid = 200; // Component/Subsystem ID, 1-255 |
shimniok | 0:a6a169de725f | 992 | |
shimniok | 0:a6a169de725f | 993 | //mavlink_attitude_t mav_attitude; |
shimniok | 0:a6a169de725f | 994 | //mavlink_sys_status_t mav_stat; |
shimniok | 0:a6a169de725f | 995 | mavlink_vfr_hud_t mav_hud; |
shimniok | 0:a6a169de725f | 996 | |
shimniok | 0:a6a169de725f | 997 | //mav_stat.mode = MAV_MODE_MANUAL; |
shimniok | 0:a6a169de725f | 998 | //mav_stat.status = MAV_STATE_STANDBY; |
shimniok | 0:a6a169de725f | 999 | //mav_stat.vbat = 8400; |
shimniok | 0:a6a169de725f | 1000 | //mav_stat.battery_remaining = 1000; |
shimniok | 0:a6a169de725f | 1001 | |
shimniok | 0:a6a169de725f | 1002 | mav_hud.airspeed = 0.0; |
shimniok | 0:a6a169de725f | 1003 | mav_hud.groundspeed = 0.0; |
shimniok | 0:a6a169de725f | 1004 | mav_hud.throttle = 0; |
shimniok | 0:a6a169de725f | 1005 | |
shimniok | 0:a6a169de725f | 1006 | fprintf(stdout, "Entering MAVlink mode; reset the MCU to exit\n\n"); |
shimniok | 0:a6a169de725f | 1007 | |
shimniok | 0:a6a169de725f | 1008 | wait(5.0); |
shimniok | 0:a6a169de725f | 1009 | |
shimniok | 0:a6a169de725f | 1010 | //gps.gsvMessage(true); |
shimniok | 0:a6a169de725f | 1011 | //gps.gsaMessage(true); |
shimniok | 0:a6a169de725f | 1012 | //gps.serial.attach(gpsRecv, Serial::RxIrq); |
shimniok | 0:a6a169de725f | 1013 | |
shimniok | 0:a6a169de725f | 1014 | timer.start(); |
shimniok | 0:a6a169de725f | 1015 | timer.reset(); |
shimniok | 0:a6a169de725f | 1016 | |
shimniok | 0:a6a169de725f | 1017 | while (done == false) { |
shimniok | 0:a6a169de725f | 1018 | |
shimniok | 0:a6a169de725f | 1019 | if (keypad.pressed == true) { // && started |
shimniok | 0:a6a169de725f | 1020 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 1021 | done = true; |
shimniok | 0:a6a169de725f | 1022 | } |
shimniok | 0:a6a169de725f | 1023 | |
shimniok | 0:a6a169de725f | 1024 | int millis = timer.read_ms(); |
shimniok | 0:a6a169de725f | 1025 | |
shimniok | 0:a6a169de725f | 1026 | if ((millis % 1000) == 0) { |
shimniok | 0:a6a169de725f | 1027 | SystemState s = state[outState]; |
shimniok | 0:a6a169de725f | 1028 | /* |
shimniok | 0:a6a169de725f | 1029 | s.millis, |
shimniok | 0:a6a169de725f | 1030 | s.current, s.voltage, |
shimniok | 0:a6a169de725f | 1031 | s.g[0], s.g[1], s.g[2], |
shimniok | 0:a6a169de725f | 1032 | s.gTemp, |
shimniok | 0:a6a169de725f | 1033 | s.a[0], s.a[1], s.a[2], |
shimniok | 0:a6a169de725f | 1034 | s.m[0], s.m[1], s.m[2], |
shimniok | 0:a6a169de725f | 1035 | s.gHeading, //s.cHeading, |
shimniok | 0:a6a169de725f | 1036 | //s.roll, s.pitch, s.yaw, |
shimniok | 0:a6a169de725f | 1037 | s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed*0.44704, s.gpsHDOP, s.gpsSats, // convert gps speed to m/s |
shimniok | 0:a6a169de725f | 1038 | s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, |
shimniok | 0:a6a169de725f | 1039 | s.estHeading, s.estLatitude, s.estLongitude, |
shimniok | 0:a6a169de725f | 1040 | // s.estNorthing, s.estEasting, |
shimniok | 0:a6a169de725f | 1041 | s.estX, s.estY, |
shimniok | 0:a6a169de725f | 1042 | s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle, |
shimniok | 0:a6a169de725f | 1043 | s.leftRanger, s.rightRanger, s.centerRanger, |
shimniok | 0:a6a169de725f | 1044 | s.crossTrackErr |
shimniok | 0:a6a169de725f | 1045 | */ |
shimniok | 0:a6a169de725f | 1046 | |
shimniok | 0:a6a169de725f | 1047 | float groundspeed = (s.lrEncSpeed + s.rrEncSpeed)/2.0; |
shimniok | 0:a6a169de725f | 1048 | //mav_hud.groundspeed *= 2.237; // convert to mph |
shimniok | 0:a6a169de725f | 1049 | //mav_hud.heading = compassHeading(); |
shimniok | 0:a6a169de725f | 1050 | |
shimniok | 0:a6a169de725f | 1051 | mav_hud.heading = 0.0; //ahrs.parser.yaw; |
shimniok | 0:a6a169de725f | 1052 | |
shimniok | 0:a6a169de725f | 1053 | mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, |
shimniok | 0:a6a169de725f | 1054 | 0.0, //ToDeg(ahrs.roll), |
shimniok | 0:a6a169de725f | 1055 | 0.0, //ToDeg(ahrs.pitch), |
shimniok | 0:a6a169de725f | 1056 | s.estHeading, |
shimniok | 0:a6a169de725f | 1057 | 0.0, // rollspeed |
shimniok | 0:a6a169de725f | 1058 | 0.0, // pitchspeed |
shimniok | 0:a6a169de725f | 1059 | 0.0 // yawspeed |
shimniok | 0:a6a169de725f | 1060 | ); |
shimniok | 0:a6a169de725f | 1061 | |
shimniok | 0:a6a169de725f | 1062 | |
shimniok | 0:a6a169de725f | 1063 | mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, |
shimniok | 0:a6a169de725f | 1064 | groundspeed, |
shimniok | 0:a6a169de725f | 1065 | groundspeed, |
shimniok | 0:a6a169de725f | 1066 | s.estHeading, |
shimniok | 0:a6a169de725f | 1067 | mav_hud.throttle, |
shimniok | 0:a6a169de725f | 1068 | 0.0, // altitude |
shimniok | 0:a6a169de725f | 1069 | 0.0 // climb |
shimniok | 0:a6a169de725f | 1070 | ); |
shimniok | 0:a6a169de725f | 1071 | |
shimniok | 0:a6a169de725f | 1072 | mavlink_msg_heartbeat_send(MAVLINK_COMM_0, system_type, autopilot_type); |
shimniok | 0:a6a169de725f | 1073 | mavlink_msg_sys_status_send(MAVLINK_COMM_0, |
shimniok | 0:a6a169de725f | 1074 | MAV_MODE_MANUAL, |
shimniok | 0:a6a169de725f | 1075 | MAV_NAV_GROUNDED, |
shimniok | 0:a6a169de725f | 1076 | MAV_STATE_STANDBY, |
shimniok | 0:a6a169de725f | 1077 | 0.0, // load |
shimniok | 0:a6a169de725f | 1078 | (uint16_t) (sensors.voltage * 1000), |
shimniok | 0:a6a169de725f | 1079 | 1000, // TODO: 3 fix batt remaining |
shimniok | 0:a6a169de725f | 1080 | 0 // packet drop |
shimniok | 0:a6a169de725f | 1081 | ); |
shimniok | 0:a6a169de725f | 1082 | |
shimniok | 0:a6a169de725f | 1083 | |
shimniok | 0:a6a169de725f | 1084 | mavlink_msg_gps_raw_send(MAVLINK_COMM_0, millis*1000, 3, |
shimniok | 0:a6a169de725f | 1085 | sensors.gps.latitude(), |
shimniok | 0:a6a169de725f | 1086 | sensors.gps.longitude(), |
shimniok | 0:a6a169de725f | 1087 | 0.0, // altitude |
shimniok | 0:a6a169de725f | 1088 | sensors.gps.hdop()*100.0, |
shimniok | 0:a6a169de725f | 1089 | 0.0, // VDOP |
shimniok | 0:a6a169de725f | 1090 | groundspeed, |
shimniok | 0:a6a169de725f | 1091 | s.estHeading |
shimniok | 0:a6a169de725f | 1092 | ); |
shimniok | 0:a6a169de725f | 1093 | |
shimniok | 0:a6a169de725f | 1094 | mavlink_msg_gps_status_send(MAVLINK_COMM_0, sensors.gps.sat_count(), 0, 0, 0, 0, 0); |
shimniok | 0:a6a169de725f | 1095 | |
shimniok | 0:a6a169de725f | 1096 | wait(0.001); |
shimniok | 0:a6a169de725f | 1097 | } // millis % 1000 |
shimniok | 0:a6a169de725f | 1098 | |
shimniok | 0:a6a169de725f | 1099 | /* |
shimniok | 0:a6a169de725f | 1100 | if (gps.nmea.rmc_ready() &&sensors.gps.nmea.gga_ready()) { |
shimniok | 0:a6a169de725f | 1101 | char gpsdate[32], gpstime[32]; |
shimniok | 0:a6a169de725f | 1102 | |
shimniok | 0:a6a169de725f | 1103 | sensors.gps.process(gps_here, gpsdate, gpstime); |
shimniok | 0:a6a169de725f | 1104 | gpsStatus = (gps.hdop > 0.0 && sensors.gps.hdop < 3.0) ? 1 : 0; |
shimniok | 0:a6a169de725f | 1105 | |
shimniok | 0:a6a169de725f | 1106 | mavlink_msg_gps_raw_send(MAVLINK_COMM_0, millis*1000, 3, |
shimniok | 0:a6a169de725f | 1107 | gps_here.latitude(), |
shimniok | 0:a6a169de725f | 1108 | gps_here.longitude(), |
shimniok | 0:a6a169de725f | 1109 | 0.0, // altitude |
shimniok | 0:a6a169de725f | 1110 | sensors.gps.nmea.f_hdop()*100.0, |
shimniok | 0:a6a169de725f | 1111 | 0.0, // VDOP |
shimniok | 0:a6a169de725f | 1112 | mav_hud.groundspeed, |
shimniok | 0:a6a169de725f | 1113 | mav_hud.heading |
shimniok | 0:a6a169de725f | 1114 | ); |
shimniok | 0:a6a169de725f | 1115 | |
shimniok | 0:a6a169de725f | 1116 | mavlink_msg_gps_status_send(MAVLINK_COMM_0, sensors.gps.nmea.sat_count(), 0, 0, 0, 0, 0); |
shimniok | 0:a6a169de725f | 1117 | |
shimniok | 0:a6a169de725f | 1118 | sensors.gps.nmea.reset_ready(); |
shimniok | 0:a6a169de725f | 1119 | |
shimniok | 0:a6a169de725f | 1120 | } //gps |
shimniok | 0:a6a169de725f | 1121 | |
shimniok | 0:a6a169de725f | 1122 | //mavlink_msg_attitude_send(MAVLINK_COMM_0, millis*1000, mav_attitude.roll, mav_attitude.pitch, mav_attitude.yaw, 0.0, 0.0, 0.0); |
shimniok | 0:a6a169de725f | 1123 | //mavlink_msg_sys_status_send(MAVLINK_COMM_0, mav_stat.mode, mav_stat.nav_mode, mav_stat.status, mav_stat.load, |
shimniok | 0:a6a169de725f | 1124 | // mav_stat.vbat, mav_stat.battery_remaining, 0); |
shimniok | 0:a6a169de725f | 1125 | |
shimniok | 0:a6a169de725f | 1126 | */ |
shimniok | 0:a6a169de725f | 1127 | |
shimniok | 0:a6a169de725f | 1128 | } |
shimniok | 0:a6a169de725f | 1129 | |
shimniok | 0:a6a169de725f | 1130 | //gps.serial.attach(NULL, Serial::RxIrq); |
shimniok | 0:a6a169de725f | 1131 | //gps.gsvMessage(false); |
shimniok | 0:a6a169de725f | 1132 | //gps.gsaMessage(false); |
shimniok | 0:a6a169de725f | 1133 | |
shimniok | 0:a6a169de725f | 1134 | fprintf(stdout, "\n"); |
shimniok | 0:a6a169de725f | 1135 | |
shimniok | 0:a6a169de725f | 1136 | return; |
shimniok | 0:a6a169de725f | 1137 | } |
shimniok | 0:a6a169de725f | 1138 | |
shimniok | 0:a6a169de725f | 1139 | // TODO: move to display |
shimniok | 0:a6a169de725f | 1140 | int setBacklight(void) { |
shimniok | 0:a6a169de725f | 1141 | Menu bmenu; |
shimniok | 0:a6a169de725f | 1142 | bool done = false; |
shimniok | 0:a6a169de725f | 1143 | bool printUpdate = false; |
shimniok | 0:a6a169de725f | 1144 | static int backlight=100; |
shimniok | 0:a6a169de725f | 1145 | |
shimniok | 0:a6a169de725f | 1146 | display.select(">> Backlight"); |
shimniok | 0:a6a169de725f | 1147 | |
shimniok | 0:a6a169de725f | 1148 | while (!done) { |
shimniok | 0:a6a169de725f | 1149 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 1150 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 1151 | printUpdate = true; |
shimniok | 0:a6a169de725f | 1152 | switch (keypad.which) { |
shimniok | 0:a6a169de725f | 1153 | case NEXT_BUTTON: |
shimniok | 0:a6a169de725f | 1154 | backlight+=5; |
shimniok | 0:a6a169de725f | 1155 | if (backlight > 100) backlight = 100; |
shimniok | 0:a6a169de725f | 1156 | lcd.backlight(backlight); |
shimniok | 0:a6a169de725f | 1157 | break; |
shimniok | 0:a6a169de725f | 1158 | case PREV_BUTTON: |
shimniok | 0:a6a169de725f | 1159 | backlight-=5; |
shimniok | 0:a6a169de725f | 1160 | if (backlight < 0) backlight = 0; |
shimniok | 0:a6a169de725f | 1161 | lcd.backlight(backlight); |
shimniok | 0:a6a169de725f | 1162 | break; |
shimniok | 0:a6a169de725f | 1163 | case SELECT_BUTTON: |
shimniok | 0:a6a169de725f | 1164 | done = true; |
shimniok | 0:a6a169de725f | 1165 | break; |
shimniok | 0:a6a169de725f | 1166 | } |
shimniok | 0:a6a169de725f | 1167 | } |
shimniok | 0:a6a169de725f | 1168 | if (printUpdate) { |
shimniok | 0:a6a169de725f | 1169 | printUpdate = false; |
shimniok | 0:a6a169de725f | 1170 | lcd.pos(0,1); |
shimniok | 0:a6a169de725f | 1171 | lcd.printf("%3d%%%-16s", backlight, ""); |
shimniok | 0:a6a169de725f | 1172 | } |
shimniok | 0:a6a169de725f | 1173 | } |
shimniok | 0:a6a169de725f | 1174 | |
shimniok | 0:a6a169de725f | 1175 | return 0; |
shimniok | 0:a6a169de725f | 1176 | } |
shimniok | 0:a6a169de725f | 1177 | |
shimniok | 0:a6a169de725f | 1178 | int reverseScreen(void) { |
shimniok | 0:a6a169de725f | 1179 | lcd.reverseMode(); |
shimniok | 0:a6a169de725f | 1180 | |
shimniok | 0:a6a169de725f | 1181 | return 0; |
shimniok | 0:a6a169de725f | 1182 | } |
shimniok | 0:a6a169de725f | 1183 | |
shimniok | 0:a6a169de725f | 1184 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 1185 | // ADC CONVERSION FUNCTIONS |
shimniok | 0:a6a169de725f | 1186 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 1187 | |
shimniok | 0:a6a169de725f | 1188 | // returns distance in m for Sharp GP2YOA710K0F |
shimniok | 0:a6a169de725f | 1189 | // to get m and b, I wrote down volt vs. dist by eyeballin the |
shimniok | 0:a6a169de725f | 1190 | // datasheet chart plot. Then used Excel to do linear regression |
shimniok | 0:a6a169de725f | 1191 | // |
shimniok | 0:a6a169de725f | 1192 | float irDistance(unsigned int adc) |
shimniok | 0:a6a169de725f | 1193 | { |
shimniok | 0:a6a169de725f | 1194 | float b = 1.0934; // Intercept from Excel |
shimniok | 0:a6a169de725f | 1195 | float m = 1.4088; // Slope from Excel |
shimniok | 0:a6a169de725f | 1196 | |
shimniok | 0:a6a169de725f | 1197 | return m / (((float) adc) * 4.95/4096 - b); |
shimniok | 0:a6a169de725f | 1198 | } |