A data acquisition and telemetry program for model rockets running on the MAX32630FTHR platform.
Dependencies: BMI160 CommandDispatcher RadioHead SDFileSystem StringFifo USBDevice USBMSD_SD gps max31865 max32630fthr swspi
main.cpp@0:83f4079b2bac, 2017-06-11 (annotated)
- Committer:
- danjulio
- Date:
- Sun Jun 11 04:26:22 2017 +0000
- Revision:
- 0:83f4079b2bac
Initial commit of Rocket, a model rocket data acquisition and telemetry program for the MAX32630FTHR platform.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
danjulio | 0:83f4079b2bac | 1 | /* |
danjulio | 0:83f4079b2bac | 2 | * Model Rocket Data Acquisition and Telemetry system using the MAX32630FTHR. |
danjulio | 0:83f4079b2bac | 3 | * - Acceleration using on-board BMI160. |
danjulio | 0:83f4079b2bac | 4 | * - Rotation using ALS-PT19-315C Ambient Light Sensor with analog input |
danjulio | 0:83f4079b2bac | 5 | * - Temperature using PT-100 RTD and MAX31865 Amplifier/Digitizer |
danjulio | 0:83f4079b2bac | 6 | * - GPS location and altitude using external serial receiver. Receiver |
danjulio | 0:83f4079b2bac | 7 | * includes local battery for faster acquisition. |
danjulio | 0:83f4079b2bac | 8 | * - Battery voltage using on-board MAX14690 PMIC. |
danjulio | 0:83f4079b2bac | 9 | * - Radio communication for telemetry and command using a RFM95W LoRa module. |
danjulio | 0:83f4079b2bac | 10 | * |
danjulio | 0:83f4079b2bac | 11 | * Data is stored as text sentences to a file on a on-board Micro-SD Card. Data |
danjulio | 0:83f4079b2bac | 12 | * storage is initiated via a user command prior to launch and terminated after |
danjulio | 0:83f4079b2bac | 13 | * a configurable delay set to bound the flight-time. Highly dynamic information |
danjulio | 0:83f4079b2bac | 14 | * such as acceleration and temperature is stored at 100 samples/second. |
danjulio | 0:83f4079b2bac | 15 | * |
danjulio | 0:83f4079b2bac | 16 | * The Micro-SD Card may be made available as a USB Storage Device for data off- |
danjulio | 0:83f4079b2bac | 17 | * load. Data is formatted as follows. |
danjulio | 0:83f4079b2bac | 18 | * |
danjulio | 0:83f4079b2bac | 19 | * File Records - one per line - terminated with CTRL-N |
danjulio | 0:83f4079b2bac | 20 | * ACC,<BMI160_SECS>,<X>,<Y>,<Z>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 21 | * TEMP,<SEQ NUM>,<TEMP>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 22 | * BATT,<SEQ NUM>,<VOLTS>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 23 | * LIGHT,<SEQ NUM>,<ADC VOLTS>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 24 | * GPS,<FIX>,<LATITUDE>,<LAT ORIENTATION>,<LONGITUDE>,<LON ORIENTATION>, |
danjulio | 0:83f4079b2bac | 25 | * <ALTITUDE>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 26 | * |
danjulio | 0:83f4079b2bac | 27 | * <CHECKSUM> is a 2-ASCII HEX value XOR of all bytes from the first character |
danjulio | 0:83f4079b2bac | 28 | * up to but not including the ',*' delimiter. |
danjulio | 0:83f4079b2bac | 29 | * |
danjulio | 0:83f4079b2bac | 30 | * GPS location (and/or other sensor information) may also be transmitted |
danjulio | 0:83f4079b2bac | 31 | * periodically via a LoRa radio to help with rocket retrieval. |
danjulio | 0:83f4079b2bac | 32 | * |
danjulio | 0:83f4079b2bac | 33 | * A simple command shell is available via the LoRa radio and diagnostic serial |
danjulio | 0:83f4079b2bac | 34 | * port supporting the following commands. Commands are terminated with a |
danjulio | 0:83f4079b2bac | 35 | * Newline character. Command output is echoed to the same interface that it |
danjulio | 0:83f4079b2bac | 36 | * was received from (either LoRa or diagnostic serial). Only one interface |
danjulio | 0:83f4079b2bac | 37 | * should ever be used at a time. |
danjulio | 0:83f4079b2bac | 38 | * |
danjulio | 0:83f4079b2bac | 39 | * "status" : Returns device status including recording state, battery voltage, |
danjulio | 0:83f4079b2bac | 40 | * temperature and current GPS coordinates. |
danjulio | 0:83f4079b2bac | 41 | * "file <filename>" : Set the name, <filename>, for the next recording session. |
danjulio | 0:83f4079b2bac | 42 | * The default filename is "flight.txt". |
danjulio | 0:83f4079b2bac | 43 | * "dump [<filename>" : Output the contents of the current or specified |
danjulio | 0:83f4079b2bac | 44 | * recording file. |
danjulio | 0:83f4079b2bac | 45 | * "dur <seconds>" : Set the duration of the recording session in seconds. |
danjulio | 0:83f4079b2bac | 46 | * The default value is 120 seconds. |
danjulio | 0:83f4079b2bac | 47 | * "msd [on/off]" : Enable or disable USB Mass Storage. USB must be detected |
danjulio | 0:83f4079b2bac | 48 | * connected. |
danjulio | 0:83f4079b2bac | 49 | * "send [accel | batt | gps | light | temp] [on/off]" : Enable or disable |
danjulio | 0:83f4079b2bac | 50 | * periodic sensor logging via the current output interface. |
danjulio | 0:83f4079b2bac | 51 | * "record [on/off]" : Enable or disable a recording session. Default is off. |
danjulio | 0:83f4079b2bac | 52 | * "shutdown" : Shut down power (via the PMIC). The MAX32630FTHR will |
danjulio | 0:83f4079b2bac | 53 | * automatically repower immediately if USB Power is connected. |
danjulio | 0:83f4079b2bac | 54 | * "help" : Output this list of commands. |
danjulio | 0:83f4079b2bac | 55 | * |
danjulio | 0:83f4079b2bac | 56 | * Status response takes the form |
danjulio | 0:83f4079b2bac | 57 | * STATUS,<RECORDING>,<BATT VOLTS>,<TEMP>,<FIX>,<LATITUDE>,<LAT ORIENTATION>, |
danjulio | 0:83f4079b2bac | 58 | * <LONGITUDE>,<LON ORIENTATION>,<ALTITUDE>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 59 | * |
danjulio | 0:83f4079b2bac | 60 | * A sign-on message is generated on both daplink and LoRa interfaces containing |
danjulio | 0:83f4079b2bac | 61 | * the firmware revision. |
danjulio | 0:83f4079b2bac | 62 | * HELLO,<VERSION_MAJOR.VERSION_MINOR>,*<CHECKSUM> |
danjulio | 0:83f4079b2bac | 63 | * |
danjulio | 0:83f4079b2bac | 64 | * A typical flight scenerio might be: |
danjulio | 0:83f4079b2bac | 65 | * Preflight |
danjulio | 0:83f4079b2bac | 66 | * 1. Power-on system |
danjulio | 0:83f4079b2bac | 67 | * 2. Prepare rocket for launch |
danjulio | 0:83f4079b2bac | 68 | * 3. Use Status command to verify rocket has GPS acquisition |
danjulio | 0:83f4079b2bac | 69 | * 4. Configure flight recording filename |
danjulio | 0:83f4079b2bac | 70 | * 5. Enable GPS logging |
danjulio | 0:83f4079b2bac | 71 | * 6. Enable recording |
danjulio | 0:83f4079b2bac | 72 | * 7. Launch rocket |
danjulio | 0:83f4079b2bac | 73 | * During flight |
danjulio | 0:83f4079b2bac | 74 | * 1. Record rough position using telemetried GPS location |
danjulio | 0:83f4079b2bac | 75 | * Postflight |
danjulio | 0:83f4079b2bac | 76 | * 1. Use telemetried GPS location to plot rocket's landing position |
danjulio | 0:83f4079b2bac | 77 | * 2. Use either command or direct USB location to obtain flight data from |
danjulio | 0:83f4079b2bac | 78 | * recording file. |
danjulio | 0:83f4079b2bac | 79 | * 3. Power down system |
danjulio | 0:83f4079b2bac | 80 | * |
danjulio | 0:83f4079b2bac | 81 | * System Architecture |
danjulio | 0:83f4079b2bac | 82 | * 1. Individual threads for sensor data aquisition. |
danjulio | 0:83f4079b2bac | 83 | * 2. Main thread handling ougoing communication. |
danjulio | 0:83f4079b2bac | 84 | * 3. Event thread handling command processing. |
danjulio | 0:83f4079b2bac | 85 | * 4. File writing and reading threads for data to/from Micro-SD Card. |
danjulio | 0:83f4079b2bac | 86 | * 5. String FIFOs used for one-way data communication between sensor |
danjulio | 0:83f4079b2bac | 87 | * acquisition threads and logging thread as well as between command |
danjulio | 0:83f4079b2bac | 88 | * response, file dump, GPS acquisition thread and communication thread. |
danjulio | 0:83f4079b2bac | 89 | * 6. Atomically accessible variables used for current battery voltage, |
danjulio | 0:83f4079b2bac | 90 | * current temperature and recording state. |
danjulio | 0:83f4079b2bac | 91 | * 7. USBMSD object for handling Micro-SD as USB Mass Storage Device. |
danjulio | 0:83f4079b2bac | 92 | * |
danjulio | 0:83f4079b2bac | 93 | * Various libraries have been used or ported (thank you to the authors of those |
danjulio | 0:83f4079b2bac | 94 | * libraries). All application code is contained in this file even though it |
danjulio | 0:83f4079b2bac | 95 | * should be distributed in a proper system. Note that the mbed USBMSD_SD |
danjulio | 0:83f4079b2bac | 96 | * library was modified NOT to attempt to "connect" during the object constructor |
danjulio | 0:83f4079b2bac | 97 | * since this blocks until USB is connected. Connection is initiated only by |
danjulio | 0:83f4079b2bac | 98 | * command and only if USB power is detected. |
danjulio | 0:83f4079b2bac | 99 | * |
danjulio | 0:83f4079b2bac | 100 | * Communication Interfaces |
danjulio | 0:83f4079b2bac | 101 | * 1. USB UART Used for diagnostic interface (daplink). |
danjulio | 0:83f4079b2bac | 102 | * 2. UART2 used for GPS interface. |
danjulio | 0:83f4079b2bac | 103 | * 3. I2CM2 used for BMI160 and MAX14690. |
danjulio | 0:83f4079b2bac | 104 | * 4. Software (bit-banged) SPI used for LoRa radio and MAX31865. This is |
danjulio | 0:83f4079b2bac | 105 | * because the built-in hardware SPI seems to require use of hardware |
danjulio | 0:83f4079b2bac | 106 | * slave-select signals and only one of those is available if I2CM2 is |
danjulio | 0:83f4079b2bac | 107 | * also being used. |
danjulio | 0:83f4079b2bac | 108 | * 5. AIN0 connected to MAX14690 MON out (battery voltage). |
danjulio | 0:83f4079b2bac | 109 | * 6. AIN1 connected to ALS-PT19-315C output (light sensor). |
danjulio | 0:83f4079b2bac | 110 | * 7. Internal 1.2V ADC ref. |
danjulio | 0:83f4079b2bac | 111 | * 8. GPIO inputs for MAX31865 ready and LoRa interrupt. |
danjulio | 0:83f4079b2bac | 112 | * 9. GPIO outputs for LoRa reset, SPI slave select, MAX31865 SPI slave select. |
danjulio | 0:83f4079b2bac | 113 | * 10. On-board RGB LED for system operation state indication. |
danjulio | 0:83f4079b2bac | 114 | * 11. GPIO output (P4_0) used for profiling during development. |
danjulio | 0:83f4079b2bac | 115 | * |
danjulio | 0:83f4079b2bac | 116 | * RGB Status LED (lowest to highest priority) |
danjulio | 0:83f4079b2bac | 117 | * YELLOW : System Idle, No GPS Fix |
danjulio | 0:83f4079b2bac | 118 | * BLUE : System Idle, GPS Fix obtained |
danjulio | 0:83f4079b2bac | 119 | * GREEN : System Idle, USB Detected attached |
danjulio | 0:83f4079b2bac | 120 | * BLUE : USB Mass Storage Enabled (USB must be attached) |
danjulio | 0:83f4079b2bac | 121 | * MAGENTA : System Recording |
danjulio | 0:83f4079b2bac | 122 | * RED : System Failure (device initialization or file open failed) |
danjulio | 0:83f4079b2bac | 123 | * |
danjulio | 0:83f4079b2bac | 124 | * |
danjulio | 0:83f4079b2bac | 125 | * Copyright (c) 2017 by Dan Julio. All rights reserved. |
danjulio | 0:83f4079b2bac | 126 | * |
danjulio | 0:83f4079b2bac | 127 | * Permission is hereby granted, free of charge, to any person obtaining a |
danjulio | 0:83f4079b2bac | 128 | * copy of this software and associated documentation files (the "Software"), |
danjulio | 0:83f4079b2bac | 129 | * to deal in the Software without restriction, including without limitation |
danjulio | 0:83f4079b2bac | 130 | * the rights to use, copy, modify, merge, publish, distribute, sublicense, |
danjulio | 0:83f4079b2bac | 131 | * and/or sell copies of the Software, and to permit persons to whom the |
danjulio | 0:83f4079b2bac | 132 | * Software is furnished to do so, subject to the following conditions: |
danjulio | 0:83f4079b2bac | 133 | * |
danjulio | 0:83f4079b2bac | 134 | * The above copyright notice and this permission notice shall be included |
danjulio | 0:83f4079b2bac | 135 | * in all copies or substantial portions of the Software. |
danjulio | 0:83f4079b2bac | 136 | * |
danjulio | 0:83f4079b2bac | 137 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS |
danjulio | 0:83f4079b2bac | 138 | * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
danjulio | 0:83f4079b2bac | 139 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. |
danjulio | 0:83f4079b2bac | 140 | * IN NO EVENT SHALL DAN JULIO BE LIABLE FOR ANY CLAIM, DAMAGES |
danjulio | 0:83f4079b2bac | 141 | * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, |
danjulio | 0:83f4079b2bac | 142 | * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
danjulio | 0:83f4079b2bac | 143 | * OTHER DEALINGS IN THE SOFTWARE. |
danjulio | 0:83f4079b2bac | 144 | * |
danjulio | 0:83f4079b2bac | 145 | */ |
danjulio | 0:83f4079b2bac | 146 | #include <stdio.h> |
danjulio | 0:83f4079b2bac | 147 | #include <stdlib.h> |
danjulio | 0:83f4079b2bac | 148 | #include <errno.h> |
danjulio | 0:83f4079b2bac | 149 | #include <math.h> |
danjulio | 0:83f4079b2bac | 150 | #include "mbed.h" |
danjulio | 0:83f4079b2bac | 151 | #include "max32630fthr.h" |
danjulio | 0:83f4079b2bac | 152 | #include "bmi160.h" |
danjulio | 0:83f4079b2bac | 153 | #include "CommandDispatcher.h" |
danjulio | 0:83f4079b2bac | 154 | #include "gps.h" |
danjulio | 0:83f4079b2bac | 155 | #include "Adafruit_MAX31865.h" |
danjulio | 0:83f4079b2bac | 156 | #include "RH_RF95.h" |
danjulio | 0:83f4079b2bac | 157 | #include "SDFileSystem.h" |
danjulio | 0:83f4079b2bac | 158 | #include "StringFifo.h" |
danjulio | 0:83f4079b2bac | 159 | #include "swspi.h" |
danjulio | 0:83f4079b2bac | 160 | #include "USBMSD_SD.h" |
danjulio | 0:83f4079b2bac | 161 | |
danjulio | 0:83f4079b2bac | 162 | |
danjulio | 0:83f4079b2bac | 163 | // |
danjulio | 0:83f4079b2bac | 164 | // Version |
danjulio | 0:83f4079b2bac | 165 | // |
danjulio | 0:83f4079b2bac | 166 | #define VER_MAJOR 0 |
danjulio | 0:83f4079b2bac | 167 | #define VER_MINOR 8 |
danjulio | 0:83f4079b2bac | 168 | |
danjulio | 0:83f4079b2bac | 169 | |
danjulio | 0:83f4079b2bac | 170 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 171 | // Configuration Constants |
danjulio | 0:83f4079b2bac | 172 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 173 | |
danjulio | 0:83f4079b2bac | 174 | // |
danjulio | 0:83f4079b2bac | 175 | // Length (in characters) of the FIFO used to store sensor data sentences to be |
danjulio | 0:83f4079b2bac | 176 | // logged in the Micro-SD card. This should be long enough to prevent any |
danjulio | 0:83f4079b2bac | 177 | // sensor pushing data into the FIFO from hanging until there is room and ideally |
danjulio | 0:83f4079b2bac | 178 | // determined by analyzing the time required to log data, the maximum flight time |
danjulio | 0:83f4079b2bac | 179 | // and computing the maximum data rate. |
danjulio | 0:83f4079b2bac | 180 | // |
danjulio | 0:83f4079b2bac | 181 | #define LOG_FIFO_LENGTH 16384 |
danjulio | 0:83f4079b2bac | 182 | |
danjulio | 0:83f4079b2bac | 183 | // |
danjulio | 0:83f4079b2bac | 184 | // Length (in characters) for the FIFO used to store GPS data sentences, command |
danjulio | 0:83f4079b2bac | 185 | // responses and file dump sentences to be transmitted via the LoRa radio (which |
danjulio | 0:83f4079b2bac | 186 | // is slow when working in the most reliable configuration) or the diagnostic |
danjulio | 0:83f4079b2bac | 187 | // serial interface. |
danjulio | 0:83f4079b2bac | 188 | // |
danjulio | 0:83f4079b2bac | 189 | #define RSP_FIFO_LENGTH 16384 |
danjulio | 0:83f4079b2bac | 190 | |
danjulio | 0:83f4079b2bac | 191 | // |
danjulio | 0:83f4079b2bac | 192 | // Time (in seconds) for default recording session. |
danjulio | 0:83f4079b2bac | 193 | // |
danjulio | 0:83f4079b2bac | 194 | #define DEF_RECORD_SECONDS 120 |
danjulio | 0:83f4079b2bac | 195 | |
danjulio | 0:83f4079b2bac | 196 | // |
danjulio | 0:83f4079b2bac | 197 | // Default recording file name and maximum length |
danjulio | 0:83f4079b2bac | 198 | // |
danjulio | 0:83f4079b2bac | 199 | #define DEF_RECORD_FILE "flight.txt" |
danjulio | 0:83f4079b2bac | 200 | #define MAX_RECORD_FN_LEN 32 |
danjulio | 0:83f4079b2bac | 201 | |
danjulio | 0:83f4079b2bac | 202 | // |
danjulio | 0:83f4079b2bac | 203 | // Command and sentence limits (number of chars/line) |
danjulio | 0:83f4079b2bac | 204 | // |
danjulio | 0:83f4079b2bac | 205 | #define MAX_CMD_LEN 64 |
danjulio | 0:83f4079b2bac | 206 | #define MAX_CMD_RSP_LEN 128 |
danjulio | 0:83f4079b2bac | 207 | #define MAX_SENTENCE_LEN 80 |
danjulio | 0:83f4079b2bac | 208 | |
danjulio | 0:83f4079b2bac | 209 | // |
danjulio | 0:83f4079b2bac | 210 | // Low-battery auto-shutdown voltage. |
danjulio | 0:83f4079b2bac | 211 | // |
danjulio | 0:83f4079b2bac | 212 | #define SHUTDOWN_VOLTS 3.0f |
danjulio | 0:83f4079b2bac | 213 | |
danjulio | 0:83f4079b2bac | 214 | // |
danjulio | 0:83f4079b2bac | 215 | // ADC Vref volts (this is fixed for MAX32630) |
danjulio | 0:83f4079b2bac | 216 | // |
danjulio | 0:83f4079b2bac | 217 | #define VREF 1.2f |
danjulio | 0:83f4079b2bac | 218 | |
danjulio | 0:83f4079b2bac | 219 | // |
danjulio | 0:83f4079b2bac | 220 | // Status LED set/clear masks (fixed) |
danjulio | 0:83f4079b2bac | 221 | // |
danjulio | 0:83f4079b2bac | 222 | #define ST_STARTUP 0x01 |
danjulio | 0:83f4079b2bac | 223 | #define ST_GPS_FIX 0x02 |
danjulio | 0:83f4079b2bac | 224 | #define ST_USB_PWR 0x04 |
danjulio | 0:83f4079b2bac | 225 | #define ST_RECORD 0x10 |
danjulio | 0:83f4079b2bac | 226 | #define ST_MSD 0x20 |
danjulio | 0:83f4079b2bac | 227 | #define ST_ERROR 0x80 |
danjulio | 0:83f4079b2bac | 228 | |
danjulio | 0:83f4079b2bac | 229 | |
danjulio | 0:83f4079b2bac | 230 | |
danjulio | 0:83f4079b2bac | 231 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 232 | // Objects |
danjulio | 0:83f4079b2bac | 233 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 234 | |
danjulio | 0:83f4079b2bac | 235 | // |
danjulio | 0:83f4079b2bac | 236 | // Hardware-related objects |
danjulio | 0:83f4079b2bac | 237 | // |
danjulio | 0:83f4079b2bac | 238 | MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3); |
danjulio | 0:83f4079b2bac | 239 | |
danjulio | 0:83f4079b2bac | 240 | Serial daplink(P2_1, P2_0); |
danjulio | 0:83f4079b2bac | 241 | |
danjulio | 0:83f4079b2bac | 242 | Serial gps_serial(P3_1, P3_0); |
danjulio | 0:83f4079b2bac | 243 | Adafruit_GPS gps(&gps_serial); |
danjulio | 0:83f4079b2bac | 244 | |
danjulio | 0:83f4079b2bac | 245 | I2C i2cBus(P5_7, P6_0); |
danjulio | 0:83f4079b2bac | 246 | BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO); |
danjulio | 0:83f4079b2bac | 247 | |
danjulio | 0:83f4079b2bac | 248 | swspi sw_spi(P5_1, P5_2, P5_0, P3_4, P5_3); // mosi, miso, sclk, ss1, ss2 |
danjulio | 0:83f4079b2bac | 249 | Adafruit_MAX31865 tempSense(sw_spi, 1, P3_5); // spi, ss<N>, rdyPin |
danjulio | 0:83f4079b2bac | 250 | RH_RF95 radio(sw_spi, 2); // spi, ss<N> |
danjulio | 0:83f4079b2bac | 251 | InterruptIn radioInt(P5_4); |
danjulio | 0:83f4079b2bac | 252 | DigitalOut radioRstN(P5_5); |
danjulio | 0:83f4079b2bac | 253 | |
danjulio | 0:83f4079b2bac | 254 | AnalogIn battSensor(AIN_0); |
danjulio | 0:83f4079b2bac | 255 | AnalogIn lightSensor(AIN_1); |
danjulio | 0:83f4079b2bac | 256 | |
danjulio | 0:83f4079b2bac | 257 | SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // mosi, miso, sclk, cs |
danjulio | 0:83f4079b2bac | 258 | USBMSD_SD ms_sd(P0_5, P0_6, P0_4, P0_7); // mosi, miso, sclk, cs |
danjulio | 0:83f4079b2bac | 259 | |
danjulio | 0:83f4079b2bac | 260 | DigitalOut rLED(LED1); |
danjulio | 0:83f4079b2bac | 261 | DigitalOut gLED(LED2); |
danjulio | 0:83f4079b2bac | 262 | DigitalOut bLED(LED3); |
danjulio | 0:83f4079b2bac | 263 | |
danjulio | 0:83f4079b2bac | 264 | DigitalOut profOut(P4_0); |
danjulio | 0:83f4079b2bac | 265 | |
danjulio | 0:83f4079b2bac | 266 | |
danjulio | 0:83f4079b2bac | 267 | // |
danjulio | 0:83f4079b2bac | 268 | // Data objects |
danjulio | 0:83f4079b2bac | 269 | // |
danjulio | 0:83f4079b2bac | 270 | StringFifo sensorFifo(LOG_FIFO_LENGTH); |
danjulio | 0:83f4079b2bac | 271 | StringFifo rspFifo(RSP_FIFO_LENGTH); |
danjulio | 0:83f4079b2bac | 272 | |
danjulio | 0:83f4079b2bac | 273 | CommandDispatcher cmdDispatcher; |
danjulio | 0:83f4079b2bac | 274 | |
danjulio | 0:83f4079b2bac | 275 | EventQueue mQueue(16*EVENTS_EVENT_SIZE); |
danjulio | 0:83f4079b2bac | 276 | |
danjulio | 0:83f4079b2bac | 277 | |
danjulio | 0:83f4079b2bac | 278 | // |
danjulio | 0:83f4079b2bac | 279 | // Threads |
danjulio | 0:83f4079b2bac | 280 | // |
danjulio | 0:83f4079b2bac | 281 | Thread mQueueThread; |
danjulio | 0:83f4079b2bac | 282 | Thread daplinkRxThread; |
danjulio | 0:83f4079b2bac | 283 | Thread accelThread; |
danjulio | 0:83f4079b2bac | 284 | Thread battThread; |
danjulio | 0:83f4079b2bac | 285 | Thread gpsRxThread; |
danjulio | 0:83f4079b2bac | 286 | Thread gpsLogThread; |
danjulio | 0:83f4079b2bac | 287 | Thread lightThread; |
danjulio | 0:83f4079b2bac | 288 | Thread tempThread; |
danjulio | 0:83f4079b2bac | 289 | Thread fileWriteThread; |
danjulio | 0:83f4079b2bac | 290 | Thread fileReadThread; |
danjulio | 0:83f4079b2bac | 291 | |
danjulio | 0:83f4079b2bac | 292 | |
danjulio | 0:83f4079b2bac | 293 | // |
danjulio | 0:83f4079b2bac | 294 | // Sensor trigger timers |
danjulio | 0:83f4079b2bac | 295 | // |
danjulio | 0:83f4079b2bac | 296 | Ticker accelTicker; |
danjulio | 0:83f4079b2bac | 297 | Ticker battTicker; |
danjulio | 0:83f4079b2bac | 298 | Ticker gpsTicker; |
danjulio | 0:83f4079b2bac | 299 | Ticker lightTicker; |
danjulio | 0:83f4079b2bac | 300 | Ticker tempTicker; |
danjulio | 0:83f4079b2bac | 301 | |
danjulio | 0:83f4079b2bac | 302 | |
danjulio | 0:83f4079b2bac | 303 | |
danjulio | 0:83f4079b2bac | 304 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 305 | // Global Variables |
danjulio | 0:83f4079b2bac | 306 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 307 | |
danjulio | 0:83f4079b2bac | 308 | // |
danjulio | 0:83f4079b2bac | 309 | // RGB Status LED colors |
danjulio | 0:83f4079b2bac | 310 | // |
danjulio | 0:83f4079b2bac | 311 | enum LedColors |
danjulio | 0:83f4079b2bac | 312 | { |
danjulio | 0:83f4079b2bac | 313 | BLACK, |
danjulio | 0:83f4079b2bac | 314 | RED, |
danjulio | 0:83f4079b2bac | 315 | YELLOW, |
danjulio | 0:83f4079b2bac | 316 | GREEN, |
danjulio | 0:83f4079b2bac | 317 | CYAN, |
danjulio | 0:83f4079b2bac | 318 | BLUE, |
danjulio | 0:83f4079b2bac | 319 | MAGENTA, |
danjulio | 0:83f4079b2bac | 320 | WHITE |
danjulio | 0:83f4079b2bac | 321 | }; |
danjulio | 0:83f4079b2bac | 322 | |
danjulio | 0:83f4079b2bac | 323 | |
danjulio | 0:83f4079b2bac | 324 | // |
danjulio | 0:83f4079b2bac | 325 | // Indicies into sensor-related control arrays |
danjulio | 0:83f4079b2bac | 326 | // |
danjulio | 0:83f4079b2bac | 327 | enum SensorSelect |
danjulio | 0:83f4079b2bac | 328 | { |
danjulio | 0:83f4079b2bac | 329 | ACCEL = 0, |
danjulio | 0:83f4079b2bac | 330 | BATT, |
danjulio | 0:83f4079b2bac | 331 | GPS, |
danjulio | 0:83f4079b2bac | 332 | LIGHT, |
danjulio | 0:83f4079b2bac | 333 | TEMP |
danjulio | 0:83f4079b2bac | 334 | }; |
danjulio | 0:83f4079b2bac | 335 | |
danjulio | 0:83f4079b2bac | 336 | |
danjulio | 0:83f4079b2bac | 337 | // |
danjulio | 0:83f4079b2bac | 338 | // System state |
danjulio | 0:83f4079b2bac | 339 | // |
danjulio | 0:83f4079b2bac | 340 | bool enSnsLogging[5]; // Set to enable transmitting various sensor values |
danjulio | 0:83f4079b2bac | 341 | bool enSnsTick[5]; // Set when time for a sensor to log or record info |
danjulio | 0:83f4079b2bac | 342 | |
danjulio | 0:83f4079b2bac | 343 | bool trigRecording = false; // {command} --> {file writer} |
danjulio | 0:83f4079b2bac | 344 | bool trigEndRecording = false; // {command, timer} --> {file writer} |
danjulio | 0:83f4079b2bac | 345 | bool enRecording = false; // Set by command to note recording, cleared by writer |
danjulio | 0:83f4079b2bac | 346 | char curRecFileName[MAX_RECORD_FN_LEN + 1]; |
danjulio | 0:83f4079b2bac | 347 | float recTimeoutSecs = DEF_RECORD_SECONDS; |
danjulio | 0:83f4079b2bac | 348 | FILE* fp; // Use for recording or dumping file |
danjulio | 0:83f4079b2bac | 349 | |
danjulio | 0:83f4079b2bac | 350 | bool trigFileDump = false; // {command} --> {file reader} |
danjulio | 0:83f4079b2bac | 351 | bool enFileDump = false; // Set by command to note dumping, cleared by reader |
danjulio | 0:83f4079b2bac | 352 | char curReadFileName[MAX_RECORD_FN_LEN + 1]; |
danjulio | 0:83f4079b2bac | 353 | |
danjulio | 0:83f4079b2bac | 354 | bool enMassStorage = false; // Set by command when mass storage is enabled |
danjulio | 0:83f4079b2bac | 355 | |
danjulio | 0:83f4079b2bac | 356 | // |
danjulio | 0:83f4079b2bac | 357 | // Command and output processing |
danjulio | 0:83f4079b2bac | 358 | // |
danjulio | 0:83f4079b2bac | 359 | volatile bool cmdFromRadio = true; |
danjulio | 0:83f4079b2bac | 360 | char cmdString[MAX_CMD_LEN+1]; |
danjulio | 0:83f4079b2bac | 361 | |
danjulio | 0:83f4079b2bac | 362 | // |
danjulio | 0:83f4079b2bac | 363 | // System status |
danjulio | 0:83f4079b2bac | 364 | // |
danjulio | 0:83f4079b2bac | 365 | float curBattV = 0.0f; |
danjulio | 0:83f4079b2bac | 366 | float curTempC = 0.0f; |
danjulio | 0:83f4079b2bac | 367 | Mutex gpsMutex; |
danjulio | 0:83f4079b2bac | 368 | float gpsLat = 0.0f; |
danjulio | 0:83f4079b2bac | 369 | char gpsLatOrient = ' '; |
danjulio | 0:83f4079b2bac | 370 | float gpsLon = 0.0f; |
danjulio | 0:83f4079b2bac | 371 | char gpsLonOrient = ' '; |
danjulio | 0:83f4079b2bac | 372 | float gpsAlt = 0.0f; |
danjulio | 0:83f4079b2bac | 373 | bool gpsUpdated = false; // {GpsRx} -> {GpsLog} |
danjulio | 0:83f4079b2bac | 374 | bool gpsFix = false; |
danjulio | 0:83f4079b2bac | 375 | |
danjulio | 0:83f4079b2bac | 376 | bool usbPower = false; // Set while the battery thread detects ext pwr |
danjulio | 0:83f4079b2bac | 377 | |
danjulio | 0:83f4079b2bac | 378 | uint8_t ledStatus = 0x00; |
danjulio | 0:83f4079b2bac | 379 | |
danjulio | 0:83f4079b2bac | 380 | |
danjulio | 0:83f4079b2bac | 381 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 382 | // Necessary forward declarations |
danjulio | 0:83f4079b2bac | 383 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 384 | |
danjulio | 0:83f4079b2bac | 385 | void CommandEvent(); |
danjulio | 0:83f4079b2bac | 386 | void RecordEndEvent(); |
danjulio | 0:83f4079b2bac | 387 | void ShutdownEvent(); |
danjulio | 0:83f4079b2bac | 388 | bool InitRadio(); |
danjulio | 0:83f4079b2bac | 389 | void SetStatusLed(uint8_t f); |
danjulio | 0:83f4079b2bac | 390 | void ClearStatusLed(uint8_t f); |
danjulio | 0:83f4079b2bac | 391 | void UpdateStatusLed(); |
danjulio | 0:83f4079b2bac | 392 | float GpsNMEAtoFrac(float c); |
danjulio | 0:83f4079b2bac | 393 | void AddChecksum(char* s); |
danjulio | 0:83f4079b2bac | 394 | |
danjulio | 0:83f4079b2bac | 395 | |
danjulio | 0:83f4079b2bac | 396 | |
danjulio | 0:83f4079b2bac | 397 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 398 | // Thread code |
danjulio | 0:83f4079b2bac | 399 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 400 | |
danjulio | 0:83f4079b2bac | 401 | void daplinkRxProcess() |
danjulio | 0:83f4079b2bac | 402 | { |
danjulio | 0:83f4079b2bac | 403 | int rxIndex = 0; |
danjulio | 0:83f4079b2bac | 404 | char c; |
danjulio | 0:83f4079b2bac | 405 | |
danjulio | 0:83f4079b2bac | 406 | while (1) { |
danjulio | 0:83f4079b2bac | 407 | // Wait for and then process incoming data |
danjulio | 0:83f4079b2bac | 408 | while (!daplink.readable()) { |
danjulio | 0:83f4079b2bac | 409 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 410 | } |
danjulio | 0:83f4079b2bac | 411 | |
danjulio | 0:83f4079b2bac | 412 | // Process a character |
danjulio | 0:83f4079b2bac | 413 | c = daplink.getc(); |
danjulio | 0:83f4079b2bac | 414 | if (c == '\n') { |
danjulio | 0:83f4079b2bac | 415 | cmdString[rxIndex] = 0; |
danjulio | 0:83f4079b2bac | 416 | rxIndex = 0; |
danjulio | 0:83f4079b2bac | 417 | cmdFromRadio = false; |
danjulio | 0:83f4079b2bac | 418 | mQueue.call(CommandEvent); |
danjulio | 0:83f4079b2bac | 419 | } else if ((c != '\r') && (rxIndex < MAX_CMD_LEN)) { |
danjulio | 0:83f4079b2bac | 420 | cmdString[rxIndex++] = c; |
danjulio | 0:83f4079b2bac | 421 | } |
danjulio | 0:83f4079b2bac | 422 | } |
danjulio | 0:83f4079b2bac | 423 | } |
danjulio | 0:83f4079b2bac | 424 | |
danjulio | 0:83f4079b2bac | 425 | |
danjulio | 0:83f4079b2bac | 426 | void AccelProcess() |
danjulio | 0:83f4079b2bac | 427 | { |
danjulio | 0:83f4079b2bac | 428 | BMI160::AccConfig accConfig; |
danjulio | 0:83f4079b2bac | 429 | BMI160::SensorData accData; |
danjulio | 0:83f4079b2bac | 430 | BMI160::SensorTime sensorTime; |
danjulio | 0:83f4079b2bac | 431 | char accelString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 432 | uint32_t seqNum = 0; |
danjulio | 0:83f4079b2bac | 433 | bool initSuccess = true; // Will be set false if necessary |
danjulio | 0:83f4079b2bac | 434 | |
danjulio | 0:83f4079b2bac | 435 | // Setup accelerometer configuration |
danjulio | 0:83f4079b2bac | 436 | accConfig.range = BMI160::SENS_16G; |
danjulio | 0:83f4079b2bac | 437 | accConfig.us = BMI160::ACC_US_OFF; |
danjulio | 0:83f4079b2bac | 438 | accConfig.bwp = BMI160::ACC_BWP_2; |
danjulio | 0:83f4079b2bac | 439 | accConfig.odr = BMI160::ACC_ODR_8; |
danjulio | 0:83f4079b2bac | 440 | |
danjulio | 0:83f4079b2bac | 441 | // Initialize |
danjulio | 0:83f4079b2bac | 442 | if (imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) { |
danjulio | 0:83f4079b2bac | 443 | daplink.printf("Failed to set gyro power mode\n\r"); |
danjulio | 0:83f4079b2bac | 444 | initSuccess = false; |
danjulio | 0:83f4079b2bac | 445 | } |
danjulio | 0:83f4079b2bac | 446 | wait_ms(100); |
danjulio | 0:83f4079b2bac | 447 | if (imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) { |
danjulio | 0:83f4079b2bac | 448 | daplink.printf("Failed to set accelerometer power mode\n\r"); |
danjulio | 0:83f4079b2bac | 449 | initSuccess = false; |
danjulio | 0:83f4079b2bac | 450 | } |
danjulio | 0:83f4079b2bac | 451 | wait_ms(100); |
danjulio | 0:83f4079b2bac | 452 | if (initSuccess){ |
danjulio | 0:83f4079b2bac | 453 | if (imu.setSensorConfig(accConfig) != BMI160::RTN_NO_ERROR) { |
danjulio | 0:83f4079b2bac | 454 | daplink.printf("Failed to reconfigure accelerometer\n\r"); |
danjulio | 0:83f4079b2bac | 455 | initSuccess = false; |
danjulio | 0:83f4079b2bac | 456 | } |
danjulio | 0:83f4079b2bac | 457 | } |
danjulio | 0:83f4079b2bac | 458 | |
danjulio | 0:83f4079b2bac | 459 | if (!initSuccess) { |
danjulio | 0:83f4079b2bac | 460 | SetStatusLed(ST_ERROR); |
danjulio | 0:83f4079b2bac | 461 | } else { |
danjulio | 0:83f4079b2bac | 462 | // Main loop |
danjulio | 0:83f4079b2bac | 463 | while (1) { |
danjulio | 0:83f4079b2bac | 464 | // Wait to be triggered |
danjulio | 0:83f4079b2bac | 465 | while (!enSnsTick[ACCEL]) { |
danjulio | 0:83f4079b2bac | 466 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 467 | } |
danjulio | 0:83f4079b2bac | 468 | enSnsTick[ACCEL] = false; |
danjulio | 0:83f4079b2bac | 469 | |
danjulio | 0:83f4079b2bac | 470 | // Read accelerometer |
danjulio | 0:83f4079b2bac | 471 | (void) imu.getSensorXYZandSensorTime(accData, sensorTime, accConfig.range); |
danjulio | 0:83f4079b2bac | 472 | |
danjulio | 0:83f4079b2bac | 473 | // Send for logging |
danjulio | 0:83f4079b2bac | 474 | sprintf(accelString, "ACC,%f,%f,%f,%f", sensorTime.seconds, |
danjulio | 0:83f4079b2bac | 475 | accData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled); |
danjulio | 0:83f4079b2bac | 476 | AddChecksum(accelString); |
danjulio | 0:83f4079b2bac | 477 | |
danjulio | 0:83f4079b2bac | 478 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 479 | sensorFifo.PushString(accelString); |
danjulio | 0:83f4079b2bac | 480 | } |
danjulio | 0:83f4079b2bac | 481 | |
danjulio | 0:83f4079b2bac | 482 | if ((enSnsLogging[ACCEL] && (seqNum++ % 100) == 0)) { |
danjulio | 0:83f4079b2bac | 483 | // Only send to our user occasionally |
danjulio | 0:83f4079b2bac | 484 | rspFifo.PushString(accelString); |
danjulio | 0:83f4079b2bac | 485 | } |
danjulio | 0:83f4079b2bac | 486 | } |
danjulio | 0:83f4079b2bac | 487 | } |
danjulio | 0:83f4079b2bac | 488 | } |
danjulio | 0:83f4079b2bac | 489 | |
danjulio | 0:83f4079b2bac | 490 | |
danjulio | 0:83f4079b2bac | 491 | void BattProcess() |
danjulio | 0:83f4079b2bac | 492 | { |
danjulio | 0:83f4079b2bac | 493 | char battString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 494 | uint32_t seqNum = 0; |
danjulio | 0:83f4079b2bac | 495 | char regVal; |
danjulio | 0:83f4079b2bac | 496 | |
danjulio | 0:83f4079b2bac | 497 | // Initialize - setup the monitor input for 1:4 scaling to fit our |
danjulio | 0:83f4079b2bac | 498 | // ADC's fullscale range |
danjulio | 0:83f4079b2bac | 499 | pegasus.max14690.monSet(pegasus.max14690.MON_BAT, pegasus.max14690.MON_DIV4); |
danjulio | 0:83f4079b2bac | 500 | |
danjulio | 0:83f4079b2bac | 501 | while (1) { |
danjulio | 0:83f4079b2bac | 502 | // Wait to be triggered |
danjulio | 0:83f4079b2bac | 503 | while (!enSnsTick[BATT]) { |
danjulio | 0:83f4079b2bac | 504 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 505 | } |
danjulio | 0:83f4079b2bac | 506 | enSnsTick[BATT] = false; |
danjulio | 0:83f4079b2bac | 507 | |
danjulio | 0:83f4079b2bac | 508 | // Read battery voltage and check for shutdown condition |
danjulio | 0:83f4079b2bac | 509 | curBattV = battSensor.read()*4.0f*VREF; |
danjulio | 0:83f4079b2bac | 510 | |
danjulio | 0:83f4079b2bac | 511 | if (curBattV < SHUTDOWN_VOLTS) { |
danjulio | 0:83f4079b2bac | 512 | // Attempt to tell the world we're shutting down |
danjulio | 0:83f4079b2bac | 513 | rspFifo.PushString("Low Battery Shutdown"); |
danjulio | 0:83f4079b2bac | 514 | |
danjulio | 0:83f4079b2bac | 515 | // Schedule shutdown |
danjulio | 0:83f4079b2bac | 516 | mQueue.call(ShutdownEvent); |
danjulio | 0:83f4079b2bac | 517 | } |
danjulio | 0:83f4079b2bac | 518 | |
danjulio | 0:83f4079b2bac | 519 | // Send for logging |
danjulio | 0:83f4079b2bac | 520 | sprintf(battString, "BATT,%d,%1.2f", seqNum++, curBattV); |
danjulio | 0:83f4079b2bac | 521 | AddChecksum(battString); |
danjulio | 0:83f4079b2bac | 522 | |
danjulio | 0:83f4079b2bac | 523 | |
danjulio | 0:83f4079b2bac | 524 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 525 | sensorFifo.PushString(battString); |
danjulio | 0:83f4079b2bac | 526 | } |
danjulio | 0:83f4079b2bac | 527 | |
danjulio | 0:83f4079b2bac | 528 | if (enSnsLogging[BATT]) { |
danjulio | 0:83f4079b2bac | 529 | rspFifo.PushString(battString); |
danjulio | 0:83f4079b2bac | 530 | } |
danjulio | 0:83f4079b2bac | 531 | |
danjulio | 0:83f4079b2bac | 532 | // Read the MAX14690 StatusB register bit[3] UsbOk to determine |
danjulio | 0:83f4079b2bac | 533 | // if we are connected to a USB system or not |
danjulio | 0:83f4079b2bac | 534 | if (pegasus.max14690.readReg(MAX14690::REG_STATUS_B, ®Val) == MAX14690_NO_ERROR) { |
danjulio | 0:83f4079b2bac | 535 | if (regVal & 0x08) { |
danjulio | 0:83f4079b2bac | 536 | if (!usbPower) { |
danjulio | 0:83f4079b2bac | 537 | usbPower = true; |
danjulio | 0:83f4079b2bac | 538 | SetStatusLed(ST_USB_PWR); |
danjulio | 0:83f4079b2bac | 539 | } |
danjulio | 0:83f4079b2bac | 540 | } else { |
danjulio | 0:83f4079b2bac | 541 | if (usbPower) { |
danjulio | 0:83f4079b2bac | 542 | usbPower = false; |
danjulio | 0:83f4079b2bac | 543 | ClearStatusLed(ST_USB_PWR); |
danjulio | 0:83f4079b2bac | 544 | } |
danjulio | 0:83f4079b2bac | 545 | } |
danjulio | 0:83f4079b2bac | 546 | } |
danjulio | 0:83f4079b2bac | 547 | } |
danjulio | 0:83f4079b2bac | 548 | } |
danjulio | 0:83f4079b2bac | 549 | |
danjulio | 0:83f4079b2bac | 550 | |
danjulio | 0:83f4079b2bac | 551 | void GpsRxProcess() |
danjulio | 0:83f4079b2bac | 552 | { |
danjulio | 0:83f4079b2bac | 553 | // Initialize |
danjulio | 0:83f4079b2bac | 554 | gps.begin(9600); |
danjulio | 0:83f4079b2bac | 555 | |
danjulio | 0:83f4079b2bac | 556 | while (1) { |
danjulio | 0:83f4079b2bac | 557 | // Read complete lines from the GPS receiver (blocks until data avail) |
danjulio | 0:83f4079b2bac | 558 | gps.readNMEA(); |
danjulio | 0:83f4079b2bac | 559 | |
danjulio | 0:83f4079b2bac | 560 | // Process any received NMEA sentences |
danjulio | 0:83f4079b2bac | 561 | if (gps.newNMEAreceived()) { |
danjulio | 0:83f4079b2bac | 562 | if (gps.parse(gps.lastNMEA())) { |
danjulio | 0:83f4079b2bac | 563 | if (gps.fix) { |
danjulio | 0:83f4079b2bac | 564 | // Update our status |
danjulio | 0:83f4079b2bac | 565 | gpsMutex.lock(); |
danjulio | 0:83f4079b2bac | 566 | gpsLat = GpsNMEAtoFrac(gps.latitude); |
danjulio | 0:83f4079b2bac | 567 | gpsLatOrient = gps.lat; |
danjulio | 0:83f4079b2bac | 568 | gpsLon = GpsNMEAtoFrac(gps.longitude); |
danjulio | 0:83f4079b2bac | 569 | gpsLonOrient = gps.lon; |
danjulio | 0:83f4079b2bac | 570 | gpsAlt = gps.altitude; |
danjulio | 0:83f4079b2bac | 571 | gpsUpdated = true; |
danjulio | 0:83f4079b2bac | 572 | gpsMutex.unlock(); |
danjulio | 0:83f4079b2bac | 573 | } |
danjulio | 0:83f4079b2bac | 574 | } |
danjulio | 0:83f4079b2bac | 575 | } |
danjulio | 0:83f4079b2bac | 576 | } |
danjulio | 0:83f4079b2bac | 577 | } |
danjulio | 0:83f4079b2bac | 578 | |
danjulio | 0:83f4079b2bac | 579 | |
danjulio | 0:83f4079b2bac | 580 | void GpsLogProcess() |
danjulio | 0:83f4079b2bac | 581 | { |
danjulio | 0:83f4079b2bac | 582 | char gpsString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 583 | int gpsMissedUpdateCount = 0; |
danjulio | 0:83f4079b2bac | 584 | |
danjulio | 0:83f4079b2bac | 585 | while (1) { |
danjulio | 0:83f4079b2bac | 586 | // Wait to be triggered |
danjulio | 0:83f4079b2bac | 587 | while (!enSnsTick[GPS]) { |
danjulio | 0:83f4079b2bac | 588 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 589 | } |
danjulio | 0:83f4079b2bac | 590 | enSnsTick[GPS] = false; |
danjulio | 0:83f4079b2bac | 591 | |
danjulio | 0:83f4079b2bac | 592 | // Evaluate fix detecton logic |
danjulio | 0:83f4079b2bac | 593 | if (gpsUpdated) { |
danjulio | 0:83f4079b2bac | 594 | gpsUpdated = false; |
danjulio | 0:83f4079b2bac | 595 | gpsMissedUpdateCount = 0; |
danjulio | 0:83f4079b2bac | 596 | if (!gpsFix) { |
danjulio | 0:83f4079b2bac | 597 | gpsFix = true; |
danjulio | 0:83f4079b2bac | 598 | SetStatusLed(ST_GPS_FIX); |
danjulio | 0:83f4079b2bac | 599 | } |
danjulio | 0:83f4079b2bac | 600 | } else { |
danjulio | 0:83f4079b2bac | 601 | if (++gpsMissedUpdateCount > 5) { |
danjulio | 0:83f4079b2bac | 602 | if (gpsFix) { |
danjulio | 0:83f4079b2bac | 603 | gpsFix = false; |
danjulio | 0:83f4079b2bac | 604 | ClearStatusLed(ST_GPS_FIX); |
danjulio | 0:83f4079b2bac | 605 | } |
danjulio | 0:83f4079b2bac | 606 | } |
danjulio | 0:83f4079b2bac | 607 | } |
danjulio | 0:83f4079b2bac | 608 | |
danjulio | 0:83f4079b2bac | 609 | // Send for logging |
danjulio | 0:83f4079b2bac | 610 | gpsMutex.lock(); |
danjulio | 0:83f4079b2bac | 611 | sprintf(gpsString, "GPS,%d,%1.6f,%c,%1.6f,%c,%1.1f", |
danjulio | 0:83f4079b2bac | 612 | gpsFix, gpsLat, gpsLatOrient, gpsLon, gpsLonOrient, gps.altitude); |
danjulio | 0:83f4079b2bac | 613 | gpsMutex.unlock(); |
danjulio | 0:83f4079b2bac | 614 | AddChecksum(gpsString); |
danjulio | 0:83f4079b2bac | 615 | |
danjulio | 0:83f4079b2bac | 616 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 617 | sensorFifo.PushString(gpsString); |
danjulio | 0:83f4079b2bac | 618 | } |
danjulio | 0:83f4079b2bac | 619 | |
danjulio | 0:83f4079b2bac | 620 | if (enSnsLogging[GPS]) { |
danjulio | 0:83f4079b2bac | 621 | rspFifo.PushString(gpsString); |
danjulio | 0:83f4079b2bac | 622 | } |
danjulio | 0:83f4079b2bac | 623 | } |
danjulio | 0:83f4079b2bac | 624 | } |
danjulio | 0:83f4079b2bac | 625 | |
danjulio | 0:83f4079b2bac | 626 | |
danjulio | 0:83f4079b2bac | 627 | void LightProcess() |
danjulio | 0:83f4079b2bac | 628 | { |
danjulio | 0:83f4079b2bac | 629 | char lightString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 630 | uint32_t seqNum = 0; |
danjulio | 0:83f4079b2bac | 631 | float lightV; |
danjulio | 0:83f4079b2bac | 632 | |
danjulio | 0:83f4079b2bac | 633 | while (1) { |
danjulio | 0:83f4079b2bac | 634 | // Wait to be triggered |
danjulio | 0:83f4079b2bac | 635 | while (!enSnsTick[LIGHT]) { |
danjulio | 0:83f4079b2bac | 636 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 637 | } |
danjulio | 0:83f4079b2bac | 638 | enSnsTick[LIGHT] = false; |
danjulio | 0:83f4079b2bac | 639 | |
danjulio | 0:83f4079b2bac | 640 | // Read light sensor voltage |
danjulio | 0:83f4079b2bac | 641 | lightV = lightSensor.read()*VREF; |
danjulio | 0:83f4079b2bac | 642 | |
danjulio | 0:83f4079b2bac | 643 | // Send for logging |
danjulio | 0:83f4079b2bac | 644 | sprintf(lightString, "LIGHT,%d,%1.2f", seqNum++, lightV); |
danjulio | 0:83f4079b2bac | 645 | AddChecksum(lightString); |
danjulio | 0:83f4079b2bac | 646 | |
danjulio | 0:83f4079b2bac | 647 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 648 | sensorFifo.PushString(lightString); |
danjulio | 0:83f4079b2bac | 649 | } |
danjulio | 0:83f4079b2bac | 650 | |
danjulio | 0:83f4079b2bac | 651 | if ((enSnsLogging[LIGHT] && (seqNum % 100) == 0)) { |
danjulio | 0:83f4079b2bac | 652 | // Only send to our user occasionally |
danjulio | 0:83f4079b2bac | 653 | rspFifo.PushString(lightString); |
danjulio | 0:83f4079b2bac | 654 | } |
danjulio | 0:83f4079b2bac | 655 | } |
danjulio | 0:83f4079b2bac | 656 | } |
danjulio | 0:83f4079b2bac | 657 | |
danjulio | 0:83f4079b2bac | 658 | |
danjulio | 0:83f4079b2bac | 659 | void TempProcess() |
danjulio | 0:83f4079b2bac | 660 | { |
danjulio | 0:83f4079b2bac | 661 | char tempString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 662 | uint32_t seqNum = 0; |
danjulio | 0:83f4079b2bac | 663 | uint16_t rtd; |
danjulio | 0:83f4079b2bac | 664 | |
danjulio | 0:83f4079b2bac | 665 | // Initialize temperature sensor for continuous sensing |
danjulio | 0:83f4079b2bac | 666 | tempSense.begin(); |
danjulio | 0:83f4079b2bac | 667 | tempSense.enableBias(true); |
danjulio | 0:83f4079b2bac | 668 | tempSense.autoConvert(true); |
danjulio | 0:83f4079b2bac | 669 | |
danjulio | 0:83f4079b2bac | 670 | while (1) { |
danjulio | 0:83f4079b2bac | 671 | // Wait to be triggered |
danjulio | 0:83f4079b2bac | 672 | while (!enSnsTick[TEMP]) { |
danjulio | 0:83f4079b2bac | 673 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 674 | } |
danjulio | 0:83f4079b2bac | 675 | enSnsTick[TEMP] = false; |
danjulio | 0:83f4079b2bac | 676 | |
danjulio | 0:83f4079b2bac | 677 | // Get the temperature if available |
danjulio | 0:83f4079b2bac | 678 | if (tempSense.isReady(&rtd)) { |
danjulio | 0:83f4079b2bac | 679 | // Using 100 ohm RTD and 430 ohm reference resistor |
danjulio | 0:83f4079b2bac | 680 | curTempC = tempSense.temperature(100.0f, 430.0f, rtd); |
danjulio | 0:83f4079b2bac | 681 | |
danjulio | 0:83f4079b2bac | 682 | // Send for logging |
danjulio | 0:83f4079b2bac | 683 | sprintf(tempString, "TEMP,%d,%1.2f", seqNum++, curTempC); |
danjulio | 0:83f4079b2bac | 684 | AddChecksum(tempString); |
danjulio | 0:83f4079b2bac | 685 | |
danjulio | 0:83f4079b2bac | 686 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 687 | sensorFifo.PushString(tempString); |
danjulio | 0:83f4079b2bac | 688 | } |
danjulio | 0:83f4079b2bac | 689 | |
danjulio | 0:83f4079b2bac | 690 | if ((enSnsLogging[TEMP] && (seqNum % 10) == 0)) { |
danjulio | 0:83f4079b2bac | 691 | // Only send to our user occasionally |
danjulio | 0:83f4079b2bac | 692 | rspFifo.PushString(tempString); |
danjulio | 0:83f4079b2bac | 693 | } |
danjulio | 0:83f4079b2bac | 694 | } |
danjulio | 0:83f4079b2bac | 695 | } |
danjulio | 0:83f4079b2bac | 696 | } |
danjulio | 0:83f4079b2bac | 697 | |
danjulio | 0:83f4079b2bac | 698 | |
danjulio | 0:83f4079b2bac | 699 | void FileWriteProcess() |
danjulio | 0:83f4079b2bac | 700 | { |
danjulio | 0:83f4079b2bac | 701 | char fullRecFileName[MAX_RECORD_FN_LEN + 5]; |
danjulio | 0:83f4079b2bac | 702 | char logString[MAX_SENTENCE_LEN]; |
danjulio | 0:83f4079b2bac | 703 | |
danjulio | 0:83f4079b2bac | 704 | while (1) { |
danjulio | 0:83f4079b2bac | 705 | // Each time through before possibly blocking on the fifo, check |
danjulio | 0:83f4079b2bac | 706 | // if we need to close the file after recording finishes |
danjulio | 0:83f4079b2bac | 707 | if (trigEndRecording && enRecording) { |
danjulio | 0:83f4079b2bac | 708 | enRecording = false; |
danjulio | 0:83f4079b2bac | 709 | trigEndRecording = false; |
danjulio | 0:83f4079b2bac | 710 | fclose(fp); |
danjulio | 0:83f4079b2bac | 711 | ClearStatusLed(ST_RECORD); |
danjulio | 0:83f4079b2bac | 712 | } |
danjulio | 0:83f4079b2bac | 713 | |
danjulio | 0:83f4079b2bac | 714 | // Look for sensor data |
danjulio | 0:83f4079b2bac | 715 | sensorFifo.PopString(logString); |
danjulio | 0:83f4079b2bac | 716 | |
danjulio | 0:83f4079b2bac | 717 | // See if we need to start recording |
danjulio | 0:83f4079b2bac | 718 | if (trigRecording) { |
danjulio | 0:83f4079b2bac | 719 | trigRecording = false; |
danjulio | 0:83f4079b2bac | 720 | // Attempt to open the file for writing |
danjulio | 0:83f4079b2bac | 721 | strcpy(fullRecFileName, "/sd/"); |
danjulio | 0:83f4079b2bac | 722 | strcpy(&fullRecFileName[4], curRecFileName); |
danjulio | 0:83f4079b2bac | 723 | fp = fopen(fullRecFileName, "w"); |
danjulio | 0:83f4079b2bac | 724 | if (fp == 0) { |
danjulio | 0:83f4079b2bac | 725 | enRecording = false; |
danjulio | 0:83f4079b2bac | 726 | sprintf(logString, "Cannot open %s, errno = %d", fullRecFileName, errno); |
danjulio | 0:83f4079b2bac | 727 | rspFifo.PushString(logString); |
danjulio | 0:83f4079b2bac | 728 | SetStatusLed(ST_ERROR); |
danjulio | 0:83f4079b2bac | 729 | } else { |
danjulio | 0:83f4079b2bac | 730 | mQueue.call_in(recTimeoutSecs*1000, RecordEndEvent); |
danjulio | 0:83f4079b2bac | 731 | ClearStatusLed(ST_ERROR); // Just in case they screwed up the filename earlier |
danjulio | 0:83f4079b2bac | 732 | SetStatusLed(ST_RECORD); |
danjulio | 0:83f4079b2bac | 733 | } |
danjulio | 0:83f4079b2bac | 734 | } |
danjulio | 0:83f4079b2bac | 735 | |
danjulio | 0:83f4079b2bac | 736 | // Write to file if we are logging (Unix style) |
danjulio | 0:83f4079b2bac | 737 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 738 | profOut = 1; |
danjulio | 0:83f4079b2bac | 739 | fprintf(fp, "%s\n", logString); |
danjulio | 0:83f4079b2bac | 740 | profOut = 0; |
danjulio | 0:83f4079b2bac | 741 | Thread::yield(); // Allow other threads to run too |
danjulio | 0:83f4079b2bac | 742 | } |
danjulio | 0:83f4079b2bac | 743 | } |
danjulio | 0:83f4079b2bac | 744 | } |
danjulio | 0:83f4079b2bac | 745 | |
danjulio | 0:83f4079b2bac | 746 | |
danjulio | 0:83f4079b2bac | 747 | void FileReadProcess() |
danjulio | 0:83f4079b2bac | 748 | { |
danjulio | 0:83f4079b2bac | 749 | char fullReadFileName[MAX_RECORD_FN_LEN + 5]; |
danjulio | 0:83f4079b2bac | 750 | char curLine[MAX_CMD_RSP_LEN + 1]; |
danjulio | 0:83f4079b2bac | 751 | char c; |
danjulio | 0:83f4079b2bac | 752 | int i; |
danjulio | 0:83f4079b2bac | 753 | |
danjulio | 0:83f4079b2bac | 754 | while (1) { |
danjulio | 0:83f4079b2bac | 755 | // Wait for a trigger to start dumping a file |
danjulio | 0:83f4079b2bac | 756 | while (!trigFileDump) { |
danjulio | 0:83f4079b2bac | 757 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 758 | } |
danjulio | 0:83f4079b2bac | 759 | trigFileDump = false; |
danjulio | 0:83f4079b2bac | 760 | |
danjulio | 0:83f4079b2bac | 761 | // Attempt to open the file for reading |
danjulio | 0:83f4079b2bac | 762 | strcpy(fullReadFileName, "/sd/"); |
danjulio | 0:83f4079b2bac | 763 | strcpy(&fullReadFileName[4], curReadFileName); |
danjulio | 0:83f4079b2bac | 764 | fp = fopen(fullReadFileName, "r"); |
danjulio | 0:83f4079b2bac | 765 | if (fp == 0) { |
danjulio | 0:83f4079b2bac | 766 | sprintf(curLine, "Cannot open %s: errno %d", fullReadFileName, errno); |
danjulio | 0:83f4079b2bac | 767 | rspFifo.PushString(curLine); |
danjulio | 0:83f4079b2bac | 768 | } else { |
danjulio | 0:83f4079b2bac | 769 | // Dump file one line at a time, assuming Unix <LF> line endings |
danjulio | 0:83f4079b2bac | 770 | // but skipping <CR> if they occur. They can stop us by clearing |
danjulio | 0:83f4079b2bac | 771 | // enFileDump (although we may have buffered a lot of data in |
danjulio | 0:83f4079b2bac | 772 | // rspFifo). |
danjulio | 0:83f4079b2bac | 773 | i = 0; |
danjulio | 0:83f4079b2bac | 774 | while (!feof(fp) && enFileDump) { |
danjulio | 0:83f4079b2bac | 775 | c = getc(fp); |
danjulio | 0:83f4079b2bac | 776 | if (c == '\n') { |
danjulio | 0:83f4079b2bac | 777 | // End of line |
danjulio | 0:83f4079b2bac | 778 | curLine[i] = 0; |
danjulio | 0:83f4079b2bac | 779 | i = 0; |
danjulio | 0:83f4079b2bac | 780 | rspFifo.PushString(curLine); |
danjulio | 0:83f4079b2bac | 781 | Thread::yield(); // Allow other activity while dumping |
danjulio | 0:83f4079b2bac | 782 | } else if (c != '\r') { |
danjulio | 0:83f4079b2bac | 783 | if (i < MAX_CMD_RSP_LEN) { |
danjulio | 0:83f4079b2bac | 784 | curLine[i++] = c; |
danjulio | 0:83f4079b2bac | 785 | } |
danjulio | 0:83f4079b2bac | 786 | } |
danjulio | 0:83f4079b2bac | 787 | } |
danjulio | 0:83f4079b2bac | 788 | if (i != 0) { |
danjulio | 0:83f4079b2bac | 789 | // Push any final, unterminated lines |
danjulio | 0:83f4079b2bac | 790 | rspFifo.PushString(curLine); |
danjulio | 0:83f4079b2bac | 791 | } |
danjulio | 0:83f4079b2bac | 792 | rspFifo.PushString("END"); |
danjulio | 0:83f4079b2bac | 793 | fclose(fp); |
danjulio | 0:83f4079b2bac | 794 | } |
danjulio | 0:83f4079b2bac | 795 | enFileDump = false; |
danjulio | 0:83f4079b2bac | 796 | } |
danjulio | 0:83f4079b2bac | 797 | } |
danjulio | 0:83f4079b2bac | 798 | |
danjulio | 0:83f4079b2bac | 799 | |
danjulio | 0:83f4079b2bac | 800 | |
danjulio | 0:83f4079b2bac | 801 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 802 | // Command handlers |
danjulio | 0:83f4079b2bac | 803 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 804 | |
danjulio | 0:83f4079b2bac | 805 | void statusCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 806 | { |
danjulio | 0:83f4079b2bac | 807 | gpsMutex.lock(); |
danjulio | 0:83f4079b2bac | 808 | sprintf(result, "STATUS,%d,%1.2f,%1.2f,%d,%1.6f,%c,%1.6f,%c,%1.1f", |
danjulio | 0:83f4079b2bac | 809 | enRecording, curBattV, curTempC, gpsFix, gpsLat, gpsLatOrient, gpsLon, |
danjulio | 0:83f4079b2bac | 810 | gpsLonOrient, gpsAlt); |
danjulio | 0:83f4079b2bac | 811 | gpsMutex.unlock(); |
danjulio | 0:83f4079b2bac | 812 | AddChecksum(result); |
danjulio | 0:83f4079b2bac | 813 | } |
danjulio | 0:83f4079b2bac | 814 | |
danjulio | 0:83f4079b2bac | 815 | |
danjulio | 0:83f4079b2bac | 816 | void fileCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 817 | { |
danjulio | 0:83f4079b2bac | 818 | if (argc >= 2) { |
danjulio | 0:83f4079b2bac | 819 | strcpy(curRecFileName, argv[1]); |
danjulio | 0:83f4079b2bac | 820 | sprintf(result, "Filename = %s", curRecFileName); |
danjulio | 0:83f4079b2bac | 821 | } else { |
danjulio | 0:83f4079b2bac | 822 | sprintf(result, "Must also specify filename"); |
danjulio | 0:83f4079b2bac | 823 | } |
danjulio | 0:83f4079b2bac | 824 | } |
danjulio | 0:83f4079b2bac | 825 | |
danjulio | 0:83f4079b2bac | 826 | |
danjulio | 0:83f4079b2bac | 827 | void dumpCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 828 | { |
danjulio | 0:83f4079b2bac | 829 | if (enRecording == true) { |
danjulio | 0:83f4079b2bac | 830 | sprintf(result, "Cannot dump while recording!"); |
danjulio | 0:83f4079b2bac | 831 | } else { |
danjulio | 0:83f4079b2bac | 832 | if (!enFileDump) { |
danjulio | 0:83f4079b2bac | 833 | if (argc >= 2) { |
danjulio | 0:83f4079b2bac | 834 | // They provided a specific file to read |
danjulio | 0:83f4079b2bac | 835 | strcpy(curReadFileName, argv[1]); |
danjulio | 0:83f4079b2bac | 836 | } else { |
danjulio | 0:83f4079b2bac | 837 | // Use the existing record file name |
danjulio | 0:83f4079b2bac | 838 | strcpy(curReadFileName, curRecFileName); |
danjulio | 0:83f4079b2bac | 839 | } |
danjulio | 0:83f4079b2bac | 840 | enFileDump = true; // Allow sensors to start filling sensorFifo |
danjulio | 0:83f4079b2bac | 841 | trigFileDump = true; |
danjulio | 0:83f4079b2bac | 842 | } |
danjulio | 0:83f4079b2bac | 843 | sprintf(result, ""); |
danjulio | 0:83f4079b2bac | 844 | } |
danjulio | 0:83f4079b2bac | 845 | } |
danjulio | 0:83f4079b2bac | 846 | |
danjulio | 0:83f4079b2bac | 847 | |
danjulio | 0:83f4079b2bac | 848 | void durCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 849 | { |
danjulio | 0:83f4079b2bac | 850 | float f; |
danjulio | 0:83f4079b2bac | 851 | |
danjulio | 0:83f4079b2bac | 852 | if (argc >= 2) { |
danjulio | 0:83f4079b2bac | 853 | if ((f = atof(argv[1])) != 0.0f) { |
danjulio | 0:83f4079b2bac | 854 | recTimeoutSecs = f; |
danjulio | 0:83f4079b2bac | 855 | } |
danjulio | 0:83f4079b2bac | 856 | sprintf(result, "Recording timeout = %f seconds", recTimeoutSecs); |
danjulio | 0:83f4079b2bac | 857 | } else { |
danjulio | 0:83f4079b2bac | 858 | sprintf(result, "Must also specify timeout"); |
danjulio | 0:83f4079b2bac | 859 | } |
danjulio | 0:83f4079b2bac | 860 | } |
danjulio | 0:83f4079b2bac | 861 | |
danjulio | 0:83f4079b2bac | 862 | |
danjulio | 0:83f4079b2bac | 863 | void massStorageCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 864 | { |
danjulio | 0:83f4079b2bac | 865 | if (argc >= 2) { |
danjulio | 0:83f4079b2bac | 866 | if (strcmp(argv[1], "on") == 0) { |
danjulio | 0:83f4079b2bac | 867 | if (!enMassStorage) { |
danjulio | 0:83f4079b2bac | 868 | if (!usbPower) { |
danjulio | 0:83f4079b2bac | 869 | sprintf(result, "Cannot enable USB Mass storage with no USB"); |
danjulio | 0:83f4079b2bac | 870 | } else { |
danjulio | 0:83f4079b2bac | 871 | if (ms_sd.connect(true)) { |
danjulio | 0:83f4079b2bac | 872 | enMassStorage = true; |
danjulio | 0:83f4079b2bac | 873 | sprintf(result, "Mass Storage Enabled"); |
danjulio | 0:83f4079b2bac | 874 | SetStatusLed(ST_MSD); |
danjulio | 0:83f4079b2bac | 875 | } else { |
danjulio | 0:83f4079b2bac | 876 | sprintf(result, "Mass Storage Not Enabled!"); |
danjulio | 0:83f4079b2bac | 877 | } |
danjulio | 0:83f4079b2bac | 878 | } |
danjulio | 0:83f4079b2bac | 879 | } else { |
danjulio | 0:83f4079b2bac | 880 | sprintf(result, "Mass Storage already Enabled!"); |
danjulio | 0:83f4079b2bac | 881 | } |
danjulio | 0:83f4079b2bac | 882 | } else { |
danjulio | 0:83f4079b2bac | 883 | if (enMassStorage) { |
danjulio | 0:83f4079b2bac | 884 | ms_sd.disconnect(); |
danjulio | 0:83f4079b2bac | 885 | enMassStorage = false; |
danjulio | 0:83f4079b2bac | 886 | ClearStatusLed(ST_MSD); |
danjulio | 0:83f4079b2bac | 887 | sprintf(result, "Mass Storage Disabled"); |
danjulio | 0:83f4079b2bac | 888 | } else { |
danjulio | 0:83f4079b2bac | 889 | sprintf(result, "Mass Storage already Disabled"); |
danjulio | 0:83f4079b2bac | 890 | } |
danjulio | 0:83f4079b2bac | 891 | } |
danjulio | 0:83f4079b2bac | 892 | } else { |
danjulio | 0:83f4079b2bac | 893 | sprintf(result, "Must also specify on/off"); |
danjulio | 0:83f4079b2bac | 894 | } |
danjulio | 0:83f4079b2bac | 895 | } |
danjulio | 0:83f4079b2bac | 896 | |
danjulio | 0:83f4079b2bac | 897 | |
danjulio | 0:83f4079b2bac | 898 | void sendCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 899 | { |
danjulio | 0:83f4079b2bac | 900 | bool en; |
danjulio | 0:83f4079b2bac | 901 | bool valid = true; |
danjulio | 0:83f4079b2bac | 902 | |
danjulio | 0:83f4079b2bac | 903 | if (argc >= 3) { |
danjulio | 0:83f4079b2bac | 904 | if (strcmp(argv[2], "on") == 0) { |
danjulio | 0:83f4079b2bac | 905 | en = true; |
danjulio | 0:83f4079b2bac | 906 | } else { |
danjulio | 0:83f4079b2bac | 907 | en = false; |
danjulio | 0:83f4079b2bac | 908 | } |
danjulio | 0:83f4079b2bac | 909 | |
danjulio | 0:83f4079b2bac | 910 | if (strcmp(argv[1], "accel") == 0) { |
danjulio | 0:83f4079b2bac | 911 | enSnsLogging[ACCEL] = en; |
danjulio | 0:83f4079b2bac | 912 | } else if (strcmp(argv[1], "batt") == 0) { |
danjulio | 0:83f4079b2bac | 913 | enSnsLogging[BATT] = en; |
danjulio | 0:83f4079b2bac | 914 | } else if (strcmp(argv[1], "gps") == 0) { |
danjulio | 0:83f4079b2bac | 915 | enSnsLogging[GPS] = en; |
danjulio | 0:83f4079b2bac | 916 | } else if (strcmp(argv[1], "light") == 0) { |
danjulio | 0:83f4079b2bac | 917 | enSnsLogging[LIGHT] = en; |
danjulio | 0:83f4079b2bac | 918 | } else if (strcmp(argv[1], "temp") == 0) { |
danjulio | 0:83f4079b2bac | 919 | enSnsLogging[TEMP] = en; |
danjulio | 0:83f4079b2bac | 920 | } else { |
danjulio | 0:83f4079b2bac | 921 | valid = false; |
danjulio | 0:83f4079b2bac | 922 | sprintf(result, "Unknown sensor: %s", argv[1]); |
danjulio | 0:83f4079b2bac | 923 | } |
danjulio | 0:83f4079b2bac | 924 | |
danjulio | 0:83f4079b2bac | 925 | if (valid) { |
danjulio | 0:83f4079b2bac | 926 | sprintf(result, "%s Sending = %d", argv[1], en); |
danjulio | 0:83f4079b2bac | 927 | } |
danjulio | 0:83f4079b2bac | 928 | } else { |
danjulio | 0:83f4079b2bac | 929 | sprintf(result, "Must specify sensor and on/off"); |
danjulio | 0:83f4079b2bac | 930 | } |
danjulio | 0:83f4079b2bac | 931 | } |
danjulio | 0:83f4079b2bac | 932 | |
danjulio | 0:83f4079b2bac | 933 | |
danjulio | 0:83f4079b2bac | 934 | void recordCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 935 | { |
danjulio | 0:83f4079b2bac | 936 | if (enFileDump) { |
danjulio | 0:83f4079b2bac | 937 | sprintf(result, ""); // Silently ignore command while dumping |
danjulio | 0:83f4079b2bac | 938 | } else if (argc >= 2) { |
danjulio | 0:83f4079b2bac | 939 | if (strcmp(argv[1], "on") == 0) { |
danjulio | 0:83f4079b2bac | 940 | if (!enRecording) { |
danjulio | 0:83f4079b2bac | 941 | enRecording = true; // Allow sensors to start feeding writer |
danjulio | 0:83f4079b2bac | 942 | trigRecording = true; |
danjulio | 0:83f4079b2bac | 943 | sprintf(result, "Setup to record to %s", curRecFileName); |
danjulio | 0:83f4079b2bac | 944 | } else { |
danjulio | 0:83f4079b2bac | 945 | sprintf(result, "Already recording"); |
danjulio | 0:83f4079b2bac | 946 | } |
danjulio | 0:83f4079b2bac | 947 | } else { |
danjulio | 0:83f4079b2bac | 948 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 949 | trigEndRecording = true; |
danjulio | 0:83f4079b2bac | 950 | sprintf(result, "Stopping recording"); |
danjulio | 0:83f4079b2bac | 951 | } else { |
danjulio | 0:83f4079b2bac | 952 | sprintf(result, "Recording already stopped"); |
danjulio | 0:83f4079b2bac | 953 | } |
danjulio | 0:83f4079b2bac | 954 | } |
danjulio | 0:83f4079b2bac | 955 | } else { |
danjulio | 0:83f4079b2bac | 956 | sprintf(result, "Must also specify on/off"); |
danjulio | 0:83f4079b2bac | 957 | } |
danjulio | 0:83f4079b2bac | 958 | } |
danjulio | 0:83f4079b2bac | 959 | |
danjulio | 0:83f4079b2bac | 960 | |
danjulio | 0:83f4079b2bac | 961 | void shutdownCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 962 | { |
danjulio | 0:83f4079b2bac | 963 | // Schedule shutdown |
danjulio | 0:83f4079b2bac | 964 | mQueue.call(ShutdownEvent); |
danjulio | 0:83f4079b2bac | 965 | |
danjulio | 0:83f4079b2bac | 966 | sprintf(result, "Shutdown"); |
danjulio | 0:83f4079b2bac | 967 | } |
danjulio | 0:83f4079b2bac | 968 | |
danjulio | 0:83f4079b2bac | 969 | |
danjulio | 0:83f4079b2bac | 970 | void helpCommand(unsigned int argc, char* argv[], char* result) |
danjulio | 0:83f4079b2bac | 971 | { |
danjulio | 0:83f4079b2bac | 972 | // Empty command response |
danjulio | 0:83f4079b2bac | 973 | sprintf(result, ""); |
danjulio | 0:83f4079b2bac | 974 | |
danjulio | 0:83f4079b2bac | 975 | rspFifo.PushString("Commands: "); |
danjulio | 0:83f4079b2bac | 976 | rspFifo.PushString(" status"); |
danjulio | 0:83f4079b2bac | 977 | rspFifo.PushString(" file <filename>"); |
danjulio | 0:83f4079b2bac | 978 | rspFifo.PushString(" dump [<filename>"); |
danjulio | 0:83f4079b2bac | 979 | rspFifo.PushString(" dur <seconds>"); |
danjulio | 0:83f4079b2bac | 980 | rspFifo.PushString(" msd [on/off]"); |
danjulio | 0:83f4079b2bac | 981 | rspFifo.PushString(" send [accel | batt | gps | light | temp] [on/off]"); |
danjulio | 0:83f4079b2bac | 982 | rspFifo.PushString(" record [on/off]"); |
danjulio | 0:83f4079b2bac | 983 | rspFifo.PushString(" shutdown"); |
danjulio | 0:83f4079b2bac | 984 | rspFifo.PushString(" help"); |
danjulio | 0:83f4079b2bac | 985 | } |
danjulio | 0:83f4079b2bac | 986 | |
danjulio | 0:83f4079b2bac | 987 | |
danjulio | 0:83f4079b2bac | 988 | |
danjulio | 0:83f4079b2bac | 989 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 990 | // Event Handlers |
danjulio | 0:83f4079b2bac | 991 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 992 | |
danjulio | 0:83f4079b2bac | 993 | /* |
danjulio | 0:83f4079b2bac | 994 | // NOTE: This function not used because attempting to catch daplink RX interrupts |
danjulio | 0:83f4079b2bac | 995 | // hangs the system --> So I'm falling back to polling in another thread... |
danjulio | 0:83f4079b2bac | 996 | void DaplinkRxIsr() |
danjulio | 0:83f4079b2bac | 997 | { |
danjulio | 0:83f4079b2bac | 998 | static int rxIndex = 0; |
danjulio | 0:83f4079b2bac | 999 | bool sawTerm = false; |
danjulio | 0:83f4079b2bac | 1000 | char c; |
danjulio | 0:83f4079b2bac | 1001 | |
danjulio | 0:83f4079b2bac | 1002 | // Process all incoming received data and see if we can process a command |
danjulio | 0:83f4079b2bac | 1003 | while (daplink.readable() && !sawTerm) { |
danjulio | 0:83f4079b2bac | 1004 | c = daplink.getc(); |
danjulio | 0:83f4079b2bac | 1005 | if (c == '\n') { |
danjulio | 0:83f4079b2bac | 1006 | cmdString[rxIndex] = 0; |
danjulio | 0:83f4079b2bac | 1007 | sawTerm = true; |
danjulio | 0:83f4079b2bac | 1008 | rxIndex = 0; |
danjulio | 0:83f4079b2bac | 1009 | } else if (rxIndex < MAX_CMD_LEN) { |
danjulio | 0:83f4079b2bac | 1010 | cmdString[rxIndex++] = c; |
danjulio | 0:83f4079b2bac | 1011 | } |
danjulio | 0:83f4079b2bac | 1012 | } |
danjulio | 0:83f4079b2bac | 1013 | |
danjulio | 0:83f4079b2bac | 1014 | if (sawTerm) { |
danjulio | 0:83f4079b2bac | 1015 | cmdFromRadio = false; |
danjulio | 0:83f4079b2bac | 1016 | mQueue.call(CommandEvent); |
danjulio | 0:83f4079b2bac | 1017 | } |
danjulio | 0:83f4079b2bac | 1018 | return; |
danjulio | 0:83f4079b2bac | 1019 | } |
danjulio | 0:83f4079b2bac | 1020 | */ |
danjulio | 0:83f4079b2bac | 1021 | |
danjulio | 0:83f4079b2bac | 1022 | |
danjulio | 0:83f4079b2bac | 1023 | void RadioIsrEvent() |
danjulio | 0:83f4079b2bac | 1024 | { |
danjulio | 0:83f4079b2bac | 1025 | uint8_t rxLen; |
danjulio | 0:83f4079b2bac | 1026 | |
danjulio | 0:83f4079b2bac | 1027 | // Process the Radio's Interrupt Handler - we do it in an event instead |
danjulio | 0:83f4079b2bac | 1028 | // of an ISR because it may access the swspi and the current mbed |
danjulio | 0:83f4079b2bac | 1029 | // environment won't correctly execute the mutex's protecting the swspi |
danjulio | 0:83f4079b2bac | 1030 | // access routines (which are shared with code in other threads) in an ISR. |
danjulio | 0:83f4079b2bac | 1031 | radio.handleInterrupt(); |
danjulio | 0:83f4079b2bac | 1032 | |
danjulio | 0:83f4079b2bac | 1033 | // See if we received a packet and process it if necessary |
danjulio | 0:83f4079b2bac | 1034 | if (radio.available()) { |
danjulio | 0:83f4079b2bac | 1035 | rxLen = MAX_CMD_LEN; |
danjulio | 0:83f4079b2bac | 1036 | if (radio.recv((uint8_t *) cmdString, &rxLen)) { |
danjulio | 0:83f4079b2bac | 1037 | cmdString[rxLen+1] = 0; |
danjulio | 0:83f4079b2bac | 1038 | cmdFromRadio = true; |
danjulio | 0:83f4079b2bac | 1039 | mQueue.call(CommandEvent); |
danjulio | 0:83f4079b2bac | 1040 | } |
danjulio | 0:83f4079b2bac | 1041 | } |
danjulio | 0:83f4079b2bac | 1042 | } |
danjulio | 0:83f4079b2bac | 1043 | |
danjulio | 0:83f4079b2bac | 1044 | |
danjulio | 0:83f4079b2bac | 1045 | void CommandEvent() |
danjulio | 0:83f4079b2bac | 1046 | { |
danjulio | 0:83f4079b2bac | 1047 | char rspString[MAX_CMD_RSP_LEN+1]; |
danjulio | 0:83f4079b2bac | 1048 | |
danjulio | 0:83f4079b2bac | 1049 | // Process the received command |
danjulio | 0:83f4079b2bac | 1050 | if (cmdDispatcher.executeCommand(cmdString, rspString)) { |
danjulio | 0:83f4079b2bac | 1051 | // Push response |
danjulio | 0:83f4079b2bac | 1052 | rspFifo.PushString(rspString); |
danjulio | 0:83f4079b2bac | 1053 | } |
danjulio | 0:83f4079b2bac | 1054 | } |
danjulio | 0:83f4079b2bac | 1055 | |
danjulio | 0:83f4079b2bac | 1056 | |
danjulio | 0:83f4079b2bac | 1057 | void RecordEndEvent() |
danjulio | 0:83f4079b2bac | 1058 | { |
danjulio | 0:83f4079b2bac | 1059 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 1060 | trigEndRecording = true; |
danjulio | 0:83f4079b2bac | 1061 | rspFifo.PushString("Finishing Recording"); |
danjulio | 0:83f4079b2bac | 1062 | } |
danjulio | 0:83f4079b2bac | 1063 | } |
danjulio | 0:83f4079b2bac | 1064 | |
danjulio | 0:83f4079b2bac | 1065 | |
danjulio | 0:83f4079b2bac | 1066 | void ShutdownEvent() |
danjulio | 0:83f4079b2bac | 1067 | { |
danjulio | 0:83f4079b2bac | 1068 | // Stop recording if necessary |
danjulio | 0:83f4079b2bac | 1069 | if (enRecording) { |
danjulio | 0:83f4079b2bac | 1070 | trigEndRecording = true; |
danjulio | 0:83f4079b2bac | 1071 | } |
danjulio | 0:83f4079b2bac | 1072 | |
danjulio | 0:83f4079b2bac | 1073 | // Wait a moment to let any other threads clean up |
danjulio | 0:83f4079b2bac | 1074 | wait_ms(100); |
danjulio | 0:83f4079b2bac | 1075 | |
danjulio | 0:83f4079b2bac | 1076 | // Power down |
danjulio | 0:83f4079b2bac | 1077 | pegasus.max14690.shutdown(); |
danjulio | 0:83f4079b2bac | 1078 | } |
danjulio | 0:83f4079b2bac | 1079 | |
danjulio | 0:83f4079b2bac | 1080 | |
danjulio | 0:83f4079b2bac | 1081 | void AccelTick() |
danjulio | 0:83f4079b2bac | 1082 | { |
danjulio | 0:83f4079b2bac | 1083 | enSnsTick[ACCEL] = true; |
danjulio | 0:83f4079b2bac | 1084 | } |
danjulio | 0:83f4079b2bac | 1085 | |
danjulio | 0:83f4079b2bac | 1086 | void BattTick() |
danjulio | 0:83f4079b2bac | 1087 | { |
danjulio | 0:83f4079b2bac | 1088 | enSnsTick[BATT] = true; |
danjulio | 0:83f4079b2bac | 1089 | } |
danjulio | 0:83f4079b2bac | 1090 | |
danjulio | 0:83f4079b2bac | 1091 | void GpsTick() |
danjulio | 0:83f4079b2bac | 1092 | { |
danjulio | 0:83f4079b2bac | 1093 | enSnsTick[GPS] = true; |
danjulio | 0:83f4079b2bac | 1094 | } |
danjulio | 0:83f4079b2bac | 1095 | |
danjulio | 0:83f4079b2bac | 1096 | void LightTick() |
danjulio | 0:83f4079b2bac | 1097 | { |
danjulio | 0:83f4079b2bac | 1098 | enSnsTick[LIGHT] = true; |
danjulio | 0:83f4079b2bac | 1099 | } |
danjulio | 0:83f4079b2bac | 1100 | |
danjulio | 0:83f4079b2bac | 1101 | void TempTick() |
danjulio | 0:83f4079b2bac | 1102 | { |
danjulio | 0:83f4079b2bac | 1103 | enSnsTick[TEMP] = true; |
danjulio | 0:83f4079b2bac | 1104 | } |
danjulio | 0:83f4079b2bac | 1105 | |
danjulio | 0:83f4079b2bac | 1106 | |
danjulio | 0:83f4079b2bac | 1107 | |
danjulio | 0:83f4079b2bac | 1108 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 1109 | // Support subroutines |
danjulio | 0:83f4079b2bac | 1110 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 1111 | |
danjulio | 0:83f4079b2bac | 1112 | void InitSystem() |
danjulio | 0:83f4079b2bac | 1113 | { |
danjulio | 0:83f4079b2bac | 1114 | // Indicate startup |
danjulio | 0:83f4079b2bac | 1115 | SetStatusLed(ST_STARTUP); |
danjulio | 0:83f4079b2bac | 1116 | |
danjulio | 0:83f4079b2bac | 1117 | // Misc global object initialization |
danjulio | 0:83f4079b2bac | 1118 | daplink.baud(115200); |
danjulio | 0:83f4079b2bac | 1119 | sw_spi.init(); |
danjulio | 0:83f4079b2bac | 1120 | i2cBus.frequency(400000); |
danjulio | 0:83f4079b2bac | 1121 | |
danjulio | 0:83f4079b2bac | 1122 | // Setup the Radio |
danjulio | 0:83f4079b2bac | 1123 | if (!InitRadio()) { |
danjulio | 0:83f4079b2bac | 1124 | daplink.printf("Radio Init failed\n\r"); |
danjulio | 0:83f4079b2bac | 1125 | SetStatusLed(ST_ERROR); |
danjulio | 0:83f4079b2bac | 1126 | } |
danjulio | 0:83f4079b2bac | 1127 | |
danjulio | 0:83f4079b2bac | 1128 | // Misc global variable initialization |
danjulio | 0:83f4079b2bac | 1129 | strcpy(curRecFileName, DEF_RECORD_FILE); |
danjulio | 0:83f4079b2bac | 1130 | for (int i=0; i<5; i++) { |
danjulio | 0:83f4079b2bac | 1131 | enSnsLogging[i] = false; |
danjulio | 0:83f4079b2bac | 1132 | enSnsTick[i] = false; |
danjulio | 0:83f4079b2bac | 1133 | } |
danjulio | 0:83f4079b2bac | 1134 | |
danjulio | 0:83f4079b2bac | 1135 | // Commands |
danjulio | 0:83f4079b2bac | 1136 | cmdDispatcher.addCommand("status", statusCommand); |
danjulio | 0:83f4079b2bac | 1137 | cmdDispatcher.addCommand("file", fileCommand); |
danjulio | 0:83f4079b2bac | 1138 | cmdDispatcher.addCommand("dump", dumpCommand); |
danjulio | 0:83f4079b2bac | 1139 | cmdDispatcher.addCommand("dur", durCommand); |
danjulio | 0:83f4079b2bac | 1140 | cmdDispatcher.addCommand("msd", massStorageCommand); |
danjulio | 0:83f4079b2bac | 1141 | cmdDispatcher.addCommand("send", sendCommand); |
danjulio | 0:83f4079b2bac | 1142 | cmdDispatcher.addCommand("record", recordCommand); |
danjulio | 0:83f4079b2bac | 1143 | cmdDispatcher.addCommand("shutdown", shutdownCommand); |
danjulio | 0:83f4079b2bac | 1144 | cmdDispatcher.addCommand("help", helpCommand); |
danjulio | 0:83f4079b2bac | 1145 | |
danjulio | 0:83f4079b2bac | 1146 | // Setup events |
danjulio | 0:83f4079b2bac | 1147 | //daplink.attach(&DaplinkRxIsr, Serial::RxIrq); // NOTE: This hangs the system! |
danjulio | 0:83f4079b2bac | 1148 | radioInt.rise(mQueue.event(RadioIsrEvent)); |
danjulio | 0:83f4079b2bac | 1149 | |
danjulio | 0:83f4079b2bac | 1150 | // Start threads |
danjulio | 0:83f4079b2bac | 1151 | daplinkRxThread.start(callback(daplinkRxProcess)); |
danjulio | 0:83f4079b2bac | 1152 | accelThread.start(callback(AccelProcess)); |
danjulio | 0:83f4079b2bac | 1153 | battThread.start(callback(BattProcess)); |
danjulio | 0:83f4079b2bac | 1154 | gpsRxThread.start(callback(GpsRxProcess)); |
danjulio | 0:83f4079b2bac | 1155 | gpsLogThread.start(callback(GpsLogProcess)); |
danjulio | 0:83f4079b2bac | 1156 | lightThread.start(callback(LightProcess)); |
danjulio | 0:83f4079b2bac | 1157 | tempThread.start(callback(TempProcess)); |
danjulio | 0:83f4079b2bac | 1158 | fileWriteThread.start(callback(FileWriteProcess)); |
danjulio | 0:83f4079b2bac | 1159 | fileReadThread.start(callback(FileReadProcess)); |
danjulio | 0:83f4079b2bac | 1160 | mQueueThread.start(callback(&mQueue, &EventQueue::dispatch_forever)); |
danjulio | 0:83f4079b2bac | 1161 | |
danjulio | 0:83f4079b2bac | 1162 | // Start sensor trigger timers (this sets the frequency for each sensor's data |
danjulio | 0:83f4079b2bac | 1163 | // collection) |
danjulio | 0:83f4079b2bac | 1164 | accelTicker.attach(&AccelTick, 0.01f); |
danjulio | 0:83f4079b2bac | 1165 | battTicker.attach(&BattTick, 5.0f); |
danjulio | 0:83f4079b2bac | 1166 | gpsTicker.attach(&GpsTick, 1.0f); |
danjulio | 0:83f4079b2bac | 1167 | lightTicker.attach(&LightTick, 0.01f); |
danjulio | 0:83f4079b2bac | 1168 | tempTicker.attach(&TempTick, 0.1f); |
danjulio | 0:83f4079b2bac | 1169 | } |
danjulio | 0:83f4079b2bac | 1170 | |
danjulio | 0:83f4079b2bac | 1171 | |
danjulio | 0:83f4079b2bac | 1172 | bool InitRadio() |
danjulio | 0:83f4079b2bac | 1173 | { |
danjulio | 0:83f4079b2bac | 1174 | // Reset radio |
danjulio | 0:83f4079b2bac | 1175 | radioRstN = 0; |
danjulio | 0:83f4079b2bac | 1176 | wait_ms(10); |
danjulio | 0:83f4079b2bac | 1177 | radioRstN = 1; |
danjulio | 0:83f4079b2bac | 1178 | |
danjulio | 0:83f4079b2bac | 1179 | // Attempt to initialize it |
danjulio | 0:83f4079b2bac | 1180 | if (!radio.init()) { |
danjulio | 0:83f4079b2bac | 1181 | return(false); |
danjulio | 0:83f4079b2bac | 1182 | } |
danjulio | 0:83f4079b2bac | 1183 | |
danjulio | 0:83f4079b2bac | 1184 | // Configure |
danjulio | 0:83f4079b2bac | 1185 | radio.setFrequency(915.0); |
danjulio | 0:83f4079b2bac | 1186 | radio.setTxPower(20); |
danjulio | 0:83f4079b2bac | 1187 | |
danjulio | 0:83f4079b2bac | 1188 | return(true); |
danjulio | 0:83f4079b2bac | 1189 | } |
danjulio | 0:83f4079b2bac | 1190 | |
danjulio | 0:83f4079b2bac | 1191 | |
danjulio | 0:83f4079b2bac | 1192 | void SetStatusLed(uint8_t f) |
danjulio | 0:83f4079b2bac | 1193 | { |
danjulio | 0:83f4079b2bac | 1194 | ledStatus |= f; |
danjulio | 0:83f4079b2bac | 1195 | |
danjulio | 0:83f4079b2bac | 1196 | UpdateStatusLed(); |
danjulio | 0:83f4079b2bac | 1197 | } |
danjulio | 0:83f4079b2bac | 1198 | |
danjulio | 0:83f4079b2bac | 1199 | |
danjulio | 0:83f4079b2bac | 1200 | void ClearStatusLed(uint8_t f) |
danjulio | 0:83f4079b2bac | 1201 | { |
danjulio | 0:83f4079b2bac | 1202 | ledStatus &= ~f; |
danjulio | 0:83f4079b2bac | 1203 | |
danjulio | 0:83f4079b2bac | 1204 | UpdateStatusLed(); |
danjulio | 0:83f4079b2bac | 1205 | } |
danjulio | 0:83f4079b2bac | 1206 | |
danjulio | 0:83f4079b2bac | 1207 | |
danjulio | 0:83f4079b2bac | 1208 | void UpdateStatusLed() |
danjulio | 0:83f4079b2bac | 1209 | { |
danjulio | 0:83f4079b2bac | 1210 | LedColors c; |
danjulio | 0:83f4079b2bac | 1211 | |
danjulio | 0:83f4079b2bac | 1212 | // Priority Encoder |
danjulio | 0:83f4079b2bac | 1213 | if (ledStatus & ST_ERROR) c = RED; |
danjulio | 0:83f4079b2bac | 1214 | else if (ledStatus & ST_MSD) c = BLUE; |
danjulio | 0:83f4079b2bac | 1215 | else if (ledStatus & ST_RECORD) c = MAGENTA; |
danjulio | 0:83f4079b2bac | 1216 | else if (ledStatus & ST_USB_PWR) c = GREEN; |
danjulio | 0:83f4079b2bac | 1217 | else if (ledStatus & ST_GPS_FIX) c = CYAN; |
danjulio | 0:83f4079b2bac | 1218 | else if (ledStatus & ST_STARTUP) c = YELLOW; |
danjulio | 0:83f4079b2bac | 1219 | else c = BLACK; |
danjulio | 0:83f4079b2bac | 1220 | |
danjulio | 0:83f4079b2bac | 1221 | // Hardware control |
danjulio | 0:83f4079b2bac | 1222 | switch (c) { |
danjulio | 0:83f4079b2bac | 1223 | case BLACK: |
danjulio | 0:83f4079b2bac | 1224 | rLED = 1; gLED = 1; bLED = 1; |
danjulio | 0:83f4079b2bac | 1225 | break; |
danjulio | 0:83f4079b2bac | 1226 | case RED: |
danjulio | 0:83f4079b2bac | 1227 | rLED = 0; gLED = 1; bLED = 1; |
danjulio | 0:83f4079b2bac | 1228 | break; |
danjulio | 0:83f4079b2bac | 1229 | case YELLOW: |
danjulio | 0:83f4079b2bac | 1230 | rLED = 0; gLED = 0; bLED = 1; |
danjulio | 0:83f4079b2bac | 1231 | break; |
danjulio | 0:83f4079b2bac | 1232 | case GREEN: |
danjulio | 0:83f4079b2bac | 1233 | rLED = 1; gLED = 0; bLED = 1; |
danjulio | 0:83f4079b2bac | 1234 | break; |
danjulio | 0:83f4079b2bac | 1235 | case CYAN: |
danjulio | 0:83f4079b2bac | 1236 | rLED = 1; gLED = 0; bLED = 0; |
danjulio | 0:83f4079b2bac | 1237 | break; |
danjulio | 0:83f4079b2bac | 1238 | case BLUE: |
danjulio | 0:83f4079b2bac | 1239 | rLED = 1; gLED = 1; bLED = 0; |
danjulio | 0:83f4079b2bac | 1240 | break; |
danjulio | 0:83f4079b2bac | 1241 | case MAGENTA: |
danjulio | 0:83f4079b2bac | 1242 | rLED = 0; gLED = 1; bLED = 0; |
danjulio | 0:83f4079b2bac | 1243 | break; |
danjulio | 0:83f4079b2bac | 1244 | case WHITE: |
danjulio | 0:83f4079b2bac | 1245 | rLED = 0; gLED = 0; bLED = 0; |
danjulio | 0:83f4079b2bac | 1246 | break; |
danjulio | 0:83f4079b2bac | 1247 | } |
danjulio | 0:83f4079b2bac | 1248 | } |
danjulio | 0:83f4079b2bac | 1249 | |
danjulio | 0:83f4079b2bac | 1250 | |
danjulio | 0:83f4079b2bac | 1251 | void Signon() |
danjulio | 0:83f4079b2bac | 1252 | { |
danjulio | 0:83f4079b2bac | 1253 | char msg[20]; |
danjulio | 0:83f4079b2bac | 1254 | |
danjulio | 0:83f4079b2bac | 1255 | sprintf(msg, "HELLO,%d.%d", VER_MAJOR, VER_MINOR); |
danjulio | 0:83f4079b2bac | 1256 | AddChecksum(msg); |
danjulio | 0:83f4079b2bac | 1257 | daplink.printf("%s\n\r", msg); |
danjulio | 0:83f4079b2bac | 1258 | radio.send((uint8_t *) msg, sizeof(msg)); |
danjulio | 0:83f4079b2bac | 1259 | } |
danjulio | 0:83f4079b2bac | 1260 | |
danjulio | 0:83f4079b2bac | 1261 | |
danjulio | 0:83f4079b2bac | 1262 | float GpsNMEAtoFrac(float c) |
danjulio | 0:83f4079b2bac | 1263 | { |
danjulio | 0:83f4079b2bac | 1264 | float intPart; |
danjulio | 0:83f4079b2bac | 1265 | float fracPart; |
danjulio | 0:83f4079b2bac | 1266 | |
danjulio | 0:83f4079b2bac | 1267 | // Get the integer part of the coordinate |
danjulio | 0:83f4079b2bac | 1268 | intPart = floor(c / 100.0f); |
danjulio | 0:83f4079b2bac | 1269 | |
danjulio | 0:83f4079b2bac | 1270 | // Convert the fractional part into 60ths |
danjulio | 0:83f4079b2bac | 1271 | fracPart = c - (intPart * 100.0f); // Get rid of the integer part |
danjulio | 0:83f4079b2bac | 1272 | fracPart = fracPart / 60.0f; |
danjulio | 0:83f4079b2bac | 1273 | |
danjulio | 0:83f4079b2bac | 1274 | return(intPart + fracPart); |
danjulio | 0:83f4079b2bac | 1275 | } |
danjulio | 0:83f4079b2bac | 1276 | |
danjulio | 0:83f4079b2bac | 1277 | |
danjulio | 0:83f4079b2bac | 1278 | void AddChecksum(char* s) |
danjulio | 0:83f4079b2bac | 1279 | { |
danjulio | 0:83f4079b2bac | 1280 | uint8_t c = 0; |
danjulio | 0:83f4079b2bac | 1281 | int i = 0; |
danjulio | 0:83f4079b2bac | 1282 | |
danjulio | 0:83f4079b2bac | 1283 | while (*(s+i) != 0) { |
danjulio | 0:83f4079b2bac | 1284 | c ^= *(s+i++); |
danjulio | 0:83f4079b2bac | 1285 | } |
danjulio | 0:83f4079b2bac | 1286 | |
danjulio | 0:83f4079b2bac | 1287 | sprintf(s+i, ",*%X", c); |
danjulio | 0:83f4079b2bac | 1288 | } |
danjulio | 0:83f4079b2bac | 1289 | |
danjulio | 0:83f4079b2bac | 1290 | |
danjulio | 0:83f4079b2bac | 1291 | |
danjulio | 0:83f4079b2bac | 1292 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 1293 | // Main |
danjulio | 0:83f4079b2bac | 1294 | // ============================================================================= |
danjulio | 0:83f4079b2bac | 1295 | int main() |
danjulio | 0:83f4079b2bac | 1296 | { |
danjulio | 0:83f4079b2bac | 1297 | char outString[MAX_CMD_RSP_LEN+1]; |
danjulio | 0:83f4079b2bac | 1298 | |
danjulio | 0:83f4079b2bac | 1299 | InitSystem(); |
danjulio | 0:83f4079b2bac | 1300 | Signon(); |
danjulio | 0:83f4079b2bac | 1301 | |
danjulio | 0:83f4079b2bac | 1302 | while (1) { |
danjulio | 0:83f4079b2bac | 1303 | // Look for data to pass back to our user |
danjulio | 0:83f4079b2bac | 1304 | rspFifo.PopString(outString); |
danjulio | 0:83f4079b2bac | 1305 | |
danjulio | 0:83f4079b2bac | 1306 | if (cmdFromRadio) { |
danjulio | 0:83f4079b2bac | 1307 | radio.send((uint8_t *) outString, strlen(outString)); |
danjulio | 0:83f4079b2bac | 1308 | } else { |
danjulio | 0:83f4079b2bac | 1309 | while (!daplink.writeable()) { |
danjulio | 0:83f4079b2bac | 1310 | Thread::yield(); |
danjulio | 0:83f4079b2bac | 1311 | } |
danjulio | 0:83f4079b2bac | 1312 | daplink.printf("%s\n", outString); |
danjulio | 0:83f4079b2bac | 1313 | } |
danjulio | 0:83f4079b2bac | 1314 | } |
danjulio | 0:83f4079b2bac | 1315 | } |