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

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
1:cb84b477886c
Working version with priorities set and update time display

Who changed what in which revision?

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