The software I used to run my Sparkfun AVC 2011 entry, a 1:10 scale RC truck called \"Data Bus\". This is the final revision of the code as-run on April 23, 2011, the date of the contest, on the 3rd heat.
Dependencies: TinyCHR6dm PinDetect mbed IncrementalEncoder Servo Schedule DebounceIn SimpleFilter LSM303DLH Steering
main.cpp@0:9b27ebe1ce17, 2011-04-27 (annotated)
- Committer:
- shimniok
- Date:
- Wed Apr 27 19:23:43 2011 +0000
- Revision:
- 0:9b27ebe1ce17
Initial release
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:9b27ebe1ce17 | 1 | /** Code for "Data Bus" UGV entry for Sparkfun AVC 2011 |
shimniok | 0:9b27ebe1ce17 | 2 | * http://bot-thoughts.com/ |
shimniok | 0:9b27ebe1ce17 | 3 | */ |
shimniok | 0:9b27ebe1ce17 | 4 | |
shimniok | 0:9b27ebe1ce17 | 5 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 6 | // INCLUDES |
shimniok | 0:9b27ebe1ce17 | 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 8 | |
shimniok | 0:9b27ebe1ce17 | 9 | #include "mbed.h" |
shimniok | 0:9b27ebe1ce17 | 10 | #include "TinyGPS.h" |
shimniok | 0:9b27ebe1ce17 | 11 | #include "SDFileSystem.h" |
shimniok | 0:9b27ebe1ce17 | 12 | #include "ADC128S.h" |
shimniok | 0:9b27ebe1ce17 | 13 | #include "PinDetect.h" |
shimniok | 0:9b27ebe1ce17 | 14 | #include "LSM303DLH.h" |
shimniok | 0:9b27ebe1ce17 | 15 | #include "Servo.h" |
shimniok | 0:9b27ebe1ce17 | 16 | #include "IncrementalEncoder.h" |
shimniok | 0:9b27ebe1ce17 | 17 | #include "Steering.h" |
shimniok | 0:9b27ebe1ce17 | 18 | #include "Schedule.h" |
shimniok | 0:9b27ebe1ce17 | 19 | #include "GeoPosition.h" |
shimniok | 0:9b27ebe1ce17 | 20 | #include "TinyCHR6dm.h" |
shimniok | 0:9b27ebe1ce17 | 21 | #include "SimpleFilter.h" |
shimniok | 0:9b27ebe1ce17 | 22 | |
shimniok | 0:9b27ebe1ce17 | 23 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 24 | // DEFINES |
shimniok | 0:9b27ebe1ce17 | 25 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 26 | |
shimniok | 0:9b27ebe1ce17 | 27 | #define WHERE(x) debug->printf("%d\n", __LINE__); |
shimniok | 0:9b27ebe1ce17 | 28 | |
shimniok | 0:9b27ebe1ce17 | 29 | #define clamp360(x) \ |
shimniok | 0:9b27ebe1ce17 | 30 | while ((x) >= 360.0) (x) -= 360.0; \ |
shimniok | 0:9b27ebe1ce17 | 31 | while ((x) < 0) (x) += 360.0; |
shimniok | 0:9b27ebe1ce17 | 32 | #define clamp180(x) ((x) - floor((x)/360.0) * 360.0 - 180.0); |
shimniok | 0:9b27ebe1ce17 | 33 | |
shimniok | 0:9b27ebe1ce17 | 34 | #define absf(x) (x *= (x < 0.0) ? -1 : 1) |
shimniok | 0:9b27ebe1ce17 | 35 | |
shimniok | 0:9b27ebe1ce17 | 36 | #define UPDATE_PERIOD 50 // update period in ms |
shimniok | 0:9b27ebe1ce17 | 37 | #define GYRO_UPDATE UPDATE_PERIOD/5 // gyro update period in ms |
shimniok | 0:9b27ebe1ce17 | 38 | |
shimniok | 0:9b27ebe1ce17 | 39 | #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course |
shimniok | 0:9b27ebe1ce17 | 40 | #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position |
shimniok | 0:9b27ebe1ce17 | 41 | |
shimniok | 0:9b27ebe1ce17 | 42 | // Error correction gains |
shimniok | 0:9b27ebe1ce17 | 43 | #define COMPASS_GAIN 0.25 |
shimniok | 0:9b27ebe1ce17 | 44 | #define YAW_GAIN 0.25 |
shimniok | 0:9b27ebe1ce17 | 45 | |
shimniok | 0:9b27ebe1ce17 | 46 | // Driver configuration parameters |
shimniok | 0:9b27ebe1ce17 | 47 | #define SONARLEFT_CHAN 0 |
shimniok | 0:9b27ebe1ce17 | 48 | #define SONARRIGHT_CHAN 1 |
shimniok | 0:9b27ebe1ce17 | 49 | #define IRLEFT_CHAN 2 |
shimniok | 0:9b27ebe1ce17 | 50 | #define IRRIGHT_CHAN 3 |
shimniok | 0:9b27ebe1ce17 | 51 | #define TEMP_CHAN 4 |
shimniok | 0:9b27ebe1ce17 | 52 | #define GYRO_CHAN 5 |
shimniok | 0:9b27ebe1ce17 | 53 | |
shimniok | 0:9b27ebe1ce17 | 54 | // Waypoint queue parameters |
shimniok | 0:9b27ebe1ce17 | 55 | #define MAXWPT 10 |
shimniok | 0:9b27ebe1ce17 | 56 | #define ENDWPT 9999.0 |
shimniok | 0:9b27ebe1ce17 | 57 | |
shimniok | 0:9b27ebe1ce17 | 58 | // Chassis specific parameters |
shimniok | 0:9b27ebe1ce17 | 59 | #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375 wheel dia |
shimniok | 0:9b27ebe1ce17 | 60 | #define WHEELBASE 0.290 |
shimniok | 0:9b27ebe1ce17 | 61 | #define TRACK 0.280 |
shimniok | 0:9b27ebe1ce17 | 62 | |
shimniok | 0:9b27ebe1ce17 | 63 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 64 | // GLOBAL OBJECTS |
shimniok | 0:9b27ebe1ce17 | 65 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 66 | |
shimniok | 0:9b27ebe1ce17 | 67 | // OUTPUT |
shimniok | 0:9b27ebe1ce17 | 68 | DigitalOut confStatus(LED1); // Config file status LED |
shimniok | 0:9b27ebe1ce17 | 69 | DigitalOut logStatus(LED2); // Log file status LED |
shimniok | 0:9b27ebe1ce17 | 70 | DigitalOut gps2Status(LED3); // GPS fix status LED |
shimniok | 0:9b27ebe1ce17 | 71 | DigitalOut ahrsStatus(LED4); // GPS fix status LED |
shimniok | 0:9b27ebe1ce17 | 72 | DigitalOut sonarStart(p18); // Sends signal to start sonar array pings |
shimniok | 0:9b27ebe1ce17 | 73 | DigitalOut flasher(p10); // Autonomous mode warning flasher |
shimniok | 0:9b27ebe1ce17 | 74 | DigitalOut steerServo(p21); // After shutdown, pass pulses through to servo |
shimniok | 0:9b27ebe1ce17 | 75 | //Serial lcd(p9, NC); // LCD module, TX only |
shimniok | 0:9b27ebe1ce17 | 76 | |
shimniok | 0:9b27ebe1ce17 | 77 | // INPUT |
shimniok | 0:9b27ebe1ce17 | 78 | InterruptIn receiver(p23); // RC Receiver steering channel |
shimniok | 0:9b27ebe1ce17 | 79 | DigitalOut buttonPower(p19); // Power the button(s) |
shimniok | 0:9b27ebe1ce17 | 80 | //PinDetect upButton(p17); |
shimniok | 0:9b27ebe1ce17 | 81 | PinDetect selectButton(p20); // Input selectButton |
shimniok | 0:9b27ebe1ce17 | 82 | //PinDetect downButton(p18); |
shimniok | 0:9b27ebe1ce17 | 83 | |
shimniok | 0:9b27ebe1ce17 | 84 | // VEHICLE |
shimniok | 0:9b27ebe1ce17 | 85 | Servo steering(p21); // Steering Servo |
shimniok | 0:9b27ebe1ce17 | 86 | Servo throttle(p22); // ESC |
shimniok | 0:9b27ebe1ce17 | 87 | Schedule go; // Throttle profile, dead stop to full speed |
shimniok | 0:9b27ebe1ce17 | 88 | Schedule stop; // Throttle profile, full speed to dead stop |
shimniok | 0:9b27ebe1ce17 | 89 | Steering steerCalc(TRACK, WHEELBASE); // steering calculator |
shimniok | 0:9b27ebe1ce17 | 90 | int maxSpeed=520; // Servo setting for max speed |
shimniok | 0:9b27ebe1ce17 | 91 | |
shimniok | 0:9b27ebe1ce17 | 92 | // SENSORS |
shimniok | 0:9b27ebe1ce17 | 93 | //HMC6352 compass(p28, p27); // Driver for compass |
shimniok | 0:9b27ebe1ce17 | 94 | LSM303DLH compass3d(p28, p27); // Driver for compass |
shimniok | 0:9b27ebe1ce17 | 95 | //I2C cam(p28, p27); // CMUcam I2C bridge |
shimniok | 0:9b27ebe1ce17 | 96 | //ADC128S adc(p5, p6, p7, p15); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs |
shimniok | 0:9b27ebe1ce17 | 97 | ADC128S adc(p11, p12, p13, p14); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs |
shimniok | 0:9b27ebe1ce17 | 98 | IncrementalEncoder left(p30); // Left wheel encoder |
shimniok | 0:9b27ebe1ce17 | 99 | IncrementalEncoder right(p29); // Right wheel encoder |
shimniok | 0:9b27ebe1ce17 | 100 | //Serial gps1(p26, p25); // GPS1, Locosys LS20031 |
shimniok | 0:9b27ebe1ce17 | 101 | Serial ahrs(p26, p25); // CHR-6dm AHRS |
shimniok | 0:9b27ebe1ce17 | 102 | Serial gps2(p9, p10); // GPS2, iGPS-500 |
shimniok | 0:9b27ebe1ce17 | 103 | Serial *dev; // For use with bridge |
shimniok | 0:9b27ebe1ce17 | 104 | //TinyGPS gps1Parse; // GPS NMEA parser |
shimniok | 0:9b27ebe1ce17 | 105 | TinyCHR6dm ahrsParser; // CHR-6dm AHRS parser |
shimniok | 0:9b27ebe1ce17 | 106 | TinyGPS gps2Parse; // GPS NMEA parser |
shimniok | 0:9b27ebe1ce17 | 107 | |
shimniok | 0:9b27ebe1ce17 | 108 | // COMM |
shimniok | 0:9b27ebe1ce17 | 109 | Serial pc(USBTX, USBRX); // PC usb communications |
shimniok | 0:9b27ebe1ce17 | 110 | Serial *debug = &pc; |
shimniok | 0:9b27ebe1ce17 | 111 | |
shimniok | 0:9b27ebe1ce17 | 112 | // MISC |
shimniok | 0:9b27ebe1ce17 | 113 | Timer timer; // For main loop scheduling |
shimniok | 0:9b27ebe1ce17 | 114 | SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs |
shimniok | 0:9b27ebe1ce17 | 115 | LocalFileSystem local("etc"); // Create the local filesystem under the name "local" |
shimniok | 0:9b27ebe1ce17 | 116 | |
shimniok | 0:9b27ebe1ce17 | 117 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 118 | // GLOBAL VARIABLES |
shimniok | 0:9b27ebe1ce17 | 119 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 120 | |
shimniok | 0:9b27ebe1ce17 | 121 | // GPS Variables |
shimniok | 0:9b27ebe1ce17 | 122 | double gps1_lat; // latitude |
shimniok | 0:9b27ebe1ce17 | 123 | double gps1_lon; // longitude |
shimniok | 0:9b27ebe1ce17 | 124 | double gps2_lat; // latitude |
shimniok | 0:9b27ebe1ce17 | 125 | double gps2_lon; // longitude |
shimniok | 0:9b27ebe1ce17 | 126 | unsigned long age = 0; // gps fix age |
shimniok | 0:9b27ebe1ce17 | 127 | float gps1_hdop = 0.0; // gps horizontal dilution of precision |
shimniok | 0:9b27ebe1ce17 | 128 | float gps2_hdop = 0.0; // gps horizontal dilution of precision |
shimniok | 0:9b27ebe1ce17 | 129 | int year; // gps date variables |
shimniok | 0:9b27ebe1ce17 | 130 | byte month; |
shimniok | 0:9b27ebe1ce17 | 131 | byte day; |
shimniok | 0:9b27ebe1ce17 | 132 | byte hour; |
shimniok | 0:9b27ebe1ce17 | 133 | byte minute; |
shimniok | 0:9b27ebe1ce17 | 134 | byte second; |
shimniok | 0:9b27ebe1ce17 | 135 | byte hundredths; |
shimniok | 0:9b27ebe1ce17 | 136 | |
shimniok | 0:9b27ebe1ce17 | 137 | // schedule for LED warning flasher |
shimniok | 0:9b27ebe1ce17 | 138 | Schedule blink; |
shimniok | 0:9b27ebe1ce17 | 139 | |
shimniok | 0:9b27ebe1ce17 | 140 | // Useful globals |
shimniok | 0:9b27ebe1ce17 | 141 | float declination; // compass declination for local correction |
shimniok | 0:9b27ebe1ce17 | 142 | bool goGoGo = false; // signal to start moving |
shimniok | 0:9b27ebe1ce17 | 143 | bool done = false; // signal that we're done navigating |
shimniok | 0:9b27ebe1ce17 | 144 | |
shimniok | 0:9b27ebe1ce17 | 145 | // Gyro Variables |
shimniok | 0:9b27ebe1ce17 | 146 | float gyro = 0; // gyro reading |
shimniok | 0:9b27ebe1ce17 | 147 | float gyroBias = 2027.0; // gyro bias in raw 12-bit ADC value |
shimniok | 0:9b27ebe1ce17 | 148 | unsigned int temp = 0; // gyro temp reading |
shimniok | 0:9b27ebe1ce17 | 149 | float gyroSens = 4.89; // in LSB/deg/sec since we're using ratiometric ADC and gyro |
shimniok | 0:9b27ebe1ce17 | 150 | float gyroSum[UPDATE_PERIOD/GYRO_UPDATE]; // for summijng/averaging gyro between DR / compass updates |
shimniok | 0:9b27ebe1ce17 | 151 | int gyroCount = 0; // counter for gyroSum array |
shimniok | 0:9b27ebe1ce17 | 152 | |
shimniok | 0:9b27ebe1ce17 | 153 | // Navigation Variables |
shimniok | 0:9b27ebe1ce17 | 154 | GeoPosition wpt[MAXWPT]; // course waypoints |
shimniok | 0:9b27ebe1ce17 | 155 | unsigned int wptCount = 0; // number of waypoints configured |
shimniok | 0:9b27ebe1ce17 | 156 | int wptCurrent = 0; // current waypoint |
shimniok | 0:9b27ebe1ce17 | 157 | GeoPosition here1; // current gps position |
shimniok | 0:9b27ebe1ce17 | 158 | GeoPosition here2; // current gps position |
shimniok | 0:9b27ebe1ce17 | 159 | GeoPosition dr_here; // current dead reckoning position |
shimniok | 0:9b27ebe1ce17 | 160 | GeoPosition dr_here_last; // DR position at last GPS packet |
shimniok | 0:9b27ebe1ce17 | 161 | |
shimniok | 0:9b27ebe1ce17 | 162 | // Misc |
shimniok | 0:9b27ebe1ce17 | 163 | FILE *fp = 0; |
shimniok | 0:9b27ebe1ce17 | 164 | bool buttonPressed = false; |
shimniok | 0:9b27ebe1ce17 | 165 | bool buttonReleased = false; |
shimniok | 0:9b27ebe1ce17 | 166 | |
shimniok | 0:9b27ebe1ce17 | 167 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 168 | // FUNCTION DEFINITIONS |
shimniok | 0:9b27ebe1ce17 | 169 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 170 | |
shimniok | 0:9b27ebe1ce17 | 171 | FILE *initLogfile(void); |
shimniok | 0:9b27ebe1ce17 | 172 | void initButtons(void); |
shimniok | 0:9b27ebe1ce17 | 173 | void initCompass(void); |
shimniok | 0:9b27ebe1ce17 | 174 | void initGPS(void); |
shimniok | 0:9b27ebe1ce17 | 175 | void initAHRS(void); |
shimniok | 0:9b27ebe1ce17 | 176 | void initFlasher(void); |
shimniok | 0:9b27ebe1ce17 | 177 | void initSteering(void); |
shimniok | 0:9b27ebe1ce17 | 178 | void initThrottle(void); |
shimniok | 0:9b27ebe1ce17 | 179 | void initDR(void); |
shimniok | 0:9b27ebe1ce17 | 180 | void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain); |
shimniok | 0:9b27ebe1ce17 | 181 | void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time); |
shimniok | 0:9b27ebe1ce17 | 182 | void autonomousMode(void); |
shimniok | 0:9b27ebe1ce17 | 183 | void findGPSBias(double *brg, double *dist, int n, GeoPosition place); |
shimniok | 0:9b27ebe1ce17 | 184 | void idleMode(void); |
shimniok | 0:9b27ebe1ce17 | 185 | void compassCalibrate(void); |
shimniok | 0:9b27ebe1ce17 | 186 | void servoCalibrate(void); |
shimniok | 0:9b27ebe1ce17 | 187 | void serialBridge(Serial &gps); |
shimniok | 0:9b27ebe1ce17 | 188 | void instrumentCheck(void); |
shimniok | 0:9b27ebe1ce17 | 189 | float compassHeading(void); |
shimniok | 0:9b27ebe1ce17 | 190 | float gyroRate(unsigned int adc); |
shimniok | 0:9b27ebe1ce17 | 191 | float sonarDistance(unsigned int adc); |
shimniok | 0:9b27ebe1ce17 | 192 | float irDistance(unsigned int adc); |
shimniok | 0:9b27ebe1ce17 | 193 | |
shimniok | 0:9b27ebe1ce17 | 194 | // If we don't close the log file, when we restart, all the written data |
shimniok | 0:9b27ebe1ce17 | 195 | // will be lost. So we have to use a button to force mbed to close the |
shimniok | 0:9b27ebe1ce17 | 196 | // file and preserve the data. |
shimniok | 0:9b27ebe1ce17 | 197 | // |
shimniok | 0:9b27ebe1ce17 | 198 | void assertButton() { |
shimniok | 0:9b27ebe1ce17 | 199 | buttonPressed = true; |
shimniok | 0:9b27ebe1ce17 | 200 | buttonReleased = false; |
shimniok | 0:9b27ebe1ce17 | 201 | } |
shimniok | 0:9b27ebe1ce17 | 202 | |
shimniok | 0:9b27ebe1ce17 | 203 | void deassertButton() { |
shimniok | 0:9b27ebe1ce17 | 204 | buttonPressed = false; |
shimniok | 0:9b27ebe1ce17 | 205 | buttonReleased = true; |
shimniok | 0:9b27ebe1ce17 | 206 | } |
shimniok | 0:9b27ebe1ce17 | 207 | |
shimniok | 0:9b27ebe1ce17 | 208 | // Use interrupt handler to receive AHRS serial comm and parse |
shimniok | 0:9b27ebe1ce17 | 209 | // it using TinyCHR6dm library |
shimniok | 0:9b27ebe1ce17 | 210 | // |
shimniok | 0:9b27ebe1ce17 | 211 | void recv1() { |
shimniok | 0:9b27ebe1ce17 | 212 | while (ahrs.readable()) |
shimniok | 0:9b27ebe1ce17 | 213 | ahrsParser.parse(ahrs.getc()); |
shimniok | 0:9b27ebe1ce17 | 214 | } |
shimniok | 0:9b27ebe1ce17 | 215 | |
shimniok | 0:9b27ebe1ce17 | 216 | void recv2() { |
shimniok | 0:9b27ebe1ce17 | 217 | while (gps2.readable()) |
shimniok | 0:9b27ebe1ce17 | 218 | gps2Parse.encode(gps2.getc()); |
shimniok | 0:9b27ebe1ce17 | 219 | } |
shimniok | 0:9b27ebe1ce17 | 220 | |
shimniok | 0:9b27ebe1ce17 | 221 | char getInput(const char *prompt) |
shimniok | 0:9b27ebe1ce17 | 222 | { |
shimniok | 0:9b27ebe1ce17 | 223 | char c; |
shimniok | 0:9b27ebe1ce17 | 224 | |
shimniok | 0:9b27ebe1ce17 | 225 | if (prompt) pc.printf("%s ", prompt); |
shimniok | 0:9b27ebe1ce17 | 226 | while (!pc.readable()) |
shimniok | 0:9b27ebe1ce17 | 227 | wait(0.001); |
shimniok | 0:9b27ebe1ce17 | 228 | c = pc.getc(); |
shimniok | 0:9b27ebe1ce17 | 229 | pc.printf("%c\n", c); |
shimniok | 0:9b27ebe1ce17 | 230 | |
shimniok | 0:9b27ebe1ce17 | 231 | return c; |
shimniok | 0:9b27ebe1ce17 | 232 | } |
shimniok | 0:9b27ebe1ce17 | 233 | |
shimniok | 0:9b27ebe1ce17 | 234 | int main() |
shimniok | 0:9b27ebe1ce17 | 235 | { |
shimniok | 0:9b27ebe1ce17 | 236 | // Send data back to the PC |
shimniok | 0:9b27ebe1ce17 | 237 | pc.baud(115200); |
shimniok | 0:9b27ebe1ce17 | 238 | |
shimniok | 0:9b27ebe1ce17 | 239 | //receiver.rise(&killSwitch); // Detects remote kill switch / rc takeover |
shimniok | 0:9b27ebe1ce17 | 240 | initButtons(); // Initialize input buttons |
shimniok | 0:9b27ebe1ce17 | 241 | initCompass(); |
shimniok | 0:9b27ebe1ce17 | 242 | initSteering(); |
shimniok | 0:9b27ebe1ce17 | 243 | initThrottle(); |
shimniok | 0:9b27ebe1ce17 | 244 | // initFlasher(); // Initialize autonomous mode flasher |
shimniok | 0:9b27ebe1ce17 | 245 | initGPS(); |
shimniok | 0:9b27ebe1ce17 | 246 | |
shimniok | 0:9b27ebe1ce17 | 247 | // Insert menu system here w/ timeout |
shimniok | 0:9b27ebe1ce17 | 248 | bool autoBoot=true; |
shimniok | 0:9b27ebe1ce17 | 249 | /* |
shimniok | 0:9b27ebe1ce17 | 250 | pc.printf("Booting to autonomous mode in 10 seconds, any key to abort.\n "); |
shimniok | 0:9b27ebe1ce17 | 251 | for (int i=1; i > 0; i--) { |
shimniok | 0:9b27ebe1ce17 | 252 | pc.printf("%d...", i); |
shimniok | 0:9b27ebe1ce17 | 253 | if (pc.readable()) { |
shimniok | 0:9b27ebe1ce17 | 254 | autoBoot = false; |
shimniok | 0:9b27ebe1ce17 | 255 | while (pc.readable()) pc.getc(); // clear buffer |
shimniok | 0:9b27ebe1ce17 | 256 | break; |
shimniok | 0:9b27ebe1ce17 | 257 | } |
shimniok | 0:9b27ebe1ce17 | 258 | wait(1.0); |
shimniok | 0:9b27ebe1ce17 | 259 | } |
shimniok | 0:9b27ebe1ce17 | 260 | pc.printf("\n"); |
shimniok | 0:9b27ebe1ce17 | 261 | */ |
shimniok | 0:9b27ebe1ce17 | 262 | |
shimniok | 0:9b27ebe1ce17 | 263 | char cmd; |
shimniok | 0:9b27ebe1ce17 | 264 | |
shimniok | 0:9b27ebe1ce17 | 265 | while (1) { |
shimniok | 0:9b27ebe1ce17 | 266 | |
shimniok | 0:9b27ebe1ce17 | 267 | if (autoBoot) { |
shimniok | 0:9b27ebe1ce17 | 268 | autoBoot = false; |
shimniok | 0:9b27ebe1ce17 | 269 | cmd = 'a'; |
shimniok | 0:9b27ebe1ce17 | 270 | } else { |
shimniok | 0:9b27ebe1ce17 | 271 | pc.printf("==============\nData Bus Menu\n==============\n"); |
shimniok | 0:9b27ebe1ce17 | 272 | pc.printf("(a)utonomous mode\n"); |
shimniok | 0:9b27ebe1ce17 | 273 | pc.printf("(s)erial bridge\n (1) Port 1 (2) Port 2 (GPS)\n"); |
shimniok | 0:9b27ebe1ce17 | 274 | pc.printf("(c)ompass calibration\n"); |
shimniok | 0:9b27ebe1ce17 | 275 | pc.printf("(i)nstrument check\n"); |
shimniok | 0:9b27ebe1ce17 | 276 | //pc.printf("(s)ervo calibration\n"); |
shimniok | 0:9b27ebe1ce17 | 277 | cmd = getInput("\nSelect from the above:"); |
shimniok | 0:9b27ebe1ce17 | 278 | } |
shimniok | 0:9b27ebe1ce17 | 279 | |
shimniok | 0:9b27ebe1ce17 | 280 | switch (cmd) { |
shimniok | 0:9b27ebe1ce17 | 281 | case 'a' : |
shimniok | 0:9b27ebe1ce17 | 282 | autonomousMode(); |
shimniok | 0:9b27ebe1ce17 | 283 | break; |
shimniok | 0:9b27ebe1ce17 | 284 | case 's' : |
shimniok | 0:9b27ebe1ce17 | 285 | // which GPS |
shimniok | 0:9b27ebe1ce17 | 286 | cmd = getInput("GPS2 (2) or AHRS(1)?"); |
shimniok | 0:9b27ebe1ce17 | 287 | switch (cmd) { |
shimniok | 0:9b27ebe1ce17 | 288 | case '1' : |
shimniok | 0:9b27ebe1ce17 | 289 | serialBridge(ahrs); |
shimniok | 0:9b27ebe1ce17 | 290 | break; |
shimniok | 0:9b27ebe1ce17 | 291 | case '2' : |
shimniok | 0:9b27ebe1ce17 | 292 | serialBridge(gps2); |
shimniok | 0:9b27ebe1ce17 | 293 | break; |
shimniok | 0:9b27ebe1ce17 | 294 | default : |
shimniok | 0:9b27ebe1ce17 | 295 | break; |
shimniok | 0:9b27ebe1ce17 | 296 | } |
shimniok | 0:9b27ebe1ce17 | 297 | break; |
shimniok | 0:9b27ebe1ce17 | 298 | case 'i' : |
shimniok | 0:9b27ebe1ce17 | 299 | instrumentCheck(); |
shimniok | 0:9b27ebe1ce17 | 300 | break; |
shimniok | 0:9b27ebe1ce17 | 301 | case 'c' : |
shimniok | 0:9b27ebe1ce17 | 302 | compassCalibrate(); |
shimniok | 0:9b27ebe1ce17 | 303 | break; |
shimniok | 0:9b27ebe1ce17 | 304 | default : |
shimniok | 0:9b27ebe1ce17 | 305 | break; |
shimniok | 0:9b27ebe1ce17 | 306 | } |
shimniok | 0:9b27ebe1ce17 | 307 | } |
shimniok | 0:9b27ebe1ce17 | 308 | |
shimniok | 0:9b27ebe1ce17 | 309 | } |
shimniok | 0:9b27ebe1ce17 | 310 | |
shimniok | 0:9b27ebe1ce17 | 311 | // Handle data from a GPS (there may be two GPS's so needed to put the code in one routine |
shimniok | 0:9b27ebe1ce17 | 312 | // |
shimniok | 0:9b27ebe1ce17 | 313 | void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time) |
shimniok | 0:9b27ebe1ce17 | 314 | { |
shimniok | 0:9b27ebe1ce17 | 315 | double lat, lon; |
shimniok | 0:9b27ebe1ce17 | 316 | unsigned long age; |
shimniok | 0:9b27ebe1ce17 | 317 | |
shimniok | 0:9b27ebe1ce17 | 318 | parse.reset_ready(); // reset the flags |
shimniok | 0:9b27ebe1ce17 | 319 | //pc.printf("%d GPS RMC are ready\n", millis); |
shimniok | 0:9b27ebe1ce17 | 320 | parse.f_get_position(&lat, &lon, &age); |
shimniok | 0:9b27ebe1ce17 | 321 | parse.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age); |
shimniok | 0:9b27ebe1ce17 | 322 | |
shimniok | 0:9b27ebe1ce17 | 323 | sprintf(date, "%02d/%02d/%4d", month, day, year); |
shimniok | 0:9b27ebe1ce17 | 324 | sprintf(time, "%02d:%02d:%02d.%03d", hour, minute, second, hundredths); |
shimniok | 0:9b27ebe1ce17 | 325 | |
shimniok | 0:9b27ebe1ce17 | 326 | float hdop = parse.f_hdop(); |
shimniok | 0:9b27ebe1ce17 | 327 | |
shimniok | 0:9b27ebe1ce17 | 328 | // Want to blink the LED when GPS update arrives |
shimniok | 0:9b27ebe1ce17 | 329 | // must toggle opposite |
shimniok | 0:9b27ebe1ce17 | 330 | //status = (hdop > 0.0 && hdop < 10.0) ? 0 : 1; |
shimniok | 0:9b27ebe1ce17 | 331 | |
shimniok | 0:9b27ebe1ce17 | 332 | // Bearing and distance to waypoint |
shimniok | 0:9b27ebe1ce17 | 333 | here.set(lat, lon); |
shimniok | 0:9b27ebe1ce17 | 334 | |
shimniok | 0:9b27ebe1ce17 | 335 | //pc.printf("HDOP: %.1f gyro: %d\n", gps1_hdop, gyro); |
shimniok | 0:9b27ebe1ce17 | 336 | |
shimniok | 0:9b27ebe1ce17 | 337 | return; |
shimniok | 0:9b27ebe1ce17 | 338 | } |
shimniok | 0:9b27ebe1ce17 | 339 | |
shimniok | 0:9b27ebe1ce17 | 340 | |
shimniok | 0:9b27ebe1ce17 | 341 | |
shimniok | 0:9b27ebe1ce17 | 342 | // convert character to an int |
shimniok | 0:9b27ebe1ce17 | 343 | // |
shimniok | 0:9b27ebe1ce17 | 344 | int ctoi(char c) |
shimniok | 0:9b27ebe1ce17 | 345 | { |
shimniok | 0:9b27ebe1ce17 | 346 | int i=-1; |
shimniok | 0:9b27ebe1ce17 | 347 | |
shimniok | 0:9b27ebe1ce17 | 348 | if (c >= '0' && c <= '9') { |
shimniok | 0:9b27ebe1ce17 | 349 | i = c - '0'; |
shimniok | 0:9b27ebe1ce17 | 350 | } |
shimniok | 0:9b27ebe1ce17 | 351 | |
shimniok | 0:9b27ebe1ce17 | 352 | //printf("char: %c int %d\n", c, i); |
shimniok | 0:9b27ebe1ce17 | 353 | |
shimniok | 0:9b27ebe1ce17 | 354 | return i; |
shimniok | 0:9b27ebe1ce17 | 355 | } |
shimniok | 0:9b27ebe1ce17 | 356 | |
shimniok | 0:9b27ebe1ce17 | 357 | |
shimniok | 0:9b27ebe1ce17 | 358 | // convert string to floating point |
shimniok | 0:9b27ebe1ce17 | 359 | // |
shimniok | 0:9b27ebe1ce17 | 360 | double cvstof(char *s) |
shimniok | 0:9b27ebe1ce17 | 361 | { |
shimniok | 0:9b27ebe1ce17 | 362 | double f=0.0; |
shimniok | 0:9b27ebe1ce17 | 363 | double mult = 0.1; |
shimniok | 0:9b27ebe1ce17 | 364 | bool neg = false; |
shimniok | 0:9b27ebe1ce17 | 365 | //char dec = 1; |
shimniok | 0:9b27ebe1ce17 | 366 | |
shimniok | 0:9b27ebe1ce17 | 367 | // leading spaces |
shimniok | 0:9b27ebe1ce17 | 368 | while (*s == ' ' || *s == '\t') { |
shimniok | 0:9b27ebe1ce17 | 369 | s++; |
shimniok | 0:9b27ebe1ce17 | 370 | if (*s == 0) break; |
shimniok | 0:9b27ebe1ce17 | 371 | } |
shimniok | 0:9b27ebe1ce17 | 372 | |
shimniok | 0:9b27ebe1ce17 | 373 | // What about negative numbers? |
shimniok | 0:9b27ebe1ce17 | 374 | if (*s == '-') { |
shimniok | 0:9b27ebe1ce17 | 375 | neg = true; |
shimniok | 0:9b27ebe1ce17 | 376 | s++; |
shimniok | 0:9b27ebe1ce17 | 377 | } |
shimniok | 0:9b27ebe1ce17 | 378 | |
shimniok | 0:9b27ebe1ce17 | 379 | // before the decimal |
shimniok | 0:9b27ebe1ce17 | 380 | // |
shimniok | 0:9b27ebe1ce17 | 381 | while (*s != 0) { |
shimniok | 0:9b27ebe1ce17 | 382 | if (*s == '.') { |
shimniok | 0:9b27ebe1ce17 | 383 | s++; |
shimniok | 0:9b27ebe1ce17 | 384 | break; |
shimniok | 0:9b27ebe1ce17 | 385 | } |
shimniok | 0:9b27ebe1ce17 | 386 | f = (f * 10.0) + (double) ctoi(*s); |
shimniok | 0:9b27ebe1ce17 | 387 | s++; |
shimniok | 0:9b27ebe1ce17 | 388 | } |
shimniok | 0:9b27ebe1ce17 | 389 | // after the decimal |
shimniok | 0:9b27ebe1ce17 | 390 | while (*s != 0 && *s >= '0' && *s <= '9') { |
shimniok | 0:9b27ebe1ce17 | 391 | f += (double) ctoi(*s) * mult; |
shimniok | 0:9b27ebe1ce17 | 392 | mult /= 10; |
shimniok | 0:9b27ebe1ce17 | 393 | s++; |
shimniok | 0:9b27ebe1ce17 | 394 | } |
shimniok | 0:9b27ebe1ce17 | 395 | |
shimniok | 0:9b27ebe1ce17 | 396 | // if we were negative... |
shimniok | 0:9b27ebe1ce17 | 397 | if (neg) f = -f; |
shimniok | 0:9b27ebe1ce17 | 398 | |
shimniok | 0:9b27ebe1ce17 | 399 | return f; |
shimniok | 0:9b27ebe1ce17 | 400 | } |
shimniok | 0:9b27ebe1ce17 | 401 | |
shimniok | 0:9b27ebe1ce17 | 402 | // copy t to s until delimiter is reached |
shimniok | 0:9b27ebe1ce17 | 403 | // return location of delimiter+1 in t |
shimniok | 0:9b27ebe1ce17 | 404 | char *split(char *s, char *t, int max, char delim) |
shimniok | 0:9b27ebe1ce17 | 405 | { |
shimniok | 0:9b27ebe1ce17 | 406 | int i = 0; |
shimniok | 0:9b27ebe1ce17 | 407 | |
shimniok | 0:9b27ebe1ce17 | 408 | if (s == 0 || t == 0) |
shimniok | 0:9b27ebe1ce17 | 409 | return 0; |
shimniok | 0:9b27ebe1ce17 | 410 | |
shimniok | 0:9b27ebe1ce17 | 411 | while (*t != 0 && *t != delim && i < max) { |
shimniok | 0:9b27ebe1ce17 | 412 | *s++ = *t++; |
shimniok | 0:9b27ebe1ce17 | 413 | i++; |
shimniok | 0:9b27ebe1ce17 | 414 | } |
shimniok | 0:9b27ebe1ce17 | 415 | *s = 0; |
shimniok | 0:9b27ebe1ce17 | 416 | |
shimniok | 0:9b27ebe1ce17 | 417 | return t+1; |
shimniok | 0:9b27ebe1ce17 | 418 | } |
shimniok | 0:9b27ebe1ce17 | 419 | |
shimniok | 0:9b27ebe1ce17 | 420 | #define MAXBUF 64 |
shimniok | 0:9b27ebe1ce17 | 421 | // load configuration from filesystem |
shimniok | 0:9b27ebe1ce17 | 422 | void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain) |
shimniok | 0:9b27ebe1ce17 | 423 | { |
shimniok | 0:9b27ebe1ce17 | 424 | // FILE *fp; |
shimniok | 0:9b27ebe1ce17 | 425 | char buf[MAXBUF]; // buffer to read in data |
shimniok | 0:9b27ebe1ce17 | 426 | char tmp[MAXBUF]; // temp buffer |
shimniok | 0:9b27ebe1ce17 | 427 | char *p; |
shimniok | 0:9b27ebe1ce17 | 428 | double lat, lon; |
shimniok | 0:9b27ebe1ce17 | 429 | bool declFound = false; |
shimniok | 0:9b27ebe1ce17 | 430 | |
shimniok | 0:9b27ebe1ce17 | 431 | // Just to be safe let's wait |
shimniok | 0:9b27ebe1ce17 | 432 | //wait(2.0); |
shimniok | 0:9b27ebe1ce17 | 433 | |
shimniok | 0:9b27ebe1ce17 | 434 | pc.printf("opening config file...\n"); |
shimniok | 0:9b27ebe1ce17 | 435 | |
shimniok | 0:9b27ebe1ce17 | 436 | fp = fopen("/etc/config.txt", "r"); |
shimniok | 0:9b27ebe1ce17 | 437 | if (fp == 0) { |
shimniok | 0:9b27ebe1ce17 | 438 | pc.printf("Could not open config.txt\n"); |
shimniok | 0:9b27ebe1ce17 | 439 | } else { |
shimniok | 0:9b27ebe1ce17 | 440 | wptCount = 0; |
shimniok | 0:9b27ebe1ce17 | 441 | declination = 0.0; |
shimniok | 0:9b27ebe1ce17 | 442 | while (!feof(fp)) { |
shimniok | 0:9b27ebe1ce17 | 443 | fgets(buf, MAXBUF-1, fp); |
shimniok | 0:9b27ebe1ce17 | 444 | p = split(tmp, buf, MAXBUF, ','); // split off the first field |
shimniok | 0:9b27ebe1ce17 | 445 | switch (tmp[0]) { |
shimniok | 0:9b27ebe1ce17 | 446 | case 'W' : // Waypoint |
shimniok | 0:9b27ebe1ce17 | 447 | p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp |
shimniok | 0:9b27ebe1ce17 | 448 | lat = cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 449 | p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp |
shimniok | 0:9b27ebe1ce17 | 450 | lon = cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 451 | if (wptCount < MAXWPT) { |
shimniok | 0:9b27ebe1ce17 | 452 | wpt[wptCount].set(lat, lon); |
shimniok | 0:9b27ebe1ce17 | 453 | wptCount++; |
shimniok | 0:9b27ebe1ce17 | 454 | } |
shimniok | 0:9b27ebe1ce17 | 455 | break; |
shimniok | 0:9b27ebe1ce17 | 456 | case 'G' : // Gyro Bias |
shimniok | 0:9b27ebe1ce17 | 457 | p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp |
shimniok | 0:9b27ebe1ce17 | 458 | gyroBias = (float) cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 459 | break; |
shimniok | 0:9b27ebe1ce17 | 460 | case 'D' : // Compass Declination |
shimniok | 0:9b27ebe1ce17 | 461 | p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp |
shimniok | 0:9b27ebe1ce17 | 462 | declination = (float) cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 463 | declFound = true; |
shimniok | 0:9b27ebe1ce17 | 464 | break; |
shimniok | 0:9b27ebe1ce17 | 465 | case 'I' : // Intercept distance |
shimniok | 0:9b27ebe1ce17 | 466 | p = split(tmp, p, MAXBUF, ','); // split off the number to tmp |
shimniok | 0:9b27ebe1ce17 | 467 | interceptDist = (float) cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 468 | break; |
shimniok | 0:9b27ebe1ce17 | 469 | case 'S' : // Speed maximum |
shimniok | 0:9b27ebe1ce17 | 470 | p = split(tmp, p, MAXBUF, ','); // split off the number to tmp |
shimniok | 0:9b27ebe1ce17 | 471 | maxSpeed = atoi(tmp); |
shimniok | 0:9b27ebe1ce17 | 472 | //pc.printf("tmp:%s maxSpeed:%d\n", tmp, maxSpeed); |
shimniok | 0:9b27ebe1ce17 | 473 | break; |
shimniok | 0:9b27ebe1ce17 | 474 | case 'E' : |
shimniok | 0:9b27ebe1ce17 | 475 | p = split(tmp, p, MAXBUF, ','); // split off the number to tmp |
shimniok | 0:9b27ebe1ce17 | 476 | compassGain = (float) cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 477 | p = split(tmp, p, MAXBUF, ','); // split off the number to tmp |
shimniok | 0:9b27ebe1ce17 | 478 | yawGain = (float) cvstof(tmp); |
shimniok | 0:9b27ebe1ce17 | 479 | default : |
shimniok | 0:9b27ebe1ce17 | 480 | break; |
shimniok | 0:9b27ebe1ce17 | 481 | } // switch |
shimniok | 0:9b27ebe1ce17 | 482 | } // while |
shimniok | 0:9b27ebe1ce17 | 483 | |
shimniok | 0:9b27ebe1ce17 | 484 | // Did we get the values we were looking for? |
shimniok | 0:9b27ebe1ce17 | 485 | if (wptCount > 0 && declFound) { |
shimniok | 0:9b27ebe1ce17 | 486 | confStatus = 1; |
shimniok | 0:9b27ebe1ce17 | 487 | } |
shimniok | 0:9b27ebe1ce17 | 488 | |
shimniok | 0:9b27ebe1ce17 | 489 | } // if fp |
shimniok | 0:9b27ebe1ce17 | 490 | |
shimniok | 0:9b27ebe1ce17 | 491 | if (fp != 0) |
shimniok | 0:9b27ebe1ce17 | 492 | fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 493 | |
shimniok | 0:9b27ebe1ce17 | 494 | pc.printf("Intercept Dist: %.1f\n", interceptDist); |
shimniok | 0:9b27ebe1ce17 | 495 | pc.printf("Declination: %.1f\n", declination); |
shimniok | 0:9b27ebe1ce17 | 496 | pc.printf("MaxSpeed: %d\n", maxSpeed); |
shimniok | 0:9b27ebe1ce17 | 497 | pc.printf("CompassGain; %.3f YawGain: %.3f\n", compassGain, yawGain); |
shimniok | 0:9b27ebe1ce17 | 498 | for (int w = 0; w < MAXWPT && w < wptCount; w++) { |
shimniok | 0:9b27ebe1ce17 | 499 | pc.printf("Waypoint #%d lat: %.6f lon: %.6f\n", w, wpt[w].latitude(), wpt[w].longitude()); |
shimniok | 0:9b27ebe1ce17 | 500 | } |
shimniok | 0:9b27ebe1ce17 | 501 | |
shimniok | 0:9b27ebe1ce17 | 502 | } |
shimniok | 0:9b27ebe1ce17 | 503 | |
shimniok | 0:9b27ebe1ce17 | 504 | |
shimniok | 0:9b27ebe1ce17 | 505 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 506 | // INITIALIZATION ROUTINES |
shimniok | 0:9b27ebe1ce17 | 507 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 508 | |
shimniok | 0:9b27ebe1ce17 | 509 | // Find the next unused filename of the form logger##.csv where # is 0-9 |
shimniok | 0:9b27ebe1ce17 | 510 | // |
shimniok | 0:9b27ebe1ce17 | 511 | FILE *initLogfile() |
shimniok | 0:9b27ebe1ce17 | 512 | { |
shimniok | 0:9b27ebe1ce17 | 513 | FILE *fp = 0; |
shimniok | 0:9b27ebe1ce17 | 514 | char myname[64]; |
shimniok | 0:9b27ebe1ce17 | 515 | |
shimniok | 0:9b27ebe1ce17 | 516 | pc.printf("Opening log file...\n"); |
shimniok | 0:9b27ebe1ce17 | 517 | |
shimniok | 0:9b27ebe1ce17 | 518 | while (fp == 0) { |
shimniok | 0:9b27ebe1ce17 | 519 | if ((fp = fopen("/log/test.txt", "r")) == 0) { |
shimniok | 0:9b27ebe1ce17 | 520 | pc.printf("Waiting for filesystem to come online..."); |
shimniok | 0:9b27ebe1ce17 | 521 | wait(0.200); |
shimniok | 0:9b27ebe1ce17 | 522 | } |
shimniok | 0:9b27ebe1ce17 | 523 | } |
shimniok | 0:9b27ebe1ce17 | 524 | fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 525 | |
shimniok | 0:9b27ebe1ce17 | 526 | for (int i = 0; i < 1000; i++) { |
shimniok | 0:9b27ebe1ce17 | 527 | sprintf(myname, "/log/log%04d.csv", i); |
shimniok | 0:9b27ebe1ce17 | 528 | //pc.printf("Try file: <%s>\n", myname); |
shimniok | 0:9b27ebe1ce17 | 529 | if ((fp = fopen(myname, "r")) == 0) { |
shimniok | 0:9b27ebe1ce17 | 530 | //pc.printf("File not found: <%s>\n", myname); |
shimniok | 0:9b27ebe1ce17 | 531 | break; |
shimniok | 0:9b27ebe1ce17 | 532 | } else { |
shimniok | 0:9b27ebe1ce17 | 533 | //pc.printf("File exists: <%s>\n", myname); |
shimniok | 0:9b27ebe1ce17 | 534 | fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 535 | } |
shimniok | 0:9b27ebe1ce17 | 536 | } |
shimniok | 0:9b27ebe1ce17 | 537 | |
shimniok | 0:9b27ebe1ce17 | 538 | fp = fopen(myname, "w"); |
shimniok | 0:9b27ebe1ce17 | 539 | if (fp == 0) { |
shimniok | 0:9b27ebe1ce17 | 540 | pc.printf("file write failed: %s\n", myname); |
shimniok | 0:9b27ebe1ce17 | 541 | } else { |
shimniok | 0:9b27ebe1ce17 | 542 | //status = true; |
shimniok | 0:9b27ebe1ce17 | 543 | pc.printf("opened %s for writing\n", myname); |
shimniok | 0:9b27ebe1ce17 | 544 | fprintf(fp, "Millis, Gyro, GyroHdg, CompassHeading, Latitude, Longitude, Age, Date, Time, Altitude, Course, Speed, HDOP, Bearing, Distance, Left Enc, Right Enc, Gyro Temp, OdoHdg\n"); |
shimniok | 0:9b27ebe1ce17 | 545 | //fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 546 | } |
shimniok | 0:9b27ebe1ce17 | 547 | |
shimniok | 0:9b27ebe1ce17 | 548 | return fp; |
shimniok | 0:9b27ebe1ce17 | 549 | } |
shimniok | 0:9b27ebe1ce17 | 550 | |
shimniok | 0:9b27ebe1ce17 | 551 | |
shimniok | 0:9b27ebe1ce17 | 552 | void initButtons() |
shimniok | 0:9b27ebe1ce17 | 553 | { |
shimniok | 0:9b27ebe1ce17 | 554 | // Set up button (plugs into two GPIOs, active low |
shimniok | 0:9b27ebe1ce17 | 555 | buttonPower = 0; |
shimniok | 0:9b27ebe1ce17 | 556 | selectButton.mode(PullUp); |
shimniok | 0:9b27ebe1ce17 | 557 | selectButton.setSamplesTillAssert(20); |
shimniok | 0:9b27ebe1ce17 | 558 | selectButton.setAssertValue(0); // active low logic |
shimniok | 0:9b27ebe1ce17 | 559 | selectButton.setSampleFrequency(20); // 20us |
shimniok | 0:9b27ebe1ce17 | 560 | selectButton.attach_asserted( &assertButton ); |
shimniok | 0:9b27ebe1ce17 | 561 | selectButton.attach_deasserted( &deassertButton ); |
shimniok | 0:9b27ebe1ce17 | 562 | } |
shimniok | 0:9b27ebe1ce17 | 563 | |
shimniok | 0:9b27ebe1ce17 | 564 | void initCompass() |
shimniok | 0:9b27ebe1ce17 | 565 | { |
shimniok | 0:9b27ebe1ce17 | 566 | // Initialize compass; continuous mode, periodic set/reset, 20Hz measurement rate. |
shimniok | 0:9b27ebe1ce17 | 567 | //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
shimniok | 0:9b27ebe1ce17 | 568 | |
shimniok | 0:9b27ebe1ce17 | 569 | // Set calibration parameters for this particular LSM303DLH |
shimniok | 0:9b27ebe1ce17 | 570 | //compass3d.setOffset(29.50, -0.50, 4.00); |
shimniok | 0:9b27ebe1ce17 | 571 | //compass3d.setScale(1.00, 1.03, 1.21); |
shimniok | 0:9b27ebe1ce17 | 572 | compass3d.setOffset(44.50, 5.00, -0.50); // Apr 11 testing |
shimniok | 0:9b27ebe1ce17 | 573 | compass3d.setScale(1.00, 1.04, 1.29); // Apr 11 testing |
shimniok | 0:9b27ebe1ce17 | 574 | |
shimniok | 0:9b27ebe1ce17 | 575 | //compass3d._debug = &pc; |
shimniok | 0:9b27ebe1ce17 | 576 | } |
shimniok | 0:9b27ebe1ce17 | 577 | |
shimniok | 0:9b27ebe1ce17 | 578 | |
shimniok | 0:9b27ebe1ce17 | 579 | void initGPS() |
shimniok | 0:9b27ebe1ce17 | 580 | { |
shimniok | 0:9b27ebe1ce17 | 581 | // Initialize the GPS comm and handler |
shimniok | 0:9b27ebe1ce17 | 582 | //gps1.baud(57600); // LOCOSYS LS20031 |
shimniok | 0:9b27ebe1ce17 | 583 | |
shimniok | 0:9b27ebe1ce17 | 584 | // Set LCD baud rate ; has to be 4800 |
shimniok | 0:9b27ebe1ce17 | 585 | // because we share with 4800 bps GPS\ |
shimniok | 0:9b27ebe1ce17 | 586 | // send chr(124) and CTRL-L |
shimniok | 0:9b27ebe1ce17 | 587 | //gps2.baud(9600); |
shimniok | 0:9b27ebe1ce17 | 588 | //gps2.printf("Data Bus"); |
shimniok | 0:9b27ebe1ce17 | 589 | //gps2.printf("%c%c", 124, 12); |
shimniok | 0:9b27ebe1ce17 | 590 | |
shimniok | 0:9b27ebe1ce17 | 591 | gps2.baud(4800); // Pharos iGPS-500 |
shimniok | 0:9b27ebe1ce17 | 592 | |
shimniok | 0:9b27ebe1ce17 | 593 | // Synchronize with GPS |
shimniok | 0:9b27ebe1ce17 | 594 | //gps1Parse.reset_ready(); |
shimniok | 0:9b27ebe1ce17 | 595 | gps2Parse.reset_ready(); |
shimniok | 0:9b27ebe1ce17 | 596 | } |
shimniok | 0:9b27ebe1ce17 | 597 | |
shimniok | 0:9b27ebe1ce17 | 598 | void initAHRS() |
shimniok | 0:9b27ebe1ce17 | 599 | { |
shimniok | 0:9b27ebe1ce17 | 600 | char data[MAX_BYTES]; |
shimniok | 0:9b27ebe1ce17 | 601 | int status; |
shimniok | 0:9b27ebe1ce17 | 602 | int ok = 0; // counts number of command_complete messages |
shimniok | 0:9b27ebe1ce17 | 603 | int c = 10; // timeout for status |
shimniok | 0:9b27ebe1ce17 | 604 | |
shimniok | 0:9b27ebe1ce17 | 605 | ahrs.baud(115200); |
shimniok | 0:9b27ebe1ce17 | 606 | ahrs.attach(recv1, Serial::RxIrq); |
shimniok | 0:9b27ebe1ce17 | 607 | wait(0.5); |
shimniok | 0:9b27ebe1ce17 | 608 | |
shimniok | 0:9b27ebe1ce17 | 609 | // Configure AHRS to use only acceleromters and gyro, no magnetometer |
shimniok | 0:9b27ebe1ce17 | 610 | data[0] = Accel_EN; |
shimniok | 0:9b27ebe1ce17 | 611 | ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data); |
shimniok | 0:9b27ebe1ce17 | 612 | c = 10; |
shimniok | 0:9b27ebe1ce17 | 613 | while (!ahrsParser.statusReady() && c-- > 0) |
shimniok | 0:9b27ebe1ce17 | 614 | wait(0.1); |
shimniok | 0:9b27ebe1ce17 | 615 | status = ahrsParser.status(); |
shimniok | 0:9b27ebe1ce17 | 616 | if (status == PT_COMMAND_COMPLETE) ok++; |
shimniok | 0:9b27ebe1ce17 | 617 | pc.printf("SET_EKF_CONFIG: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:9b27ebe1ce17 | 618 | |
shimniok | 0:9b27ebe1ce17 | 619 | ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0); |
shimniok | 0:9b27ebe1ce17 | 620 | c = 10; |
shimniok | 0:9b27ebe1ce17 | 621 | while (!ahrsParser.statusReady() && c-- > 0) |
shimniok | 0:9b27ebe1ce17 | 622 | wait(0.1); |
shimniok | 0:9b27ebe1ce17 | 623 | status = ahrsParser.status(); |
shimniok | 0:9b27ebe1ce17 | 624 | if (status == PT_COMMAND_COMPLETE) ok++; |
shimniok | 0:9b27ebe1ce17 | 625 | pc.printf("ZERO_RATE_GYROS: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:9b27ebe1ce17 | 626 | |
shimniok | 0:9b27ebe1ce17 | 627 | data[0] = 0; // 20Hz |
shimniok | 0:9b27ebe1ce17 | 628 | ahrsParser.send_packet(&ahrs, SET_BROADCAST_MODE, 1, data); |
shimniok | 0:9b27ebe1ce17 | 629 | c = 10; |
shimniok | 0:9b27ebe1ce17 | 630 | while (!ahrsParser.statusReady() && c-- > 0) |
shimniok | 0:9b27ebe1ce17 | 631 | wait(0.1); |
shimniok | 0:9b27ebe1ce17 | 632 | status = ahrsParser.status(); |
shimniok | 0:9b27ebe1ce17 | 633 | if (status == PT_COMMAND_COMPLETE) ok++; |
shimniok | 0:9b27ebe1ce17 | 634 | pc.printf("SET_BROADCAST_MODE: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:9b27ebe1ce17 | 635 | |
shimniok | 0:9b27ebe1ce17 | 636 | ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0); |
shimniok | 0:9b27ebe1ce17 | 637 | c = 10; |
shimniok | 0:9b27ebe1ce17 | 638 | while (!ahrsParser.statusReady() && c-- > 0) |
shimniok | 0:9b27ebe1ce17 | 639 | wait(0.1); |
shimniok | 0:9b27ebe1ce17 | 640 | status = ahrsParser.status(); |
shimniok | 0:9b27ebe1ce17 | 641 | if (status == PT_COMMAND_COMPLETE) ok++; |
shimniok | 0:9b27ebe1ce17 | 642 | pc.printf("EKF_RESET: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:9b27ebe1ce17 | 643 | //ahrs.printf("%s", ahrsParser.send_packet()) // <--mag calibration here// |
shimniok | 0:9b27ebe1ce17 | 644 | |
shimniok | 0:9b27ebe1ce17 | 645 | if (ok == 4) ahrsStatus = 1; // turn on status LED |
shimniok | 0:9b27ebe1ce17 | 646 | |
shimniok | 0:9b27ebe1ce17 | 647 | ahrsParser.resetReady(); |
shimniok | 0:9b27ebe1ce17 | 648 | } |
shimniok | 0:9b27ebe1ce17 | 649 | |
shimniok | 0:9b27ebe1ce17 | 650 | |
shimniok | 0:9b27ebe1ce17 | 651 | void initDR() |
shimniok | 0:9b27ebe1ce17 | 652 | { |
shimniok | 0:9b27ebe1ce17 | 653 | dr_here.set(wpt[0]); // Initialize Dead Reckoning to starting waypoint |
shimniok | 0:9b27ebe1ce17 | 654 | dr_here_last.set(wpt[0]); |
shimniok | 0:9b27ebe1ce17 | 655 | } |
shimniok | 0:9b27ebe1ce17 | 656 | |
shimniok | 0:9b27ebe1ce17 | 657 | |
shimniok | 0:9b27ebe1ce17 | 658 | void initFlasher() |
shimniok | 0:9b27ebe1ce17 | 659 | { |
shimniok | 0:9b27ebe1ce17 | 660 | // Set up flasher schedule; 3 flashes every 80ms |
shimniok | 0:9b27ebe1ce17 | 661 | // for 80ms total, with a 9x80ms period |
shimniok | 0:9b27ebe1ce17 | 662 | blink.max(9); |
shimniok | 0:9b27ebe1ce17 | 663 | blink.scale(80); |
shimniok | 0:9b27ebe1ce17 | 664 | blink.mode(Schedule::repeat); |
shimniok | 0:9b27ebe1ce17 | 665 | blink.set(0, 1); blink.set(2, 1); blink.set(4, 1); |
shimniok | 0:9b27ebe1ce17 | 666 | } |
shimniok | 0:9b27ebe1ce17 | 667 | |
shimniok | 0:9b27ebe1ce17 | 668 | |
shimniok | 0:9b27ebe1ce17 | 669 | void initSteering() |
shimniok | 0:9b27ebe1ce17 | 670 | { |
shimniok | 0:9b27ebe1ce17 | 671 | // Setup steering servo |
shimniok | 0:9b27ebe1ce17 | 672 | steering = 0.5; |
shimniok | 0:9b27ebe1ce17 | 673 | steering.calibrate(0.005, 45.0); |
shimniok | 0:9b27ebe1ce17 | 674 | } |
shimniok | 0:9b27ebe1ce17 | 675 | |
shimniok | 0:9b27ebe1ce17 | 676 | |
shimniok | 0:9b27ebe1ce17 | 677 | void initThrottle() |
shimniok | 0:9b27ebe1ce17 | 678 | { |
shimniok | 0:9b27ebe1ce17 | 679 | throttle = 0.5; |
shimniok | 0:9b27ebe1ce17 | 680 | throttle.calibrate(0.0005, 45.0); |
shimniok | 0:9b27ebe1ce17 | 681 | } |
shimniok | 0:9b27ebe1ce17 | 682 | |
shimniok | 0:9b27ebe1ce17 | 683 | |
shimniok | 0:9b27ebe1ce17 | 684 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 685 | // OPERATIONAL MODE FUNCTIONS |
shimniok | 0:9b27ebe1ce17 | 686 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 687 | |
shimniok | 0:9b27ebe1ce17 | 688 | |
shimniok | 0:9b27ebe1ce17 | 689 | void autonomousMode() |
shimniok | 0:9b27ebe1ce17 | 690 | { |
shimniok | 0:9b27ebe1ce17 | 691 | // Navigation |
shimniok | 0:9b27ebe1ce17 | 692 | float interceptDist = 1.5; // Course correction intercept distance ; GPS_MAX_HDOP calculates to 20* SA |
shimniok | 0:9b27ebe1ce17 | 693 | |
shimniok | 0:9b27ebe1ce17 | 694 | // Compass Variables |
shimniok | 0:9b27ebe1ce17 | 695 | float compassHdg = 0.0; // compass heading |
shimniok | 0:9b27ebe1ce17 | 696 | float compassHdg_last = -999; // heading at last GPS packet |
shimniok | 0:9b27ebe1ce17 | 697 | float compassGain = COMPASS_GAIN; |
shimniok | 0:9b27ebe1ce17 | 698 | |
shimniok | 0:9b27ebe1ce17 | 699 | // AHRS Variables |
shimniok | 0:9b27ebe1ce17 | 700 | float yaw = 0.0; // course |
shimniok | 0:9b27ebe1ce17 | 701 | float yawHdg = -1.0; // heading calculated from AHRS |
shimniok | 0:9b27ebe1ce17 | 702 | float yawRate = 0.0; // calculated rate of change in yaw |
shimniok | 0:9b27ebe1ce17 | 703 | float initialHdg = -999; // initial heading (course) |
shimniok | 0:9b27ebe1ce17 | 704 | float yawGain = YAW_GAIN; |
shimniok | 0:9b27ebe1ce17 | 705 | |
shimniok | 0:9b27ebe1ce17 | 706 | // Variables we calculate |
shimniok | 0:9b27ebe1ce17 | 707 | double bearing = 0.0; // bearing to next waypoint |
shimniok | 0:9b27ebe1ce17 | 708 | double distance = 0.0; // distance to next waypoint |
shimniok | 0:9b27ebe1ce17 | 709 | float heading = 0.0; // estimated heading |
shimniok | 0:9b27ebe1ce17 | 710 | float relativeBrg = 0.0; // relative bearing to next waypoint |
shimniok | 0:9b27ebe1ce17 | 711 | double gps2_bearing = 0.0; // bearing, gps2 |
shimniok | 0:9b27ebe1ce17 | 712 | double gps2_distance = 0.0; // distance, gps2 |
shimniok | 0:9b27ebe1ce17 | 713 | float gps2_heading = 0.0; // current heading, gps2 |
shimniok | 0:9b27ebe1ce17 | 714 | double dr_bearing = 0.0; // bearing, dead reckoning |
shimniok | 0:9b27ebe1ce17 | 715 | double dr_distance = 0.0; // distance, dead reckoning |
shimniok | 0:9b27ebe1ce17 | 716 | float dr_heading = 0.0; // dead reckoning heading |
shimniok | 0:9b27ebe1ce17 | 717 | float odo_heading = 0.0; // heading calc from odometry |
shimniok | 0:9b27ebe1ce17 | 718 | double err_bearing = 0.0; // bearing between dr and gps |
shimniok | 0:9b27ebe1ce17 | 719 | double err_distance = 0.0; // distance between dr and gps |
shimniok | 0:9b27ebe1ce17 | 720 | float err_yaw = 0.0; // error in ahrs yaw versus gps heading. |
shimniok | 0:9b27ebe1ce17 | 721 | float err_compass = 0.0; // error in compass versus gps heading |
shimniok | 0:9b27ebe1ce17 | 722 | double gps2_off_dist = 0.0; // distance from gps fix to starting point |
shimniok | 0:9b27ebe1ce17 | 723 | double gps2_off_brg = 0.0; // bearing from gps fix to starting point |
shimniok | 0:9b27ebe1ce17 | 724 | double encDistance = 0.0; // encoder average distance recorded |
shimniok | 0:9b27ebe1ce17 | 725 | double encSpeed = 0.0; // encoder calculated speed |
shimniok | 0:9b27ebe1ce17 | 726 | |
shimniok | 0:9b27ebe1ce17 | 727 | goGoGo = false; |
shimniok | 0:9b27ebe1ce17 | 728 | |
shimniok | 0:9b27ebe1ce17 | 729 | loadConfig(interceptDist, declination, compassGain, yawGain); // Load the waypoints, declination, and other config stuff |
shimniok | 0:9b27ebe1ce17 | 730 | steerCalc.setIntercept(interceptDist); // Setup steering calculator based on intercept distance |
shimniok | 0:9b27ebe1ce17 | 731 | |
shimniok | 0:9b27ebe1ce17 | 732 | go.set(UPDATE_PERIOD, 20, 650, maxSpeed, Schedule::hold); // Set throttle profile |
shimniok | 0:9b27ebe1ce17 | 733 | //stop.set(UPDATE_PERIOD, 10, 500, 400, Schedule::hold); // Set brake profile |
shimniok | 0:9b27ebe1ce17 | 734 | // TODO: Parameterize max brake |
shimniok | 0:9b27ebe1ce17 | 735 | // obstacle: brake, speed up |
shimniok | 0:9b27ebe1ce17 | 736 | // turn: brake or slowdown, speed up again |
shimniok | 0:9b27ebe1ce17 | 737 | // brake speed up |
shimniok | 0:9b27ebe1ce17 | 738 | |
shimniok | 0:9b27ebe1ce17 | 739 | initAHRS(); |
shimniok | 0:9b27ebe1ce17 | 740 | |
shimniok | 0:9b27ebe1ce17 | 741 | wptCurrent = 0; |
shimniok | 0:9b27ebe1ce17 | 742 | initDR(); // initalize dead reckoning |
shimniok | 0:9b27ebe1ce17 | 743 | wptCurrent++; // Point to the next waypoint; first wpt is the starting point |
shimniok | 0:9b27ebe1ce17 | 744 | done = false; |
shimniok | 0:9b27ebe1ce17 | 745 | buttonReleased = buttonPressed = false; |
shimniok | 0:9b27ebe1ce17 | 746 | bool started = false; // flag to indicate robot has exceeded min speed. |
shimniok | 0:9b27ebe1ce17 | 747 | |
shimniok | 0:9b27ebe1ce17 | 748 | // TODO--make sure all variables are initialized here |
shimniok | 0:9b27ebe1ce17 | 749 | yawHdg = -1.0; |
shimniok | 0:9b27ebe1ce17 | 750 | gyroCount = 0; |
shimniok | 0:9b27ebe1ce17 | 751 | err_bearing = 0.0; |
shimniok | 0:9b27ebe1ce17 | 752 | err_distance = 0.0; |
shimniok | 0:9b27ebe1ce17 | 753 | err_compass = 0.0; |
shimniok | 0:9b27ebe1ce17 | 754 | err_yaw = 0.0; |
shimniok | 0:9b27ebe1ce17 | 755 | |
shimniok | 0:9b27ebe1ce17 | 756 | |
shimniok | 0:9b27ebe1ce17 | 757 | fp = initLogfile(); // Open the log file in sprintf format string style; numbers go in %d |
shimniok | 0:9b27ebe1ce17 | 758 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 759 | |
shimniok | 0:9b27ebe1ce17 | 760 | //gps1.attach(recv1, Serial::RxIrq); |
shimniok | 0:9b27ebe1ce17 | 761 | ahrs.attach(recv1, Serial::RxIrq); |
shimniok | 0:9b27ebe1ce17 | 762 | gps2.attach(recv2, Serial::RxIrq); |
shimniok | 0:9b27ebe1ce17 | 763 | |
shimniok | 0:9b27ebe1ce17 | 764 | // Figure out the "offset" or "bias" of the GPS reading |
shimniok | 0:9b27ebe1ce17 | 765 | // since we know the robot's starting point |
shimniok | 0:9b27ebe1ce17 | 766 | |
shimniok | 0:9b27ebe1ce17 | 767 | // 20110419 1052 |
shimniok | 0:9b27ebe1ce17 | 768 | findGPSBias(&gps2_off_brg, &gps2_off_dist, 2, wpt[0]); |
shimniok | 0:9b27ebe1ce17 | 769 | |
shimniok | 0:9b27ebe1ce17 | 770 | // Find average compass heading and use for |
shimniok | 0:9b27ebe1ce17 | 771 | // initializing base heading used by AHRS gyro+accel |
shimniok | 0:9b27ebe1ce17 | 772 | // Compass LSM303DLH runs at 15Hz = .06666 sec |
shimniok | 0:9b27ebe1ce17 | 773 | pc.printf("Computing initial yaw heading...\n"); |
shimniok | 0:9b27ebe1ce17 | 774 | initialHdg = 0.0; |
shimniok | 0:9b27ebe1ce17 | 775 | for (int i = 0; i < 10; i++) { |
shimniok | 0:9b27ebe1ce17 | 776 | float hdg = compassHeading(); |
shimniok | 0:9b27ebe1ce17 | 777 | initialHdg += hdg / 10.0; |
shimniok | 0:9b27ebe1ce17 | 778 | pc.printf("Compass: %.2f Initial Heading: %.2f\n", hdg, initialHdg); |
shimniok | 0:9b27ebe1ce17 | 779 | wait(0.05); |
shimniok | 0:9b27ebe1ce17 | 780 | } |
shimniok | 0:9b27ebe1ce17 | 781 | initialHdg = 91.0; // hard code due to issues |
shimniok | 0:9b27ebe1ce17 | 782 | pc.printf("Initial Heading: %.2f\n", initialHdg); |
shimniok | 0:9b27ebe1ce17 | 783 | odo_heading = initialHdg; |
shimniok | 0:9b27ebe1ce17 | 784 | |
shimniok | 0:9b27ebe1ce17 | 785 | //while (!ahrsParser.dataReady()); // sync to ahrs |
shimniok | 0:9b27ebe1ce17 | 786 | //ahrsParser.resetReady(); |
shimniok | 0:9b27ebe1ce17 | 787 | timer.reset(); // Keep track of milliseconds, elapsed |
shimniok | 0:9b27ebe1ce17 | 788 | timer.start(); |
shimniok | 0:9b27ebe1ce17 | 789 | |
shimniok | 0:9b27ebe1ce17 | 790 | float lastYaw = ahrsParser.readYaw(); // try to capture rate of change |
shimniok | 0:9b27ebe1ce17 | 791 | float initialHdgFilt = initialHdg; // initialize the initial heading filter |
shimniok | 0:9b27ebe1ce17 | 792 | bool gpsSync = false; // Allows us to sync closer to actual fix time |
shimniok | 0:9b27ebe1ce17 | 793 | int syncTime = 0; // stores next millisecond count at which to save DR data |
shimniok | 0:9b27ebe1ce17 | 794 | bool firstGPS = 0; // is this the first GPS packet? If so, don't calc errors |
shimniok | 0:9b27ebe1ce17 | 795 | |
shimniok | 0:9b27ebe1ce17 | 796 | // Main loop |
shimniok | 0:9b27ebe1ce17 | 797 | // |
shimniok | 0:9b27ebe1ce17 | 798 | while(done == false) { |
shimniok | 0:9b27ebe1ce17 | 799 | |
shimniok | 0:9b27ebe1ce17 | 800 | int millis = timer.read_ms(); |
shimniok | 0:9b27ebe1ce17 | 801 | |
shimniok | 0:9b27ebe1ce17 | 802 | // reset led status; allows blinking, 10ms long |
shimniok | 0:9b27ebe1ce17 | 803 | if ((millis % UPDATE_PERIOD) == 10) { |
shimniok | 0:9b27ebe1ce17 | 804 | logStatus = (fp != 0) ? 1 : 0; |
shimniok | 0:9b27ebe1ce17 | 805 | //ahrsStatus = (gps1Parse.f_hdop() > 0.0 && gps1Parse.f_hdop() < 10.0) ? 1 : 0; |
shimniok | 0:9b27ebe1ce17 | 806 | gps2Status = (gps2Parse.f_hdop() > 0.0 && gps2Parse.f_hdop() < 10.0) ? 1 : 0; |
shimniok | 0:9b27ebe1ce17 | 807 | } |
shimniok | 0:9b27ebe1ce17 | 808 | |
shimniok | 0:9b27ebe1ce17 | 809 | // Warning flasher |
shimniok | 0:9b27ebe1ce17 | 810 | //if (blink.ticked(millis)) { |
shimniok | 0:9b27ebe1ce17 | 811 | // flasher = blink.get(); |
shimniok | 0:9b27ebe1ce17 | 812 | //} |
shimniok | 0:9b27ebe1ce17 | 813 | |
shimniok | 0:9b27ebe1ce17 | 814 | // Button state machine |
shimniok | 0:9b27ebe1ce17 | 815 | // if we've not started going, button starts us |
shimniok | 0:9b27ebe1ce17 | 816 | // if we have started going, button stops us |
shimniok | 0:9b27ebe1ce17 | 817 | // but only if we've released it first |
shimniok | 0:9b27ebe1ce17 | 818 | // |
shimniok | 0:9b27ebe1ce17 | 819 | // set throttle only if goGoGo set |
shimniok | 0:9b27ebe1ce17 | 820 | if (goGoGo) { |
shimniok | 0:9b27ebe1ce17 | 821 | if (go.ticked(millis)) { |
shimniok | 0:9b27ebe1ce17 | 822 | throttle = go.get() / 1000.0; |
shimniok | 0:9b27ebe1ce17 | 823 | //pc.printf("throttle: %0.3f\n", throttle.read()); |
shimniok | 0:9b27ebe1ce17 | 824 | } |
shimniok | 0:9b27ebe1ce17 | 825 | if (buttonPressed && started) { |
shimniok | 0:9b27ebe1ce17 | 826 | pc.printf(">>>>>>>>>>>>>>>>>>>>>>> HALT\n"); |
shimniok | 0:9b27ebe1ce17 | 827 | done = true; |
shimniok | 0:9b27ebe1ce17 | 828 | goGoGo = false; |
shimniok | 0:9b27ebe1ce17 | 829 | } |
shimniok | 0:9b27ebe1ce17 | 830 | } else { |
shimniok | 0:9b27ebe1ce17 | 831 | if (buttonPressed == true) { |
shimniok | 0:9b27ebe1ce17 | 832 | pc.printf(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n"); |
shimniok | 0:9b27ebe1ce17 | 833 | goGoGo = true; |
shimniok | 0:9b27ebe1ce17 | 834 | buttonPressed = false; |
shimniok | 0:9b27ebe1ce17 | 835 | } |
shimniok | 0:9b27ebe1ce17 | 836 | } |
shimniok | 0:9b27ebe1ce17 | 837 | |
shimniok | 0:9b27ebe1ce17 | 838 | |
shimniok | 0:9b27ebe1ce17 | 839 | // UPDATE EVERY UPDATE_PERIOD ms (50Hz) |
shimniok | 0:9b27ebe1ce17 | 840 | // The compass updates only ever 50ms |
shimniok | 0:9b27ebe1ce17 | 841 | // The gyro updates however fast we can read it |
shimniok | 0:9b27ebe1ce17 | 842 | // |
shimniok | 0:9b27ebe1ce17 | 843 | |
shimniok | 0:9b27ebe1ce17 | 844 | if ((millis % UPDATE_PERIOD) == 0) { |
shimniok | 0:9b27ebe1ce17 | 845 | unsigned int adcResult[8]; |
shimniok | 0:9b27ebe1ce17 | 846 | |
shimniok | 0:9b27ebe1ce17 | 847 | //WHERE(); |
shimniok | 0:9b27ebe1ce17 | 848 | |
shimniok | 0:9b27ebe1ce17 | 849 | adc.setChannel(0); |
shimniok | 0:9b27ebe1ce17 | 850 | for (int i=0; i < 8; i++) { |
shimniok | 0:9b27ebe1ce17 | 851 | adcResult[i] = adc.read(); |
shimniok | 0:9b27ebe1ce17 | 852 | } |
shimniok | 0:9b27ebe1ce17 | 853 | |
shimniok | 0:9b27ebe1ce17 | 854 | temp = adcResult[TEMP_CHAN]; |
shimniok | 0:9b27ebe1ce17 | 855 | // <== read/temp calibrate here |
shimniok | 0:9b27ebe1ce17 | 856 | // Convert gyro adc reading to heading |
shimniok | 0:9b27ebe1ce17 | 857 | // TODO: |
shimniok | 0:9b27ebe1ce17 | 858 | // automatically calculate null at start |
shimniok | 0:9b27ebe1ce17 | 859 | //gyro = gyroRate(adcResult[GYRO_CHAN]); |
shimniok | 0:9b27ebe1ce17 | 860 | |
shimniok | 0:9b27ebe1ce17 | 861 | yaw = ahrsParser.readYaw(); // <-- this may have been causing trouble before. 20110420 1249 |
shimniok | 0:9b27ebe1ce17 | 862 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 863 | yawRate = yaw - lastYaw; |
shimniok | 0:9b27ebe1ce17 | 864 | lastYaw = yaw; |
shimniok | 0:9b27ebe1ce17 | 865 | |
shimniok | 0:9b27ebe1ce17 | 866 | // If we're stopped, set initialHdg to compass |
shimniok | 0:9b27ebe1ce17 | 867 | // But do a running average / leaky integrator to reduce some noise |
shimniok | 0:9b27ebe1ce17 | 868 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 869 | if (encSpeed == 0) { |
shimniok | 0:9b27ebe1ce17 | 870 | findGPSBias(&gps2_off_brg, &gps2_off_dist, 1, wpt[0]); |
shimniok | 0:9b27ebe1ce17 | 871 | initialHdgFilt += compassHdg - (initialHdgFilt * 0.25); |
shimniok | 0:9b27ebe1ce17 | 872 | initialHdg = initialHdgFilt * 0.25; |
shimniok | 0:9b27ebe1ce17 | 873 | while (initialHdg < 0.0) initialHdg += 360.0; |
shimniok | 0:9b27ebe1ce17 | 874 | while (initialHdg >= 360.0) initialHdg -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 875 | } |
shimniok | 0:9b27ebe1ce17 | 876 | |
shimniok | 0:9b27ebe1ce17 | 877 | // Substitute 6dof AHRS yaw for 1-axis gyro and 6dof compass |
shimniok | 0:9b27ebe1ce17 | 878 | yawHdg = initialHdg + yaw; |
shimniok | 0:9b27ebe1ce17 | 879 | |
shimniok | 0:9b27ebe1ce17 | 880 | while (yawHdg < 0) yawHdg += 360.0; |
shimniok | 0:9b27ebe1ce17 | 881 | while (yawHdg >= 360.0) yawHdg -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 882 | |
shimniok | 0:9b27ebe1ce17 | 883 | compassHdg = compassHeading(); |
shimniok | 0:9b27ebe1ce17 | 884 | |
shimniok | 0:9b27ebe1ce17 | 885 | // pc.printf("Gyro: %d GyroHdg: %.1f\n", gyro, yaw); |
shimniok | 0:9b27ebe1ce17 | 886 | |
shimniok | 0:9b27ebe1ce17 | 887 | // Need a better filtered heading output |
shimniok | 0:9b27ebe1ce17 | 888 | // for now just use gyro heading as it seems to be most accurate |
shimniok | 0:9b27ebe1ce17 | 889 | // provided the bias is set right. |
shimniok | 0:9b27ebe1ce17 | 890 | |
shimniok | 0:9b27ebe1ce17 | 891 | // Odometry |
shimniok | 0:9b27ebe1ce17 | 892 | unsigned int leftCount = left.read(); |
shimniok | 0:9b27ebe1ce17 | 893 | unsigned int rightCount = right.read(); |
shimniok | 0:9b27ebe1ce17 | 894 | |
shimniok | 0:9b27ebe1ce17 | 895 | // TODO--> sanity check on encoders; if difference between them |
shimniok | 0:9b27ebe1ce17 | 896 | // is huge, what do we do? Slipping wheel? Skidding wheel? |
shimniok | 0:9b27ebe1ce17 | 897 | |
shimniok | 0:9b27ebe1ce17 | 898 | double leftDist = (WHEEL_CIRC / 32) * (double) leftCount; |
shimniok | 0:9b27ebe1ce17 | 899 | double rightDist = (WHEEL_CIRC / 32) * (double) rightCount; |
shimniok | 0:9b27ebe1ce17 | 900 | encDistance = (leftDist + rightDist) / 2.0; |
shimniok | 0:9b27ebe1ce17 | 901 | encSpeed = encDistance / (UPDATE_PERIOD * 0.001); |
shimniok | 0:9b27ebe1ce17 | 902 | |
shimniok | 0:9b27ebe1ce17 | 903 | odo_heading += (leftDist - rightDist) / TRACK; |
shimniok | 0:9b27ebe1ce17 | 904 | |
shimniok | 0:9b27ebe1ce17 | 905 | if ((millis % 500) == 0) { |
shimniok | 0:9b27ebe1ce17 | 906 | pc.printf("Odo heading: %.1f\n", odo_heading); |
shimniok | 0:9b27ebe1ce17 | 907 | } |
shimniok | 0:9b27ebe1ce17 | 908 | |
shimniok | 0:9b27ebe1ce17 | 909 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 910 | if (encSpeed < GPS_MIN_SPEED) { |
shimniok | 0:9b27ebe1ce17 | 911 | dr_heading = yawHdg; |
shimniok | 0:9b27ebe1ce17 | 912 | } else { |
shimniok | 0:9b27ebe1ce17 | 913 | started = true; |
shimniok | 0:9b27ebe1ce17 | 914 | dr_heading = compassHdg; |
shimniok | 0:9b27ebe1ce17 | 915 | |
shimniok | 0:9b27ebe1ce17 | 916 | // Yaw error |
shimniok | 0:9b27ebe1ce17 | 917 | // Let yaw track closely behind compass which |
shimniok | 0:9b27ebe1ce17 | 918 | // tracks closely behind GPS |
shimniok | 0:9b27ebe1ce17 | 919 | err_yaw = clamp180(compassHdg - yawHdg); |
shimniok | 0:9b27ebe1ce17 | 920 | |
shimniok | 0:9b27ebe1ce17 | 921 | // fix yaw by tweaking the initial heading |
shimniok | 0:9b27ebe1ce17 | 922 | // which is a kludge but... hey it's 2 days before the competition |
shimniok | 0:9b27ebe1ce17 | 923 | initialHdg += err_yaw * yawGain; |
shimniok | 0:9b27ebe1ce17 | 924 | clamp360(initialHdg); |
shimniok | 0:9b27ebe1ce17 | 925 | |
shimniok | 0:9b27ebe1ce17 | 926 | } |
shimniok | 0:9b27ebe1ce17 | 927 | while (dr_heading < 0) dr_heading += 360.0; |
shimniok | 0:9b27ebe1ce17 | 928 | while (dr_heading >= 360.0) dr_heading -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 929 | |
shimniok | 0:9b27ebe1ce17 | 930 | //pc.printf("%d dr_heading: %.2f\n", __LINE__, dr_heading); |
shimniok | 0:9b27ebe1ce17 | 931 | |
shimniok | 0:9b27ebe1ce17 | 932 | // Dead Reckoning Position estimation, very simple stuff |
shimniok | 0:9b27ebe1ce17 | 933 | // lat: north +y, south -y ; cos(hdg) |
shimniok | 0:9b27ebe1ce17 | 934 | // long: west -x, east +x ; sin(hdg) |
shimniok | 0:9b27ebe1ce17 | 935 | // 1852m = 1 nautical mile = 1 deg lat / long |
shimniok | 0:9b27ebe1ce17 | 936 | dr_here.move(dr_heading, encDistance); |
shimniok | 0:9b27ebe1ce17 | 937 | |
shimniok | 0:9b27ebe1ce17 | 938 | //////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 939 | // Correct position here |
shimniok | 0:9b27ebe1ce17 | 940 | |
shimniok | 0:9b27ebe1ce17 | 941 | // |
shimniok | 0:9b27ebe1ce17 | 942 | // |
shimniok | 0:9b27ebe1ce17 | 943 | //////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 944 | |
shimniok | 0:9b27ebe1ce17 | 945 | dr_bearing = wpt[wptCurrent].bearing(dr_here); |
shimniok | 0:9b27ebe1ce17 | 946 | dr_distance = wpt[wptCurrent].distance(dr_here); |
shimniok | 0:9b27ebe1ce17 | 947 | |
shimniok | 0:9b27ebe1ce17 | 948 | //pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance); |
shimniok | 0:9b27ebe1ce17 | 949 | |
shimniok | 0:9b27ebe1ce17 | 950 | // Log dead reckoning info |
shimniok | 0:9b27ebe1ce17 | 951 | logStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 952 | fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, , , , , , %.1f, %.1f, , %.1f, %.3f, %d, %d, %d, %.1f\n", |
shimniok | 0:9b27ebe1ce17 | 953 | millis, yaw, yawHdg, compassHdg, dr_here.latitude(), dr_here.longitude(), dr_heading, encSpeed, |
shimniok | 0:9b27ebe1ce17 | 954 | dr_bearing, dr_distance, left.readTotal(), right.readTotal(), temp, odo_heading); |
shimniok | 0:9b27ebe1ce17 | 955 | |
shimniok | 0:9b27ebe1ce17 | 956 | //pc.printf("Yaw = %.1f\n", ahrsParser.readYaw()); |
shimniok | 0:9b27ebe1ce17 | 957 | |
shimniok | 0:9b27ebe1ce17 | 958 | // synchronize when RMC and GGA sentences received w/ AHRS |
shimniok | 0:9b27ebe1ce17 | 959 | if (gps2Parse.rmc_ready() && gps2Parse.gga_ready()) { |
shimniok | 0:9b27ebe1ce17 | 960 | char gps2date[32], gps2time[32]; |
shimniok | 0:9b27ebe1ce17 | 961 | |
shimniok | 0:9b27ebe1ce17 | 962 | // We synchronize on the first GPS packet that doesn't have GSV sentences |
shimniok | 0:9b27ebe1ce17 | 963 | // then every 1000ms after, we will save DR data for error comparison |
shimniok | 0:9b27ebe1ce17 | 964 | if (!gps2Parse.gsv_ready() && !gpsSync) { |
shimniok | 0:9b27ebe1ce17 | 965 | syncTime = millis + 1000; |
shimniok | 0:9b27ebe1ce17 | 966 | gpsSync = true; |
shimniok | 0:9b27ebe1ce17 | 967 | } |
shimniok | 0:9b27ebe1ce17 | 968 | |
shimniok | 0:9b27ebe1ce17 | 969 | doGPS(gps2Parse, here2, gps2Status, gps2date, gps2time); |
shimniok | 0:9b27ebe1ce17 | 970 | |
shimniok | 0:9b27ebe1ce17 | 971 | if (gps2_hdop < GPS_MAX_HDOP) { |
shimniok | 0:9b27ebe1ce17 | 972 | |
shimniok | 0:9b27ebe1ce17 | 973 | // Correct here2 for offset (bias) |
shimniok | 0:9b27ebe1ce17 | 974 | here2.move(gps2_off_brg, gps2_off_dist); |
shimniok | 0:9b27ebe1ce17 | 975 | |
shimniok | 0:9b27ebe1ce17 | 976 | //pc.printf("GPS2 %d HDOP: %.1f Compass: %.1f Gyro: %d Gyro Temp: %d\n", millis, gps1Parse.f_hdop(), compassHdg, gyro, temp); |
shimniok | 0:9b27ebe1ce17 | 977 | |
shimniok | 0:9b27ebe1ce17 | 978 | gps2_bearing = wpt[wptCurrent].bearing(here2); |
shimniok | 0:9b27ebe1ce17 | 979 | gps2_distance = wpt[wptCurrent].distance(here2); |
shimniok | 0:9b27ebe1ce17 | 980 | gps2_heading = gps2Parse.f_course(); |
shimniok | 0:9b27ebe1ce17 | 981 | |
shimniok | 0:9b27ebe1ce17 | 982 | //pc.printf("GPS2: lat: %.6f lon: %.6f wpt[%d]: lat:%.6f lon:%.6f distance=%.2f\n", |
shimniok | 0:9b27ebe1ce17 | 983 | // here2.latitude(), here2.longitude(), wptCurrent, |
shimniok | 0:9b27ebe1ce17 | 984 | // wpt[wptCurrent].latitude(), wpt[wptCurrent].longitude(), distance); |
shimniok | 0:9b27ebe1ce17 | 985 | |
shimniok | 0:9b27ebe1ce17 | 986 | // TODO --> Estimate new DR position here |
shimniok | 0:9b27ebe1ce17 | 987 | // simple: take a weighted average midpoint |
shimniok | 0:9b27ebe1ce17 | 988 | // TODO --> gradually move dr position by using same err distance/bearing but move a fraction of the distance |
shimniok | 0:9b27ebe1ce17 | 989 | // over the next n samples between gps readings |
shimniok | 0:9b27ebe1ce17 | 990 | // We have to address GPS lag. The easy (but probably incorrect way) is to use the DR info at the last time |
shimniok | 0:9b27ebe1ce17 | 991 | // we received a GPS update. Because the GPS is spitting out CSV sentences every 5 seconds or so, that |
shimniok | 0:9b27ebe1ce17 | 992 | // throws off the timing. |
shimniok | 0:9b27ebe1ce17 | 993 | |
shimniok | 0:9b27ebe1ce17 | 994 | if (!firstGPS) { |
shimniok | 0:9b27ebe1ce17 | 995 | |
shimniok | 0:9b27ebe1ce17 | 996 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 997 | // Position error |
shimniok | 0:9b27ebe1ce17 | 998 | err_distance = here2.distance(dr_here_last); |
shimniok | 0:9b27ebe1ce17 | 999 | err_bearing = here2.bearing(dr_here_last); |
shimniok | 0:9b27ebe1ce17 | 1000 | |
shimniok | 0:9b27ebe1ce17 | 1001 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1002 | // Error Correction |
shimniok | 0:9b27ebe1ce17 | 1003 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1004 | // The strategy is: rely on GPS when hdop < 2; corrected by distance from initial starting point. At |
shimniok | 0:9b27ebe1ce17 | 1005 | // hdop > 2, gps position becomes unreliable, so fall back on compass and/or AHRS yaw and dead reckoning. |
shimniok | 0:9b27ebe1ce17 | 1006 | // The latter is pretty reliable. But heading isn't. Compass err varies based on, apparently, mag field |
shimniok | 0:9b27ebe1ce17 | 1007 | // from the motor/wires. We'll stick to a set speed and correct compass error while relying on gps. That |
shimniok | 0:9b27ebe1ce17 | 1008 | // plus position correction should keep DR up to date with the last gps fix with hdop < GPS_MAX_HDOP |
shimniok | 0:9b27ebe1ce17 | 1009 | // Meanwhile, AHRS yaw seems to be filtered to the point that bandwidth can't track even moderate turn |
shimniok | 0:9b27ebe1ce17 | 1010 | // rates. The exact cause is yet undiscovered. The onboard compass has a distortion I've been unable |
shimniok | 0:9b27ebe1ce17 | 1011 | // to resolve, so we're running only gyro+accelerometer |
shimniok | 0:9b27ebe1ce17 | 1012 | // |
shimniok | 0:9b27ebe1ce17 | 1013 | // At higher speeds, we can trust gps course, but at lower speeds we can't. Initially when we start, the |
shimniok | 0:9b27ebe1ce17 | 1014 | // most reliable heading/DR information comes from encoders+AHRS. At speed = 0, yaw and compass are very |
shimniok | 0:9b27ebe1ce17 | 1015 | // reliable, so yaw initial heading is set from compass. So we'll use dead reckoning until the vehicle |
shimniok | 0:9b27ebe1ce17 | 1016 | // reaches a higher speed, at which point, position info comes from gps and is used for yaw/compass/position |
shimniok | 0:9b27ebe1ce17 | 1017 | // correction. |
shimniok | 0:9b27ebe1ce17 | 1018 | // |
shimniok | 0:9b27ebe1ce17 | 1019 | // I wished I could figure out a mathematically fancy way to do this but I'm confounded by all the different |
shimniok | 0:9b27ebe1ce17 | 1020 | // sensor errors. KF is the only thing I've sort-of learned so far and I don't yet see how I can use it |
shimniok | 0:9b27ebe1ce17 | 1021 | // for the types of (apparently non-gaussian) noise/error I'm seeing. |
shimniok | 0:9b27ebe1ce17 | 1022 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1023 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 1024 | if (encSpeed >= GPS_MIN_SPEED) { |
shimniok | 0:9b27ebe1ce17 | 1025 | // Heading error |
shimniok | 0:9b27ebe1ce17 | 1026 | err_compass = compassHdg_last - gps2_heading; |
shimniok | 0:9b27ebe1ce17 | 1027 | if (err_compass < -180.0) err_compass += 360.0; |
shimniok | 0:9b27ebe1ce17 | 1028 | if (err_compass > 180.0) err_compass -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 1029 | // we'll use err_compass when computing compass heading from now on |
shimniok | 0:9b27ebe1ce17 | 1030 | |
shimniok | 0:9b27ebe1ce17 | 1031 | // fix compass by tweaking declination |
shimniok | 0:9b27ebe1ce17 | 1032 | // again, no time to do anything elegant, just hacking here |
shimniok | 0:9b27ebe1ce17 | 1033 | declination += err_compass * compassGain; |
shimniok | 0:9b27ebe1ce17 | 1034 | clamp360(declination); |
shimniok | 0:9b27ebe1ce17 | 1035 | |
shimniok | 0:9b27ebe1ce17 | 1036 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 1037 | fprintf(fp, "ERR: err_dist: %.5f err_brg: %.2f compassHdg_last: %.2f gps2_heading: %.2f err_compass: %.5f declination: %.2f\n", |
shimniok | 0:9b27ebe1ce17 | 1038 | err_distance, err_bearing, compassHdg_last, gps2_heading, err_compass, declination); |
shimniok | 0:9b27ebe1ce17 | 1039 | } |
shimniok | 0:9b27ebe1ce17 | 1040 | } // if (!firstGPS) |
shimniok | 0:9b27ebe1ce17 | 1041 | |
shimniok | 0:9b27ebe1ce17 | 1042 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 1043 | // Save currnt DR position / heading |
shimniok | 0:9b27ebe1ce17 | 1044 | // change this so that after gpsSync == true, we only save this |
shimniok | 0:9b27ebe1ce17 | 1045 | // when millis == syncTime, then syncTime += 1000 |
shimniok | 0:9b27ebe1ce17 | 1046 | dr_here_last.set( dr_here ); |
shimniok | 0:9b27ebe1ce17 | 1047 | compassHdg_last = compassHdg; |
shimniok | 0:9b27ebe1ce17 | 1048 | firstGPS = false; |
shimniok | 0:9b27ebe1ce17 | 1049 | |
shimniok | 0:9b27ebe1ce17 | 1050 | // less simple: find distance between & bearing, mult dist by weight, plot new position with move() |
shimniok | 0:9b27ebe1ce17 | 1051 | // harder: kalman filter |
shimniok | 0:9b27ebe1ce17 | 1052 | // ALSO---> need to do some kind of sanity check. If distance between each exceeds threshold, then one is bad |
shimniok | 0:9b27ebe1ce17 | 1053 | // how do we guess which? Will gps ever be way off with low hdop? |
shimniok | 0:9b27ebe1ce17 | 1054 | // maybe compare to predicted location based on last heading/speed? |
shimniok | 0:9b27ebe1ce17 | 1055 | // should we convert to UTM? |
shimniok | 0:9b27ebe1ce17 | 1056 | } |
shimniok | 0:9b27ebe1ce17 | 1057 | |
shimniok | 0:9b27ebe1ce17 | 1058 | logStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 1059 | //WHERE(); |
shimniok | 0:9b27ebe1ce17 | 1060 | fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, GPS2, %s, %s, %.2f, %.1f, %.1f, %.1f, %.1f, %.3f, , , , %d, \n", |
shimniok | 0:9b27ebe1ce17 | 1061 | millis, gyro, yawHdg, compassHdg, here2.latitude(), here2.longitude(), gps2date, gps2time, |
shimniok | 0:9b27ebe1ce17 | 1062 | gps2Parse.f_altitude(), // <-- was hanging here... but probably due to goof up somewhere else. Stack corruption?? |
shimniok | 0:9b27ebe1ce17 | 1063 | gps2_heading, gps2Parse.f_speed_mph(), gps2Parse.f_hdop(), bearing, distance, temp); |
shimniok | 0:9b27ebe1ce17 | 1064 | //WHERE(); |
shimniok | 0:9b27ebe1ce17 | 1065 | |
shimniok | 0:9b27ebe1ce17 | 1066 | } |
shimniok | 0:9b27ebe1ce17 | 1067 | |
shimniok | 0:9b27ebe1ce17 | 1068 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1069 | // Update steering and throttle |
shimniok | 0:9b27ebe1ce17 | 1070 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1071 | if ((millis % 100) == 0) { |
shimniok | 0:9b27ebe1ce17 | 1072 | |
shimniok | 0:9b27ebe1ce17 | 1073 | // TODO --> Calculate bearing/distance from GPS and DR |
shimniok | 0:9b27ebe1ce17 | 1074 | |
shimniok | 0:9b27ebe1ce17 | 1075 | // Might be able to do kalman filter here but... meanwhile: |
shimniok | 0:9b27ebe1ce17 | 1076 | // 1) Weighted averaging of heading using GPS and Gyro to account for drift |
shimniok | 0:9b27ebe1ce17 | 1077 | // 2) Correct GPS position for bias (brg/dist) |
shimniok | 0:9b27ebe1ce17 | 1078 | // 3) Take corrected GPS position, go back 2 seconds, and find error brg/dist with DR at that time |
shimniok | 0:9b27ebe1ce17 | 1079 | // 4) calculate a weighted average position based on DR and GPS, hdop and SV change |
shimniok | 0:9b27ebe1ce17 | 1080 | // 5) apportion some of the error back into the GPS position bias (brg/dist) weighted on hdop and SV changes |
shimniok | 0:9b27ebe1ce17 | 1081 | |
shimniok | 0:9b27ebe1ce17 | 1082 | |
shimniok | 0:9b27ebe1ce17 | 1083 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1084 | // Navigation -- see error correction above |
shimniok | 0:9b27ebe1ce17 | 1085 | // We'll always navigate off DR but only because we're supposed to be |
shimniok | 0:9b27ebe1ce17 | 1086 | // correcting it as we go via GPS |
shimniok | 0:9b27ebe1ce17 | 1087 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1088 | // 20110421 |
shimniok | 0:9b27ebe1ce17 | 1089 | bearing = dr_bearing; |
shimniok | 0:9b27ebe1ce17 | 1090 | distance = dr_distance; |
shimniok | 0:9b27ebe1ce17 | 1091 | heading = dr_heading; |
shimniok | 0:9b27ebe1ce17 | 1092 | |
shimniok | 0:9b27ebe1ce17 | 1093 | relativeBrg = bearing - heading; // 0 is desired heading |
shimniok | 0:9b27ebe1ce17 | 1094 | |
shimniok | 0:9b27ebe1ce17 | 1095 | // limit steering angle based on object detection ? |
shimniok | 0:9b27ebe1ce17 | 1096 | // or limit relative brg perhaps? |
shimniok | 0:9b27ebe1ce17 | 1097 | |
shimniok | 0:9b27ebe1ce17 | 1098 | // if correction angle is < -180, express as negative degree |
shimniok | 0:9b27ebe1ce17 | 1099 | if (relativeBrg < -180.0) relativeBrg += 360.0; |
shimniok | 0:9b27ebe1ce17 | 1100 | if (relativeBrg > 180.0) relativeBrg -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 1101 | |
shimniok | 0:9b27ebe1ce17 | 1102 | float steerAngle = steerCalc.calcSA(relativeBrg); |
shimniok | 0:9b27ebe1ce17 | 1103 | |
shimniok | 0:9b27ebe1ce17 | 1104 | // Convert steerAngle to servo value |
shimniok | 0:9b27ebe1ce17 | 1105 | // Testing determined near linear conversion between servo ms setting and steering angle |
shimniok | 0:9b27ebe1ce17 | 1106 | // up to 20*. Assumes a particular servo library with range = 0.005 |
shimniok | 0:9b27ebe1ce17 | 1107 | // In that case, f(SA) = servoPosition = 0.500 + SA/762.5 |
shimniok | 0:9b27ebe1ce17 | 1108 | // between 20 and 24* the slop is approximately 475 |
shimniok | 0:9b27ebe1ce17 | 1109 | // What if we ignore the linearity and just set to a max angle |
shimniok | 0:9b27ebe1ce17 | 1110 | // also range is 0.535-0.460 --> slope = 800 |
shimniok | 0:9b27ebe1ce17 | 1111 | //steering = 0.500 + (double) steerAngle / 762.5; |
shimniok | 0:9b27ebe1ce17 | 1112 | |
shimniok | 0:9b27ebe1ce17 | 1113 | if (encSpeed < GPS_MIN_SPEED) { |
shimniok | 0:9b27ebe1ce17 | 1114 | steering = 0.495; |
shimniok | 0:9b27ebe1ce17 | 1115 | } else { |
shimniok | 0:9b27ebe1ce17 | 1116 | steering = 0.500 + (double) steerAngle / 808.0; |
shimniok | 0:9b27ebe1ce17 | 1117 | } |
shimniok | 0:9b27ebe1ce17 | 1118 | pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance); |
shimniok | 0:9b27ebe1ce17 | 1119 | |
shimniok | 0:9b27ebe1ce17 | 1120 | // if within 3m move to next waypoint |
shimniok | 0:9b27ebe1ce17 | 1121 | // PARAMETERIZE DISTANCE |
shimniok | 0:9b27ebe1ce17 | 1122 | if (distance < 3.0) { |
shimniok | 0:9b27ebe1ce17 | 1123 | pc.printf("Arrived at wpt %d\n", wptCurrent); |
shimniok | 0:9b27ebe1ce17 | 1124 | wptCurrent++; |
shimniok | 0:9b27ebe1ce17 | 1125 | } |
shimniok | 0:9b27ebe1ce17 | 1126 | |
shimniok | 0:9b27ebe1ce17 | 1127 | // Are we at the last waypoint? |
shimniok | 0:9b27ebe1ce17 | 1128 | if (wptCurrent == wptCount) { |
shimniok | 0:9b27ebe1ce17 | 1129 | pc.printf("Arrived at final destination. Done.\n"); |
shimniok | 0:9b27ebe1ce17 | 1130 | done = true; |
shimniok | 0:9b27ebe1ce17 | 1131 | } |
shimniok | 0:9b27ebe1ce17 | 1132 | |
shimniok | 0:9b27ebe1ce17 | 1133 | } |
shimniok | 0:9b27ebe1ce17 | 1134 | |
shimniok | 0:9b27ebe1ce17 | 1135 | |
shimniok | 0:9b27ebe1ce17 | 1136 | /* |
shimniok | 0:9b27ebe1ce17 | 1137 | if ((millis % 1000) == 0) { |
shimniok | 0:9b27ebe1ce17 | 1138 | pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance); |
shimniok | 0:9b27ebe1ce17 | 1139 | pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance); |
shimniok | 0:9b27ebe1ce17 | 1140 | pc.printf("err_dist: %.5f\nerr_brg: %.1f\n\n", err_distance, err_bearing); |
shimniok | 0:9b27ebe1ce17 | 1141 | } |
shimniok | 0:9b27ebe1ce17 | 1142 | */ |
shimniok | 0:9b27ebe1ce17 | 1143 | |
shimniok | 0:9b27ebe1ce17 | 1144 | } |
shimniok | 0:9b27ebe1ce17 | 1145 | |
shimniok | 0:9b27ebe1ce17 | 1146 | } // while |
shimniok | 0:9b27ebe1ce17 | 1147 | |
shimniok | 0:9b27ebe1ce17 | 1148 | // shut 'er down! |
shimniok | 0:9b27ebe1ce17 | 1149 | throttle = 0.500; |
shimniok | 0:9b27ebe1ce17 | 1150 | steering = 0.500; |
shimniok | 0:9b27ebe1ce17 | 1151 | |
shimniok | 0:9b27ebe1ce17 | 1152 | if (fp) { |
shimniok | 0:9b27ebe1ce17 | 1153 | fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 1154 | logStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 1155 | pc.printf("closed log file.\n"); |
shimniok | 0:9b27ebe1ce17 | 1156 | } |
shimniok | 0:9b27ebe1ce17 | 1157 | |
shimniok | 0:9b27ebe1ce17 | 1158 | //ahrsStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 1159 | gps2Status = 0; |
shimniok | 0:9b27ebe1ce17 | 1160 | confStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 1161 | flasher = 0; |
shimniok | 0:9b27ebe1ce17 | 1162 | |
shimniok | 0:9b27ebe1ce17 | 1163 | } // autonomousMode |
shimniok | 0:9b27ebe1ce17 | 1164 | |
shimniok | 0:9b27ebe1ce17 | 1165 | |
shimniok | 0:9b27ebe1ce17 | 1166 | void idleMode() |
shimniok | 0:9b27ebe1ce17 | 1167 | { |
shimniok | 0:9b27ebe1ce17 | 1168 | while (1) { |
shimniok | 0:9b27ebe1ce17 | 1169 | ahrsStatus = 0; |
shimniok | 0:9b27ebe1ce17 | 1170 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 1171 | ahrsStatus = 1; |
shimniok | 0:9b27ebe1ce17 | 1172 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 1173 | } |
shimniok | 0:9b27ebe1ce17 | 1174 | } |
shimniok | 0:9b27ebe1ce17 | 1175 | |
shimniok | 0:9b27ebe1ce17 | 1176 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1177 | // UTILITIY FUNCTIONS |
shimniok | 0:9b27ebe1ce17 | 1178 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1179 | |
shimniok | 0:9b27ebe1ce17 | 1180 | |
shimniok | 0:9b27ebe1ce17 | 1181 | // Takes n position readings and averages the gps position |
shimniok | 0:9b27ebe1ce17 | 1182 | // in a pretty rudimentary fashion, returning bearing and distance |
shimniok | 0:9b27ebe1ce17 | 1183 | // from the specified place to the average position |
shimniok | 0:9b27ebe1ce17 | 1184 | void findGPSBias(double *brg, double *dist, int n, GeoPosition place) |
shimniok | 0:9b27ebe1ce17 | 1185 | { |
shimniok | 0:9b27ebe1ce17 | 1186 | // GPS Position averaging |
shimniok | 0:9b27ebe1ce17 | 1187 | double div = (double) n; |
shimniok | 0:9b27ebe1ce17 | 1188 | double latavg = 0; |
shimniok | 0:9b27ebe1ce17 | 1189 | double lonavg = 0; |
shimniok | 0:9b27ebe1ce17 | 1190 | double lat = 0; |
shimniok | 0:9b27ebe1ce17 | 1191 | double lon = 0; |
shimniok | 0:9b27ebe1ce17 | 1192 | gps2Parse.reset_ready(); |
shimniok | 0:9b27ebe1ce17 | 1193 | |
shimniok | 0:9b27ebe1ce17 | 1194 | pc.printf("Calculating GPS offset...\n"); |
shimniok | 0:9b27ebe1ce17 | 1195 | |
shimniok | 0:9b27ebe1ce17 | 1196 | while (n > 0) { |
shimniok | 0:9b27ebe1ce17 | 1197 | while (gps2Parse.rmc_ready() == false) |
shimniok | 0:9b27ebe1ce17 | 1198 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 1199 | |
shimniok | 0:9b27ebe1ce17 | 1200 | gps2Parse.reset_ready(); |
shimniok | 0:9b27ebe1ce17 | 1201 | gps2Parse.f_get_position(&lat, &lon, &age); |
shimniok | 0:9b27ebe1ce17 | 1202 | |
shimniok | 0:9b27ebe1ce17 | 1203 | if (gps2Parse.f_hdop() > 0 && gps2Parse.f_hdop() < 2.0) { |
shimniok | 0:9b27ebe1ce17 | 1204 | pc.printf("GPS lat: %.7f lon %.7f\n", lat, lon); |
shimniok | 0:9b27ebe1ce17 | 1205 | latavg += lat / div; |
shimniok | 0:9b27ebe1ce17 | 1206 | lonavg += lon / div; |
shimniok | 0:9b27ebe1ce17 | 1207 | n--; |
shimniok | 0:9b27ebe1ce17 | 1208 | } else { |
shimniok | 0:9b27ebe1ce17 | 1209 | pc.printf("Waiting for good GPS fix\n"); |
shimniok | 0:9b27ebe1ce17 | 1210 | gps2Status = 1; |
shimniok | 0:9b27ebe1ce17 | 1211 | wait(0.1); |
shimniok | 0:9b27ebe1ce17 | 1212 | gps2Status = 0; |
shimniok | 0:9b27ebe1ce17 | 1213 | } |
shimniok | 0:9b27ebe1ce17 | 1214 | |
shimniok | 0:9b27ebe1ce17 | 1215 | } |
shimniok | 0:9b27ebe1ce17 | 1216 | pc.printf("AVG lat: %.7f lon %.7f\n", latavg, lonavg); |
shimniok | 0:9b27ebe1ce17 | 1217 | GeoPosition avg(latavg, lonavg); |
shimniok | 0:9b27ebe1ce17 | 1218 | *dist = place.distance( avg ); |
shimniok | 0:9b27ebe1ce17 | 1219 | *brg = place.bearing( avg ); |
shimniok | 0:9b27ebe1ce17 | 1220 | // Now that we've calculated a bearing and distance to a quickie offset/bias |
shimniok | 0:9b27ebe1ce17 | 1221 | // we can subtract this from all future GPS readings by simply taking the |
shimniok | 0:9b27ebe1ce17 | 1222 | // gps position and doing a move(gps2_off_brg, gps2_off_dist) |
shimniok | 0:9b27ebe1ce17 | 1223 | // maybe do adjustments based on how far off predicted DR position is from |
shimniok | 0:9b27ebe1ce17 | 1224 | // GPS reported pos. I'd love to be able to factor in SV and HDOP info too |
shimniok | 0:9b27ebe1ce17 | 1225 | } |
shimniok | 0:9b27ebe1ce17 | 1226 | |
shimniok | 0:9b27ebe1ce17 | 1227 | |
shimniok | 0:9b27ebe1ce17 | 1228 | void compass3dCalibrate() |
shimniok | 0:9b27ebe1ce17 | 1229 | { |
shimniok | 0:9b27ebe1ce17 | 1230 | int tick; |
shimniok | 0:9b27ebe1ce17 | 1231 | FILE *fp; |
shimniok | 0:9b27ebe1ce17 | 1232 | |
shimniok | 0:9b27ebe1ce17 | 1233 | pc.printf("Entering calibration mode for 30 seconds\nRotate compass thru 360 degree sphere\n"); |
shimniok | 0:9b27ebe1ce17 | 1234 | for (int i=10; i > 0; i--) { |
shimniok | 0:9b27ebe1ce17 | 1235 | pc.printf("%d...", i); |
shimniok | 0:9b27ebe1ce17 | 1236 | if (pc.readable()) { |
shimniok | 0:9b27ebe1ce17 | 1237 | char cmd = pc.getc(); |
shimniok | 0:9b27ebe1ce17 | 1238 | } |
shimniok | 0:9b27ebe1ce17 | 1239 | wait(1.0); |
shimniok | 0:9b27ebe1ce17 | 1240 | } |
shimniok | 0:9b27ebe1ce17 | 1241 | pc.printf("Begin calibration\n"); |
shimniok | 0:9b27ebe1ce17 | 1242 | |
shimniok | 0:9b27ebe1ce17 | 1243 | fp = fopen("/log/compass.txt", "w"); |
shimniok | 0:9b27ebe1ce17 | 1244 | |
shimniok | 0:9b27ebe1ce17 | 1245 | if (fp == 0) { |
shimniok | 0:9b27ebe1ce17 | 1246 | pc.printf("Error opening file\n"); |
shimniok | 0:9b27ebe1ce17 | 1247 | } else { |
shimniok | 0:9b27ebe1ce17 | 1248 | // printf(fp, "# magX, magY, magZ\n"); |
shimniok | 0:9b27ebe1ce17 | 1249 | timer.reset(); |
shimniok | 0:9b27ebe1ce17 | 1250 | timer.start(); |
shimniok | 0:9b27ebe1ce17 | 1251 | tick = 30; |
shimniok | 0:9b27ebe1ce17 | 1252 | while (tick > 0) { |
shimniok | 0:9b27ebe1ce17 | 1253 | // run every 20ms |
shimniok | 0:9b27ebe1ce17 | 1254 | // get all three axes |
shimniok | 0:9b27ebe1ce17 | 1255 | // printf(fp, "%d, %d, %d\n" |
shimniok | 0:9b27ebe1ce17 | 1256 | if (timer.read_ms() > 1000) { |
shimniok | 0:9b27ebe1ce17 | 1257 | pc.printf("%d\n", tick--); |
shimniok | 0:9b27ebe1ce17 | 1258 | timer.reset(); |
shimniok | 0:9b27ebe1ce17 | 1259 | } |
shimniok | 0:9b27ebe1ce17 | 1260 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 1261 | } |
shimniok | 0:9b27ebe1ce17 | 1262 | fclose(fp); |
shimniok | 0:9b27ebe1ce17 | 1263 | pc.printf("\nData saved to compass.txt\n\n"); |
shimniok | 0:9b27ebe1ce17 | 1264 | } |
shimniok | 0:9b27ebe1ce17 | 1265 | } |
shimniok | 0:9b27ebe1ce17 | 1266 | |
shimniok | 0:9b27ebe1ce17 | 1267 | |
shimniok | 0:9b27ebe1ce17 | 1268 | // HMC6352 compass calibration mode |
shimniok | 0:9b27ebe1ce17 | 1269 | void compassCalibrate() |
shimniok | 0:9b27ebe1ce17 | 1270 | { |
shimniok | 0:9b27ebe1ce17 | 1271 | int tick; |
shimniok | 0:9b27ebe1ce17 | 1272 | |
shimniok | 0:9b27ebe1ce17 | 1273 | pc.printf("Entering calibration mode for 60 seconds\nRotate compass at least once through 360 degrees\n"); |
shimniok | 0:9b27ebe1ce17 | 1274 | for (int i=10; i > 0; i--) { |
shimniok | 0:9b27ebe1ce17 | 1275 | pc.printf("%d...", i); |
shimniok | 0:9b27ebe1ce17 | 1276 | if (pc.readable()) { |
shimniok | 0:9b27ebe1ce17 | 1277 | char cmd = pc.getc(); |
shimniok | 0:9b27ebe1ce17 | 1278 | } |
shimniok | 0:9b27ebe1ce17 | 1279 | wait(1.0); |
shimniok | 0:9b27ebe1ce17 | 1280 | } |
shimniok | 0:9b27ebe1ce17 | 1281 | pc.printf("Begin calibration\n"); |
shimniok | 0:9b27ebe1ce17 | 1282 | |
shimniok | 0:9b27ebe1ce17 | 1283 | //compass.setCalibrationMode(HMC6352_ENTER_CALIB); |
shimniok | 0:9b27ebe1ce17 | 1284 | timer.start(); |
shimniok | 0:9b27ebe1ce17 | 1285 | tick = 60; |
shimniok | 0:9b27ebe1ce17 | 1286 | while (tick > 0) { |
shimniok | 0:9b27ebe1ce17 | 1287 | if (timer.read_ms() > 1000) { |
shimniok | 0:9b27ebe1ce17 | 1288 | pc.printf("%d\n", tick--); |
shimniok | 0:9b27ebe1ce17 | 1289 | timer.reset(); |
shimniok | 0:9b27ebe1ce17 | 1290 | } |
shimniok | 0:9b27ebe1ce17 | 1291 | wait(0.2); |
shimniok | 0:9b27ebe1ce17 | 1292 | } |
shimniok | 0:9b27ebe1ce17 | 1293 | //compass.setCalibrationMode(HMC6352_EXIT_CALIB); |
shimniok | 0:9b27ebe1ce17 | 1294 | pc.printf("Calibration complete.\n"); |
shimniok | 0:9b27ebe1ce17 | 1295 | } |
shimniok | 0:9b27ebe1ce17 | 1296 | |
shimniok | 0:9b27ebe1ce17 | 1297 | void servoCalibrate() |
shimniok | 0:9b27ebe1ce17 | 1298 | { |
shimniok | 0:9b27ebe1ce17 | 1299 | } |
shimniok | 0:9b27ebe1ce17 | 1300 | |
shimniok | 0:9b27ebe1ce17 | 1301 | void bridgeRecv() |
shimniok | 0:9b27ebe1ce17 | 1302 | { |
shimniok | 0:9b27ebe1ce17 | 1303 | while (dev && dev->readable()) { |
shimniok | 0:9b27ebe1ce17 | 1304 | pc.putc(dev->getc()); |
shimniok | 0:9b27ebe1ce17 | 1305 | } |
shimniok | 0:9b27ebe1ce17 | 1306 | } |
shimniok | 0:9b27ebe1ce17 | 1307 | |
shimniok | 0:9b27ebe1ce17 | 1308 | void serialBridge(Serial &gps) |
shimniok | 0:9b27ebe1ce17 | 1309 | { |
shimniok | 0:9b27ebe1ce17 | 1310 | char x; |
shimniok | 0:9b27ebe1ce17 | 1311 | int count = 0; |
shimniok | 0:9b27ebe1ce17 | 1312 | bool done=false; |
shimniok | 0:9b27ebe1ce17 | 1313 | |
shimniok | 0:9b27ebe1ce17 | 1314 | pc.printf("\nEntering serial bridge in 2 seconds, +++ to escape\n\n"); |
shimniok | 0:9b27ebe1ce17 | 1315 | wait(2.0); |
shimniok | 0:9b27ebe1ce17 | 1316 | //dev = &gps; |
shimniok | 0:9b27ebe1ce17 | 1317 | gps.attach(NULL, Serial::RxIrq); |
shimniok | 0:9b27ebe1ce17 | 1318 | while (!done) { |
shimniok | 0:9b27ebe1ce17 | 1319 | if (pc.readable()) { |
shimniok | 0:9b27ebe1ce17 | 1320 | x = pc.getc(); |
shimniok | 0:9b27ebe1ce17 | 1321 | // escape sequence |
shimniok | 0:9b27ebe1ce17 | 1322 | if (x == '+') { |
shimniok | 0:9b27ebe1ce17 | 1323 | if (++count >= 3) done=true; |
shimniok | 0:9b27ebe1ce17 | 1324 | } else { |
shimniok | 0:9b27ebe1ce17 | 1325 | count = 0; |
shimniok | 0:9b27ebe1ce17 | 1326 | } |
shimniok | 0:9b27ebe1ce17 | 1327 | gps.putc(x); |
shimniok | 0:9b27ebe1ce17 | 1328 | } |
shimniok | 0:9b27ebe1ce17 | 1329 | if (gps.readable()) { |
shimniok | 0:9b27ebe1ce17 | 1330 | pc.putc(gps.getc()); |
shimniok | 0:9b27ebe1ce17 | 1331 | } |
shimniok | 0:9b27ebe1ce17 | 1332 | } |
shimniok | 0:9b27ebe1ce17 | 1333 | } |
shimniok | 0:9b27ebe1ce17 | 1334 | |
shimniok | 0:9b27ebe1ce17 | 1335 | |
shimniok | 0:9b27ebe1ce17 | 1336 | void instrumentCheck() |
shimniok | 0:9b27ebe1ce17 | 1337 | { |
shimniok | 0:9b27ebe1ce17 | 1338 | bool done = false; |
shimniok | 0:9b27ebe1ce17 | 1339 | float compassHdg = 0; |
shimniok | 0:9b27ebe1ce17 | 1340 | float initialHdg = 0; |
shimniok | 0:9b27ebe1ce17 | 1341 | float ahrsHdg = 0; |
shimniok | 0:9b27ebe1ce17 | 1342 | unsigned int adcResult[8] = {0,0,0,0,0,0,0,0}; |
shimniok | 0:9b27ebe1ce17 | 1343 | //unsigned int sonar, gyro, temp; |
shimniok | 0:9b27ebe1ce17 | 1344 | Timer timer; |
shimniok | 0:9b27ebe1ce17 | 1345 | SimpleFilter ranger(8); |
shimniok | 0:9b27ebe1ce17 | 1346 | |
shimniok | 0:9b27ebe1ce17 | 1347 | initialHdg = compassHeading(); |
shimniok | 0:9b27ebe1ce17 | 1348 | initAHRS(); |
shimniok | 0:9b27ebe1ce17 | 1349 | |
shimniok | 0:9b27ebe1ce17 | 1350 | timer.reset(); |
shimniok | 0:9b27ebe1ce17 | 1351 | timer.start(); |
shimniok | 0:9b27ebe1ce17 | 1352 | |
shimniok | 0:9b27ebe1ce17 | 1353 | buttonReleased = buttonPressed = false; |
shimniok | 0:9b27ebe1ce17 | 1354 | |
shimniok | 0:9b27ebe1ce17 | 1355 | pc.printf("press e to exit\n"); |
shimniok | 0:9b27ebe1ce17 | 1356 | while (!done) { |
shimniok | 0:9b27ebe1ce17 | 1357 | if (buttonPressed) done=true; |
shimniok | 0:9b27ebe1ce17 | 1358 | while (pc.readable()) { |
shimniok | 0:9b27ebe1ce17 | 1359 | if (pc.getc() == 'e') { |
shimniok | 0:9b27ebe1ce17 | 1360 | done = true; |
shimniok | 0:9b27ebe1ce17 | 1361 | break; |
shimniok | 0:9b27ebe1ce17 | 1362 | } |
shimniok | 0:9b27ebe1ce17 | 1363 | } |
shimniok | 0:9b27ebe1ce17 | 1364 | |
shimniok | 0:9b27ebe1ce17 | 1365 | int millis = timer.read_ms(); |
shimniok | 0:9b27ebe1ce17 | 1366 | |
shimniok | 0:9b27ebe1ce17 | 1367 | if ((millis % 20) == 0) { |
shimniok | 0:9b27ebe1ce17 | 1368 | |
shimniok | 0:9b27ebe1ce17 | 1369 | if ((millis % 100) == 0) { |
shimniok | 0:9b27ebe1ce17 | 1370 | sonarStart = 1; |
shimniok | 0:9b27ebe1ce17 | 1371 | wait_us(25); |
shimniok | 0:9b27ebe1ce17 | 1372 | sonarStart = 0; |
shimniok | 0:9b27ebe1ce17 | 1373 | wait_us(50); |
shimniok | 0:9b27ebe1ce17 | 1374 | } |
shimniok | 0:9b27ebe1ce17 | 1375 | |
shimniok | 0:9b27ebe1ce17 | 1376 | adc.setChannel(0); |
shimniok | 0:9b27ebe1ce17 | 1377 | for (int i = 0; i < 8; i++) { |
shimniok | 0:9b27ebe1ce17 | 1378 | adcResult[i] = adc.read(); |
shimniok | 0:9b27ebe1ce17 | 1379 | } |
shimniok | 0:9b27ebe1ce17 | 1380 | ranger.filter(adcResult[0]); |
shimniok | 0:9b27ebe1ce17 | 1381 | |
shimniok | 0:9b27ebe1ce17 | 1382 | if ((millis % 1000) == 0) { |
shimniok | 0:9b27ebe1ce17 | 1383 | compassHdg = compassHeading(); |
shimniok | 0:9b27ebe1ce17 | 1384 | |
shimniok | 0:9b27ebe1ce17 | 1385 | ahrsHdg = initialHdg + ahrsParser.readYaw(); |
shimniok | 0:9b27ebe1ce17 | 1386 | while (ahrsHdg >= 360.0) ahrsHdg -= 360.0; |
shimniok | 0:9b27ebe1ce17 | 1387 | while (ahrsHdg < 0) ahrsHdg += 360.0; |
shimniok | 0:9b27ebe1ce17 | 1388 | |
shimniok | 0:9b27ebe1ce17 | 1389 | char data[4]; |
shimniok | 0:9b27ebe1ce17 | 1390 | |
shimniok | 0:9b27ebe1ce17 | 1391 | // CMUcam1 I2C code goes here |
shimniok | 0:9b27ebe1ce17 | 1392 | |
shimniok | 0:9b27ebe1ce17 | 1393 | pc.printf("-----e to exit-----\nCompass: %.1f\nAHRS: %.1f\nGyro: %.3f\nGyro Temp: %d\nSonar Left: %.2f\nSonar Right: %.2f\nIR Left: %.5f\nIR Right: %.5f\nEncLeft: %d\nEncRight: %d\n\n", |
shimniok | 0:9b27ebe1ce17 | 1394 | compassHdg, ahrsHdg, gyroRate(adcResult[GYRO_CHAN]), adcResult[TEMP_CHAN], |
shimniok | 0:9b27ebe1ce17 | 1395 | sonarDistance(adcResult[SONARLEFT_CHAN]), sonarDistance(adcResult[SONARRIGHT_CHAN]), |
shimniok | 0:9b27ebe1ce17 | 1396 | irDistance(adcResult[IRLEFT_CHAN]), irDistance(adcResult[IRRIGHT_CHAN]), left.readTotal(), right.readTotal()); |
shimniok | 0:9b27ebe1ce17 | 1397 | |
shimniok | 0:9b27ebe1ce17 | 1398 | pc.printf("IR filter: %d %.1f\n", ranger.value(), irDistance(ranger.value())); |
shimniok | 0:9b27ebe1ce17 | 1399 | |
shimniok | 0:9b27ebe1ce17 | 1400 | pc.printf("\nCam Obj Box: (%d,%d) (%d,%d)\n\n", data[0], data[1], data[2], data[3]); |
shimniok | 0:9b27ebe1ce17 | 1401 | |
shimniok | 0:9b27ebe1ce17 | 1402 | for (int i = 0; i < 8; i++) { |
shimniok | 0:9b27ebe1ce17 | 1403 | pc.printf("ADC(%d)=%d\n", i, adcResult[i]); |
shimniok | 0:9b27ebe1ce17 | 1404 | } |
shimniok | 0:9b27ebe1ce17 | 1405 | pc.printf("\n"); |
shimniok | 0:9b27ebe1ce17 | 1406 | } |
shimniok | 0:9b27ebe1ce17 | 1407 | wait_ms(2); |
shimniok | 0:9b27ebe1ce17 | 1408 | } |
shimniok | 0:9b27ebe1ce17 | 1409 | } |
shimniok | 0:9b27ebe1ce17 | 1410 | // clear input buffer |
shimniok | 0:9b27ebe1ce17 | 1411 | while (pc.readable()) pc.getc(); |
shimniok | 0:9b27ebe1ce17 | 1412 | } |
shimniok | 0:9b27ebe1ce17 | 1413 | |
shimniok | 0:9b27ebe1ce17 | 1414 | |
shimniok | 0:9b27ebe1ce17 | 1415 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1416 | // ADC CONVERSION FUNCTIONS |
shimniok | 0:9b27ebe1ce17 | 1417 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:9b27ebe1ce17 | 1418 | float compassHeading() { |
shimniok | 0:9b27ebe1ce17 | 1419 | //Timer t; |
shimniok | 0:9b27ebe1ce17 | 1420 | //int usec1, usec2; |
shimniok | 0:9b27ebe1ce17 | 1421 | //t.reset(); |
shimniok | 0:9b27ebe1ce17 | 1422 | //t.start(); |
shimniok | 0:9b27ebe1ce17 | 1423 | //float compassHdg = compass.sample() / 10.0; // read compass |
shimniok | 0:9b27ebe1ce17 | 1424 | //float compassHdg = compass3d.heading2d(); |
shimniok | 0:9b27ebe1ce17 | 1425 | ////WHERE(); |
shimniok | 0:9b27ebe1ce17 | 1426 | //__disable_irq(); // Disable Interrupts |
shimniok | 0:9b27ebe1ce17 | 1427 | //usec1 = t.read_us(); |
shimniok | 0:9b27ebe1ce17 | 1428 | float compassHdg = compass3d.heading(); |
shimniok | 0:9b27ebe1ce17 | 1429 | //usec2 = t.read_us(); |
shimniok | 0:9b27ebe1ce17 | 1430 | //__enable_irq(); // Enable Interrupts |
shimniok | 0:9b27ebe1ce17 | 1431 | |
shimniok | 0:9b27ebe1ce17 | 1432 | //pc.printf("usec1: %d, usec2: %d, usec: %d\n", usec1, usec2, usec2-usec1); |
shimniok | 0:9b27ebe1ce17 | 1433 | |
shimniok | 0:9b27ebe1ce17 | 1434 | //WHERE(); |
shimniok | 0:9b27ebe1ce17 | 1435 | compassHdg -= declination; // Correct for local declination |
shimniok | 0:9b27ebe1ce17 | 1436 | clamp360(compassHdg); |
shimniok | 0:9b27ebe1ce17 | 1437 | |
shimniok | 0:9b27ebe1ce17 | 1438 | return compassHdg; |
shimniok | 0:9b27ebe1ce17 | 1439 | } |
shimniok | 0:9b27ebe1ce17 | 1440 | |
shimniok | 0:9b27ebe1ce17 | 1441 | |
shimniok | 0:9b27ebe1ce17 | 1442 | // returns rate in */sec |
shimniok | 0:9b27ebe1ce17 | 1443 | // |
shimniok | 0:9b27ebe1ce17 | 1444 | // Gyro is ratiometric and so is ADC, so we can cancel out |
shimniok | 0:9b27ebe1ce17 | 1445 | // errors due to voltage supply by putting sensitivity in |
shimniok | 0:9b27ebe1ce17 | 1446 | // terms of adc reading and calculate rate from that. |
shimniok | 0:9b27ebe1ce17 | 1447 | // (adc - bias) / sens |
shimniok | 0:9b27ebe1ce17 | 1448 | // UPDATE_PERIOD in ms, 5.91mV/degree/sec is scale factor |
shimniok | 0:9b27ebe1ce17 | 1449 | // |
shimniok | 0:9b27ebe1ce17 | 1450 | // TODO: use constants and defines |
shimniok | 0:9b27ebe1ce17 | 1451 | // |
shimniok | 0:9b27ebe1ce17 | 1452 | float gyroRate(unsigned int adc) |
shimniok | 0:9b27ebe1ce17 | 1453 | { |
shimniok | 0:9b27ebe1ce17 | 1454 | float rate = 0.0; |
shimniok | 0:9b27ebe1ce17 | 1455 | |
shimniok | 0:9b27ebe1ce17 | 1456 | rate = ((float) (adc - gyroBias)) / gyroSens; |
shimniok | 0:9b27ebe1ce17 | 1457 | |
shimniok | 0:9b27ebe1ce17 | 1458 | return rate; |
shimniok | 0:9b27ebe1ce17 | 1459 | } |
shimniok | 0:9b27ebe1ce17 | 1460 | |
shimniok | 0:9b27ebe1ce17 | 1461 | // returns distance in m for LV-EZ1 sonar |
shimniok | 0:9b27ebe1ce17 | 1462 | // |
shimniok | 0:9b27ebe1ce17 | 1463 | float sonarDistance(unsigned int adc) |
shimniok | 0:9b27ebe1ce17 | 1464 | { |
shimniok | 0:9b27ebe1ce17 | 1465 | float distance = 9999.9; |
shimniok | 0:9b27ebe1ce17 | 1466 | |
shimniok | 0:9b27ebe1ce17 | 1467 | // EZ1 uses 9.8mV/inch @ 5V or scaling factor of Vcc / 512 |
shimniok | 0:9b27ebe1ce17 | 1468 | // so we can eliminate Vcc changes by simply converting the 0-512 inch range |
shimniok | 0:9b27ebe1ce17 | 1469 | // to the ADC's 0-4096 range |
shimniok | 0:9b27ebe1ce17 | 1470 | distance = ((float) adc) * (512 * 0.0254) / 4096; // distance converted to inch then meter |
shimniok | 0:9b27ebe1ce17 | 1471 | |
shimniok | 0:9b27ebe1ce17 | 1472 | return distance; |
shimniok | 0:9b27ebe1ce17 | 1473 | } |
shimniok | 0:9b27ebe1ce17 | 1474 | |
shimniok | 0:9b27ebe1ce17 | 1475 | // returns distance in m for Sharp GP2YOA710K0F |
shimniok | 0:9b27ebe1ce17 | 1476 | // to get m and b, I wrote down volt vs. dist by eyeballin the |
shimniok | 0:9b27ebe1ce17 | 1477 | // datasheet chart plot. Then used Excel to do linear regression |
shimniok | 0:9b27ebe1ce17 | 1478 | // |
shimniok | 0:9b27ebe1ce17 | 1479 | float irDistance(unsigned int adc) |
shimniok | 0:9b27ebe1ce17 | 1480 | { |
shimniok | 0:9b27ebe1ce17 | 1481 | float b = 1.0934; // Intercept from Excel |
shimniok | 0:9b27ebe1ce17 | 1482 | float m = 1.4088; // Slope from Excel |
shimniok | 0:9b27ebe1ce17 | 1483 | |
shimniok | 0:9b27ebe1ce17 | 1484 | return m / (((float) adc) * 4.95/4096 - b); |
shimniok | 0:9b27ebe1ce17 | 1485 | } |