Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

main.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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