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@5:e301811727f7, 2018-11-29 (annotated)

Committer:
shimniok
Date:
Thu Nov 29 15:45:20 2018 +0000
Revision:
5:e301811727f7
Parent:
3:42f3821c4e54
Child:
17:08f6ee55abbe
Cleaned up includes

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