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@3:42f3821c4e54, 2013-06-07 (annotated)

Committer:
shimniok
Date:
Fri Jun 07 14:45:46 2013 +0000
Revision:
3:42f3821c4e54
Parent:
2:fbc6e3cf3ed8
Child:
5:e301811727f7
Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.

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