Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

main.cpp@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
1:cb84b477886c
Child:
3:42f3821c4e54
Sort-of working version, still some errors with estimation. Added clamp() function.

Who changed what in which revision?

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