Diff: main.cpp

Revision:

2:fbc6e3cf3ed8

Parent:

1:cb84b477886c

Child:

3:42f3821c4e54

--- a/main.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/main.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -55,6 +55,7 @@
 #define GYRO_CHAN        5
 
 // Chassis specific parameters
+// TODO 1 put WHEEL_CIRC, WHEELBASE, and TRACK in config.txt
 #define WHEEL_CIRC 0.321537             // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375m circumference
 #define WHEELBASE  0.290
 #define TRACK      0.280
@@ -169,23 +170,31 @@
 {
     // Let's try setting priorities...
     NVIC_SetPriority(TIMER3_IRQn, 0);   // updater running off Ticker
-    NVIC_SetPriority(DMA_IRQn, 1);
-    NVIC_SetPriority(EINT0_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT1_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT2_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT3_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(UART1_IRQn, 10);   // GPS p25,p26
-    NVIC_SetPriority(I2C0_IRQn, 20);    // sensors?
-    NVIC_SetPriority(I2C1_IRQn, 20);    // sensors?
-    NVIC_SetPriority(I2C2_IRQn, 20);    // sensors?
-    NVIC_SetPriority(UART3_IRQn, 30);   // LCD p17,p18
-    NVIC_SetPriority(UART0_IRQn, 30);   // USB
-    NVIC_SetPriority(ADC_IRQn, 40);     // Voltage/current
-    NVIC_SetPriority(TIMER0_IRQn, 255); // unused(?)
-    NVIC_SetPriority(TIMER1_IRQn, 255); // unused(?)
-    NVIC_SetPriority(TIMER2_IRQn, 255); // unused(?)
-    NVIC_SetPriority(SPI_IRQn, 255);    // uSD card, logging
+    NVIC_SetPriority(DMA_IRQn, 0);
+    NVIC_SetPriority(EINT0_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT1_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT2_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT3_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(UART0_IRQn, 5);   // USB
+    NVIC_SetPriority(UART1_IRQn, 10);
+    NVIC_SetPriority(UART2_IRQn, 10);
+    NVIC_SetPriority(UART3_IRQn, 10);
+    NVIC_SetPriority(I2C0_IRQn, 10);    // sensors?
+    NVIC_SetPriority(I2C1_IRQn, 10);    // sensors?
+    NVIC_SetPriority(I2C2_IRQn, 10);    // sensors?
+    NVIC_SetPriority(ADC_IRQn, 10);     // Voltage/current
+    NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?)
+    NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?)
+    NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?)
+    NVIC_SetPriority(SPI_IRQn, 10);    // uSD card, logging
 
+    // Something here is jacking up the I2C stuff
+    // Also when initializing with ESC powered, it causes motor to run which
+    // totally jacks up everything (noise?)
+    initSteering();
+    initThrottle();
+    // initFlasher();                                   // Initialize autonomous mode flasher
+    
     // Send data back to the PC
     pc.baud(115200);
     fprintf(stdout, "Data Bus 2013\n");
@@ -211,13 +220,6 @@
     if (config.load())                          // Load various configurable parameters, e.g., waypoints, declination, etc.
         confStatus = 1;
 
-    // Something here is jacking up the I2C stuff
-    // Also when initializing with ESC powered, it causes motor to run which
-    // totally jacks up everything (noise?)
-    initSteering();
-    initThrottle();
-    // initFlasher();                                   // Initialize autonomous mode flasher
-        
     sensors.Compass_Calibrate(config.magOffset, config.magScale);
     //pc.printf("Declination: %.1f\n", config.declination);
     pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", 
@@ -313,7 +315,7 @@
             display.update(s);
             nextUpdate = thisUpdate + 2000;
             // TODO move this statistic into display class
-            fprintf(stdout, "update time: %d\n", getUpdateTime());
+            //fprintf(stdout, "update time: %d\n", getUpdateTime());
         }
         
         if (keypad.pressed) {
@@ -359,10 +361,9 @@
             fprintf(stdout, "%d) Display AHRS\n", i++);
             fprintf(stdout, "%d) Mavlink mode\n", i++);
             fprintf(stdout, "%d) Shell\n", i++);
-            fprintf(stdout, "%d) Bridge serial to 2nd GPS\n", i++);
             fprintf(stdout, "R) Reset\n");
             fprintf(stdout, "\nSelect from the above: ");
-            fflush(stdout);
+            //fflush(stdout);
             printMenu = false;
         }
 
@@ -840,7 +841,8 @@
     sensors.gps.enableVerbose();
     wait(2.0);
     //dev = &gps;
-    serial.attach(NULL, Serial::RxIrq);
+    sensors.gps.disable();
+    serial.baud(4800);
     while (!done) {
         if (pc.readable()) {
             x = pc.getc();
@@ -935,6 +937,8 @@
                     sensors.gps.latitude(), sensors.gps.longitude(), sensors.gps.heading_deg(), 
                     sensors.gps.speed_mps(), sensors.gps.hdop(), sensors.gps.sat_count(),
                     (unsigned char) sensors.gps.getAvailable() );
+                fprintf(stdout, "brg=%6.2f d=%8.4f sa=%6.2f\n", 
+                    state[inState].bearing, state[inState].distance, state[inState].steerAngle);
                 /*
                 fprintf(stdout, "gps2=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", 
                     gps2.latitude(), gps2.longitude(), gps2.heading_deg(), gps2.speed_mps(), gps2.hdop(), gps2.sat_count(),