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