File content as of revision 0:83f4079b2bac:

/*
 * Model Rocket Data Acquisition and Telemetry system using the MAX32630FTHR.
 *  - Acceleration using on-board BMI160.
 *  - Rotation using ALS-PT19-315C Ambient Light Sensor with analog input
 *  - Temperature using PT-100 RTD and MAX31865 Amplifier/Digitizer
 *  - GPS location and altitude using external serial receiver.  Receiver
 *    includes local battery for faster acquisition.
 *  - Battery voltage using on-board MAX14690 PMIC.
 *  - Radio communication for telemetry and command using a RFM95W LoRa module.
 *
 * Data is stored as text sentences to a file on a on-board Micro-SD Card.  Data
 * storage is initiated via a user command prior to launch and terminated after
 * a configurable delay set to bound the flight-time.  Highly dynamic information
 * such as acceleration and temperature is stored at 100 samples/second.
 *
 * The Micro-SD Card may be made available as a USB Storage Device for data off-
 * load.  Data is formatted as follows.
 *
 *  File Records - one per line - terminated with CTRL-N
 *   ACC,<BMI160_SECS>,<X>,<Y>,<Z>,*<CHECKSUM>
 *   TEMP,<SEQ NUM>,<TEMP>,*<CHECKSUM>
 *   BATT,<SEQ NUM>,<VOLTS>,*<CHECKSUM>
 *   LIGHT,<SEQ NUM>,<ADC VOLTS>,*<CHECKSUM>
 *   GPS,<FIX>,<LATITUDE>,<LAT ORIENTATION>,<LONGITUDE>,<LON ORIENTATION>,
 *     <ALTITUDE>,*<CHECKSUM>
 *
 * <CHECKSUM> is a 2-ASCII HEX value XOR of all bytes from the first character
 * up to but not including the ',*' delimiter.
 *
 * GPS location (and/or other sensor information) may also be transmitted
 * periodically via a LoRa radio to help with rocket retrieval.
 *
 * A simple command shell is available via the LoRa radio and diagnostic serial
 * port supporting the following commands.  Commands are terminated with a
 * Newline character.  Command output is echoed to the same interface that it
 * was received from (either LoRa or diagnostic serial).  Only one interface
 * should ever be used at a time.
 *
 *   "status" : Returns device status including recording state, battery voltage,
 *       temperature and current GPS coordinates.
 *   "file <filename>" : Set the name, <filename>, for the next recording session.
 *       The default filename is "flight.txt".
 *   "dump [<filename>" : Output the contents of the current or specified 
 *       recording file.
 *   "dur <seconds>" : Set the duration of the recording session in seconds.
 *       The default value is 120 seconds.
 *   "msd [on/off]" : Enable or disable USB Mass Storage.  USB must be detected
 *       connected.
 *   "send [accel | batt | gps | light | temp] [on/off]" : Enable or disable 
 *       periodic sensor logging via the current output interface.
 *   "record [on/off]" : Enable or disable a recording session.  Default is off.
 *   "shutdown" : Shut down power (via the PMIC).  The MAX32630FTHR will
 *       automatically repower immediately if USB Power is connected.
 *   "help" : Output this list of commands.
 *
 * Status response takes the form
 *   STATUS,<RECORDING>,<BATT VOLTS>,<TEMP>,<FIX>,<LATITUDE>,<LAT ORIENTATION>,
 *      <LONGITUDE>,<LON ORIENTATION>,<ALTITUDE>,*<CHECKSUM>
 *
 * A sign-on message is generated on both daplink and LoRa interfaces containing
 * the firmware revision.
 *   HELLO,<VERSION_MAJOR.VERSION_MINOR>,*<CHECKSUM>
 *
 * A typical flight scenerio might be:
 *   Preflight
 *     1. Power-on system
 *     2. Prepare rocket for launch
 *     3. Use Status command to verify rocket has GPS acquisition
 *     4. Configure flight recording filename
 *     5. Enable GPS logging
 *     6. Enable recording
 *     7. Launch rocket
 *   During flight
 *     1. Record rough position using telemetried GPS location
 *   Postflight
 *     1. Use telemetried GPS location to plot rocket's landing position
 *     2. Use either command or direct USB location to obtain flight data from
 *        recording file.
 *     3. Power down system
 *
 * System Architecture
 *   1. Individual threads for sensor data aquisition.
 *   2. Main thread handling ougoing communication.
 *   3. Event thread handling command processing.
 *   4. File writing and reading threads for data to/from Micro-SD Card.
 *   5. String FIFOs used for one-way data communication between sensor 
 *      acquisition threads and logging thread as well as between command
 *      response, file dump, GPS acquisition thread and communication thread.
 *   6. Atomically accessible variables used for current battery voltage,
 *      current temperature and recording state.
 *   7. USBMSD object for handling Micro-SD as USB Mass Storage Device.
 *
 * Various libraries have been used or ported (thank you to the authors of those
 * libraries).  All application code is contained in this file even though it
 * should be distributed in a proper system.  Note that the mbed USBMSD_SD 
 * library was modified NOT to attempt to "connect" during the object constructor
 * since this blocks until USB is connected.  Connection is initiated only by
 * command and only if USB power is detected.
 *
 * Communication Interfaces
 *   1. USB UART Used for diagnostic interface (daplink).
 *   2. UART2 used for GPS interface.
 *   3. I2CM2 used for BMI160 and MAX14690.
 *   4. Software (bit-banged) SPI used for LoRa radio and MAX31865.  This is
 *      because the built-in hardware SPI seems to require use of hardware
 *      slave-select signals and only one of those is available if I2CM2 is
 *      also being used.
 *   5. AIN0 connected to MAX14690 MON out (battery voltage).
 *   6. AIN1 connected to ALS-PT19-315C output (light sensor).
 *   7. Internal 1.2V ADC ref.
 *   8. GPIO inputs for MAX31865 ready and LoRa interrupt.
 *   9. GPIO outputs for LoRa reset, SPI slave select, MAX31865 SPI slave select.
 *  10. On-board RGB LED for system operation state indication.
 *  11. GPIO output (P4_0) used for profiling during development.
 *
 * RGB Status LED (lowest to highest priority)
 *  YELLOW  : System Idle, No GPS Fix
 *  BLUE    : System Idle, GPS Fix obtained
 *  GREEN   : System Idle, USB Detected attached
 *  BLUE    : USB Mass Storage Enabled (USB must be attached)
 *  MAGENTA : System Recording
 *  RED     : System Failure (device initialization or file open failed)
 *
 *
 * Copyright (c) 2017 by Dan Julio.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL DAN JULIO BE LIABLE FOR ANY CLAIM, DAMAGES
 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <math.h>
#include "mbed.h"
#include "max32630fthr.h"
#include "bmi160.h"
#include "CommandDispatcher.h"
#include "gps.h"
#include "Adafruit_MAX31865.h"
#include "RH_RF95.h"
#include "SDFileSystem.h"
#include "StringFifo.h"
#include "swspi.h"
#include "USBMSD_SD.h"


//
// Version
//
#define VER_MAJOR 0
#define VER_MINOR 8


// =============================================================================
// Configuration Constants
// =============================================================================

//
// Length (in characters) of the FIFO used to store sensor data sentences to be
// logged in the Micro-SD card.  This should be long enough to prevent any
// sensor pushing data into the FIFO from hanging until there is room and ideally
// determined by analyzing the time required to log data, the maximum flight time
// and computing the maximum data rate.
//
#define LOG_FIFO_LENGTH 16384

//
// Length (in characters) for the FIFO used to store GPS data sentences, command
// responses and file dump sentences to be transmitted via the LoRa radio (which
// is slow when working in the most reliable configuration) or the diagnostic
// serial interface.
//
#define RSP_FIFO_LENGTH 16384

//
// Time (in seconds) for default recording session.
//
#define DEF_RECORD_SECONDS 120

//
// Default recording file name and maximum length
//
#define DEF_RECORD_FILE "flight.txt"
#define MAX_RECORD_FN_LEN 32

//
// Command and sentence limits (number of chars/line)
//
#define MAX_CMD_LEN      64
#define MAX_CMD_RSP_LEN  128
#define MAX_SENTENCE_LEN 80

//
// Low-battery auto-shutdown voltage.
//
#define SHUTDOWN_VOLTS 3.0f

//
// ADC Vref volts (this is fixed for MAX32630)
//
#define VREF 1.2f

//
// Status LED set/clear masks (fixed)
//
#define ST_STARTUP  0x01
#define ST_GPS_FIX  0x02
#define ST_USB_PWR  0x04
#define ST_RECORD   0x10
#define ST_MSD      0x20
#define ST_ERROR    0x80



// =============================================================================
// Objects
// =============================================================================

//
// Hardware-related objects
//
MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);

Serial daplink(P2_1, P2_0);

Serial gps_serial(P3_1, P3_0);
Adafruit_GPS gps(&gps_serial);

I2C i2cBus(P5_7, P6_0);
BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);

swspi sw_spi(P5_1, P5_2, P5_0, P3_4, P5_3);   // mosi, miso, sclk, ss1, ss2
Adafruit_MAX31865 tempSense(sw_spi, 1, P3_5); // spi, ss<N>, rdyPin
RH_RF95 radio(sw_spi, 2);                     // spi, ss<N>
InterruptIn radioInt(P5_4);
DigitalOut radioRstN(P5_5);

AnalogIn battSensor(AIN_0);
AnalogIn lightSensor(AIN_1);

SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd");  // mosi, miso, sclk, cs
USBMSD_SD ms_sd(P0_5, P0_6, P0_4, P0_7);        // mosi, miso, sclk, cs

DigitalOut rLED(LED1);
DigitalOut gLED(LED2);
DigitalOut bLED(LED3);

DigitalOut profOut(P4_0);


//
// Data objects
//
StringFifo sensorFifo(LOG_FIFO_LENGTH);
StringFifo rspFifo(RSP_FIFO_LENGTH);

CommandDispatcher cmdDispatcher;

EventQueue mQueue(16*EVENTS_EVENT_SIZE);


//
// Threads
//
Thread mQueueThread;
Thread daplinkRxThread;
Thread accelThread;
Thread battThread;
Thread gpsRxThread;
Thread gpsLogThread;
Thread lightThread;
Thread tempThread;
Thread fileWriteThread;
Thread fileReadThread;


//
// Sensor trigger timers
//
Ticker accelTicker;
Ticker battTicker;
Ticker gpsTicker;
Ticker lightTicker;
Ticker tempTicker;



// =============================================================================
// Global Variables
// =============================================================================

//
// RGB Status LED colors
//
enum LedColors
{
    BLACK,
    RED,
    YELLOW,
    GREEN,
    CYAN,
    BLUE,
    MAGENTA,
    WHITE
};


//
// Indicies into sensor-related control arrays
//
enum SensorSelect
{
    ACCEL = 0,
    BATT,
    GPS,
    LIGHT,
    TEMP
};
    

//
// System state
//
bool enSnsLogging[5];          // Set to enable transmitting various sensor values
bool enSnsTick[5];             // Set when time for a sensor to log or record info

bool trigRecording = false;    // {command} --> {file writer}
bool trigEndRecording = false; // {command, timer} --> {file writer}
bool enRecording = false;      // Set by command to note recording, cleared by writer
char curRecFileName[MAX_RECORD_FN_LEN + 1];
float recTimeoutSecs = DEF_RECORD_SECONDS;
FILE* fp;                      // Use for recording or dumping file

bool trigFileDump = false;     // {command} --> {file reader}
bool enFileDump = false;       // Set by command to note dumping, cleared by reader
char curReadFileName[MAX_RECORD_FN_LEN + 1];

bool enMassStorage = false;    // Set by command when mass storage is enabled

//
// Command and output processing
//
volatile bool cmdFromRadio = true;
char cmdString[MAX_CMD_LEN+1];

//
// System status
//
float curBattV = 0.0f;
float curTempC = 0.0f;
Mutex gpsMutex;
float gpsLat = 0.0f;
char gpsLatOrient = ' ';
float gpsLon = 0.0f;
char gpsLonOrient = ' ';
float gpsAlt = 0.0f;
bool gpsUpdated = false;          // {GpsRx} -> {GpsLog}
bool gpsFix = false;

bool usbPower = false;            // Set while the battery thread detects ext pwr

uint8_t ledStatus = 0x00;


// =============================================================================
// Necessary forward declarations
// =============================================================================

void CommandEvent();
void RecordEndEvent();
void ShutdownEvent();
bool InitRadio();
void SetStatusLed(uint8_t f);
void ClearStatusLed(uint8_t f);
void UpdateStatusLed();
float GpsNMEAtoFrac(float c);
void AddChecksum(char* s);



// =============================================================================
// Thread code
// =============================================================================

void daplinkRxProcess()
{
    int rxIndex = 0;
    char c;
    
    while (1) {
        // Wait for and then process incoming data
        while (!daplink.readable()) {
            Thread::yield();
        }
        
        // Process a character
        c = daplink.getc();
        if (c == '\n') {
            cmdString[rxIndex] = 0;
            rxIndex = 0;
            cmdFromRadio = false;
            mQueue.call(CommandEvent);
        } else if ((c != '\r') && (rxIndex < MAX_CMD_LEN)) {
            cmdString[rxIndex++] = c;
        }
    }
}


void AccelProcess()
{
    BMI160::AccConfig accConfig;
    BMI160::SensorData accData;
    BMI160::SensorTime sensorTime;
    char accelString[MAX_SENTENCE_LEN];
    uint32_t seqNum = 0;
    bool initSuccess = true;   // Will be set false if necessary
    
    // Setup accelerometer configuration
    accConfig.range = BMI160::SENS_16G;
    accConfig.us = BMI160::ACC_US_OFF;
    accConfig.bwp = BMI160::ACC_BWP_2;
    accConfig.odr = BMI160::ACC_ODR_8;
    
    // Initialize
    if (imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        daplink.printf("Failed to set gyro power mode\n\r");
        initSuccess = false;
    }
    wait_ms(100);
    if (imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        daplink.printf("Failed to set accelerometer power mode\n\r");
        initSuccess = false;
    }
    wait_ms(100);
    if (initSuccess){
        if (imu.setSensorConfig(accConfig) != BMI160::RTN_NO_ERROR) {
            daplink.printf("Failed to reconfigure accelerometer\n\r");
            initSuccess = false;
        }
    }
    
    if (!initSuccess) {
        SetStatusLed(ST_ERROR);
    } else {
        // Main loop
        while (1) {
            // Wait to be triggered
            while (!enSnsTick[ACCEL]) {
                Thread::yield();
            }
            enSnsTick[ACCEL] = false;
            
            // Read accelerometer
            (void) imu.getSensorXYZandSensorTime(accData, sensorTime, accConfig.range);
            
            // Send for logging
            sprintf(accelString, "ACC,%f,%f,%f,%f", sensorTime.seconds,
                    accData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled);
            AddChecksum(accelString);
            
            if (enRecording) {
                sensorFifo.PushString(accelString);
            }
        
            if ((enSnsLogging[ACCEL] && (seqNum++ % 100) == 0)) {
                // Only send to our user occasionally
                rspFifo.PushString(accelString);
            }
        }
    }
}


void BattProcess()
{
    char battString[MAX_SENTENCE_LEN];
    uint32_t seqNum = 0;
    char regVal;
    
    // Initialize - setup the monitor input for 1:4 scaling to fit our
    // ADC's fullscale range
    pegasus.max14690.monSet(pegasus.max14690.MON_BAT, pegasus.max14690.MON_DIV4);
    
    while (1) {
        // Wait to be triggered
        while (!enSnsTick[BATT]) {
            Thread::yield();
        }
        enSnsTick[BATT] = false;
        
        // Read battery voltage and check for shutdown condition
        curBattV = battSensor.read()*4.0f*VREF;
        
        if (curBattV < SHUTDOWN_VOLTS) {
            // Attempt to tell the world we're shutting down
            rspFifo.PushString("Low Battery Shutdown");
            
            // Schedule shutdown
            mQueue.call(ShutdownEvent);
        }
        
        // Send for logging
        sprintf(battString, "BATT,%d,%1.2f", seqNum++, curBattV);
        AddChecksum(battString);
        
        
        if (enRecording) {
            sensorFifo.PushString(battString);
        }
        
        if (enSnsLogging[BATT]) {
            rspFifo.PushString(battString);
        }
        
        // Read the MAX14690 StatusB register bit[3] UsbOk to determine
        // if we are connected to a USB system or not
        if (pegasus.max14690.readReg(MAX14690::REG_STATUS_B, &regVal) == MAX14690_NO_ERROR) {
            if (regVal & 0x08) {
                if (!usbPower) {
                    usbPower = true;
                    SetStatusLed(ST_USB_PWR);
                }
            } else {
                if (usbPower) {
                    usbPower = false;
                    ClearStatusLed(ST_USB_PWR);
                }
            }
        }
    }
}


void GpsRxProcess()
{
    // Initialize
    gps.begin(9600);
    
    while (1) {
        // Read complete lines from the GPS receiver (blocks until data avail)
        gps.readNMEA();
        
        // Process any received NMEA sentences
        if (gps.newNMEAreceived()) {
            if (gps.parse(gps.lastNMEA())) {
                if (gps.fix) {
                    // Update our status
                    gpsMutex.lock();
                    gpsLat = GpsNMEAtoFrac(gps.latitude);
                    gpsLatOrient = gps.lat;
                    gpsLon = GpsNMEAtoFrac(gps.longitude);
                    gpsLonOrient = gps.lon;
                    gpsAlt = gps.altitude;
                    gpsUpdated = true;
                    gpsMutex.unlock();
                }
            }
        }
    }
}


void GpsLogProcess()
{
    char gpsString[MAX_SENTENCE_LEN];
    int gpsMissedUpdateCount = 0;
    
    while (1) {
        // Wait to be triggered
        while (!enSnsTick[GPS]) {
            Thread::yield();
        }
        enSnsTick[GPS] = false;
        
        // Evaluate fix detecton logic
        if (gpsUpdated) {
            gpsUpdated = false;
            gpsMissedUpdateCount = 0;
            if (!gpsFix) {
                gpsFix = true;
                SetStatusLed(ST_GPS_FIX);
            }
        } else {
            if (++gpsMissedUpdateCount > 5) {
                if (gpsFix) {
                    gpsFix = false;
                    ClearStatusLed(ST_GPS_FIX);
                }
            }
        }
        
        // Send for logging
        gpsMutex.lock();
        sprintf(gpsString, "GPS,%d,%1.6f,%c,%1.6f,%c,%1.1f",
                gpsFix, gpsLat, gpsLatOrient, gpsLon, gpsLonOrient, gps.altitude);
        gpsMutex.unlock();
        AddChecksum(gpsString);
        
        if (enRecording) {
            sensorFifo.PushString(gpsString);
        }
                    
        if (enSnsLogging[GPS]) {
            rspFifo.PushString(gpsString);
        }
    }
}


void LightProcess()
{
    char lightString[MAX_SENTENCE_LEN];
    uint32_t seqNum = 0;
    float lightV;
    
    while (1) {
        // Wait to be triggered
        while (!enSnsTick[LIGHT]) {
            Thread::yield();
        }
        enSnsTick[LIGHT] = false;
        
        // Read light sensor voltage
        lightV = lightSensor.read()*VREF;
        
        // Send for logging
        sprintf(lightString, "LIGHT,%d,%1.2f", seqNum++, lightV);
        AddChecksum(lightString);
        
        if (enRecording) {
            sensorFifo.PushString(lightString);
        }
        
        if ((enSnsLogging[LIGHT] && (seqNum % 100) == 0)) {
            // Only send to our user occasionally
            rspFifo.PushString(lightString);
        }
    }
}


void TempProcess()
{
    char tempString[MAX_SENTENCE_LEN];
    uint32_t seqNum = 0;
    uint16_t rtd;
    
    // Initialize temperature sensor for continuous sensing
    tempSense.begin();
    tempSense.enableBias(true);
    tempSense.autoConvert(true);
    
    while (1) {
        // Wait to be triggered
        while (!enSnsTick[TEMP]) {
            Thread::yield();
        }
        enSnsTick[TEMP] = false;
        
        // Get the temperature if available
        if (tempSense.isReady(&rtd)) {
            // Using 100 ohm RTD and 430 ohm reference resistor
            curTempC = tempSense.temperature(100.0f, 430.0f, rtd);
            
            // Send for logging
            sprintf(tempString, "TEMP,%d,%1.2f", seqNum++, curTempC);
            AddChecksum(tempString);
        
            if (enRecording) {
                sensorFifo.PushString(tempString);
            }
        
            if ((enSnsLogging[TEMP] && (seqNum % 10) == 0)) {
                // Only send to our user occasionally
                rspFifo.PushString(tempString);
            }
        }
    }
}


void FileWriteProcess()
{
    char fullRecFileName[MAX_RECORD_FN_LEN + 5];
    char logString[MAX_SENTENCE_LEN];
    
    while (1) {
        // Each time through before possibly blocking on the fifo, check 
        // if we need to close the file after recording finishes
        if (trigEndRecording && enRecording) {
            enRecording = false;
            trigEndRecording = false;
            fclose(fp);
            ClearStatusLed(ST_RECORD);
        }
        
        // Look for sensor data
        sensorFifo.PopString(logString);
        
        // See if we need to start recording
        if (trigRecording) {
            trigRecording = false;
            // Attempt to open the file for writing
            strcpy(fullRecFileName, "/sd/");
            strcpy(&fullRecFileName[4], curRecFileName);
            fp = fopen(fullRecFileName, "w");
            if (fp == 0) {
                enRecording = false;
                sprintf(logString, "Cannot open %s, errno = %d", fullRecFileName, errno);
                rspFifo.PushString(logString);
                SetStatusLed(ST_ERROR);
            } else {
                mQueue.call_in(recTimeoutSecs*1000, RecordEndEvent);
                ClearStatusLed(ST_ERROR);  // Just in case they screwed up the filename earlier
                SetStatusLed(ST_RECORD);
            }
        }
        
        // Write to file if we are logging (Unix style)
        if (enRecording) {
            profOut = 1;
            fprintf(fp, "%s\n", logString);
            profOut = 0;
            Thread::yield();  // Allow other threads to run too
        }
    }
}


void FileReadProcess()
{
    char fullReadFileName[MAX_RECORD_FN_LEN + 5];
    char curLine[MAX_CMD_RSP_LEN + 1];
    char c;
    int i;
    
    while (1) {
        // Wait for a trigger to start dumping a file
        while (!trigFileDump) { 
            Thread::yield();
        }
        trigFileDump = false;
        
        // Attempt to open the file for reading
        strcpy(fullReadFileName, "/sd/");
        strcpy(&fullReadFileName[4], curReadFileName);
        fp = fopen(fullReadFileName, "r");
        if (fp == 0) {
            sprintf(curLine, "Cannot open %s: errno %d", fullReadFileName, errno);
            rspFifo.PushString(curLine);
        } else {
            // Dump file one line at a time, assuming Unix <LF> line endings
            // but skipping <CR> if they occur.  They can stop us by clearing
            // enFileDump (although we may have buffered a lot of data in
            // rspFifo).
            i = 0;
            while (!feof(fp) && enFileDump) {
                c = getc(fp);
                if (c == '\n') {
                    // End of line
                    curLine[i] = 0;
                    i = 0;
                    rspFifo.PushString(curLine);
                    Thread::yield();  // Allow other activity while dumping
                } else if (c != '\r') {
                    if (i < MAX_CMD_RSP_LEN) {
                        curLine[i++] = c;
                    }
                }
            }
            if (i != 0) {
                // Push any final, unterminated lines
                rspFifo.PushString(curLine);
            }
            rspFifo.PushString("END");
            fclose(fp);
        }
        enFileDump = false;
    }
}



// =============================================================================
// Command handlers
// =============================================================================

void statusCommand(unsigned int argc, char* argv[], char* result)
{
    gpsMutex.lock();
    sprintf(result, "STATUS,%d,%1.2f,%1.2f,%d,%1.6f,%c,%1.6f,%c,%1.1f",
        enRecording, curBattV, curTempC, gpsFix, gpsLat, gpsLatOrient, gpsLon,
        gpsLonOrient, gpsAlt);
    gpsMutex.unlock();
    AddChecksum(result);
}


void fileCommand(unsigned int argc, char* argv[], char* result)
{
    if (argc >= 2) {
        strcpy(curRecFileName, argv[1]);
        sprintf(result, "Filename = %s", curRecFileName);
    } else {
        sprintf(result, "Must also specify filename");
    }    
}


void dumpCommand(unsigned int argc, char* argv[], char* result)
{
    if (enRecording == true) {
        sprintf(result, "Cannot dump while recording!");
    } else {
        if (!enFileDump) {
            if (argc >= 2) {
                // They provided a specific file to read
                strcpy(curReadFileName, argv[1]);
            } else {
                // Use the existing record file name
                strcpy(curReadFileName, curRecFileName);
            }
            enFileDump = true;  // Allow sensors to start filling sensorFifo
            trigFileDump = true;
        }
        sprintf(result, "");
    }
}


void durCommand(unsigned int argc, char* argv[], char* result)
{
    float f;
    
    if (argc >= 2) {
        if ((f = atof(argv[1])) != 0.0f) {
            recTimeoutSecs = f;
        }
        sprintf(result, "Recording timeout = %f seconds", recTimeoutSecs);
    } else {
        sprintf(result, "Must also specify timeout");
    }
}


void massStorageCommand(unsigned int argc, char* argv[], char* result)
{
    if (argc >= 2) {
        if (strcmp(argv[1], "on") == 0) {
            if (!enMassStorage) {
                if (!usbPower) {
                    sprintf(result, "Cannot enable USB Mass storage with no USB");
                } else {
                    if (ms_sd.connect(true)) {
                        enMassStorage = true;
                        sprintf(result, "Mass Storage Enabled");
                        SetStatusLed(ST_MSD);
                    } else {
                        sprintf(result, "Mass Storage Not Enabled!");
                    }
                }
            } else {
                sprintf(result, "Mass Storage already Enabled!");
            }
        } else {
            if (enMassStorage) {
                ms_sd.disconnect();
                enMassStorage = false;
                ClearStatusLed(ST_MSD);
                sprintf(result, "Mass Storage Disabled");
            } else {
                sprintf(result, "Mass Storage already Disabled");
            }
        }
    } else {
        sprintf(result, "Must also specify on/off");
    }
}


void sendCommand(unsigned int argc, char* argv[], char* result)
{
    bool en;
    bool valid = true;
    
    if (argc >= 3) {
        if (strcmp(argv[2], "on") == 0) {
            en = true;
        } else {
            en = false;
        }

        if (strcmp(argv[1], "accel") == 0) {
            enSnsLogging[ACCEL] = en;
        } else if (strcmp(argv[1], "batt") == 0) {
            enSnsLogging[BATT] = en;
        } else if (strcmp(argv[1], "gps") == 0) {
            enSnsLogging[GPS] = en;
        } else if (strcmp(argv[1], "light") == 0) {
            enSnsLogging[LIGHT] = en;
        } else if (strcmp(argv[1], "temp") == 0) {
            enSnsLogging[TEMP] = en;
        } else {
            valid = false;
            sprintf(result, "Unknown sensor: %s", argv[1]);
        }
        
        if (valid) {
            sprintf(result, "%s Sending = %d", argv[1], en);
        }
    } else {
        sprintf(result, "Must specify sensor and on/off");
    }
}


void recordCommand(unsigned int argc, char* argv[], char* result)
{
    if (enFileDump) {
        sprintf(result, ""); // Silently ignore command while dumping
    } else if (argc >= 2) {
        if (strcmp(argv[1], "on") == 0) {
            if (!enRecording) {
                enRecording = true;   // Allow sensors to start feeding writer
                trigRecording = true;
                sprintf(result, "Setup to record to %s", curRecFileName);
            } else {
                sprintf(result, "Already recording");
            }
        } else {
            if (enRecording) {
                trigEndRecording = true;
                sprintf(result, "Stopping recording");
            } else {
                sprintf(result, "Recording already stopped");
            }
        }
    } else {
        sprintf(result, "Must also specify on/off");
    }
}


void shutdownCommand(unsigned int argc, char* argv[], char* result)
{
    // Schedule shutdown
    mQueue.call(ShutdownEvent);
    
    sprintf(result, "Shutdown");
}


void helpCommand(unsigned int argc, char* argv[], char* result)
{
    // Empty command response
    sprintf(result, "");
    
    rspFifo.PushString("Commands: ");
    rspFifo.PushString(" status");
    rspFifo.PushString(" file <filename>");
    rspFifo.PushString(" dump [<filename>");
    rspFifo.PushString(" dur <seconds>");
    rspFifo.PushString(" msd [on/off]");
    rspFifo.PushString(" send [accel | batt | gps | light | temp] [on/off]");
    rspFifo.PushString(" record [on/off]");
    rspFifo.PushString(" shutdown");
    rspFifo.PushString(" help");
}



// =============================================================================
// Event Handlers
// =============================================================================

/*
// NOTE: This function not used because attempting to catch daplink RX interrupts
// hangs the system --> So I'm falling back to polling in another thread...
void DaplinkRxIsr()
{
    static int rxIndex = 0;
    bool sawTerm = false;
    char c;
    
    // Process all incoming received data and see if we can process a command
    while (daplink.readable() && !sawTerm) {
        c = daplink.getc();
        if (c == '\n') {
            cmdString[rxIndex] = 0;
            sawTerm = true;
            rxIndex = 0;
        } else if (rxIndex < MAX_CMD_LEN) {
            cmdString[rxIndex++] = c;        
        }
    }
    
    if (sawTerm) {
        cmdFromRadio = false;
        mQueue.call(CommandEvent);
    }
    return;
}
*/


void RadioIsrEvent()
{
    uint8_t rxLen;

    // Process the Radio's Interrupt Handler - we do it in an event instead
    // of an ISR because it may access the swspi and the current mbed
    // environment won't correctly execute the mutex's protecting the swspi
    // access routines (which are shared with code in other threads) in an ISR.
    radio.handleInterrupt();
    
    // See if we received a packet and process it if necessary
    if (radio.available()) {
        rxLen = MAX_CMD_LEN;
        if (radio.recv((uint8_t *) cmdString, &rxLen)) {
            cmdString[rxLen+1] = 0;
            cmdFromRadio = true;
            mQueue.call(CommandEvent);
        }
    }
}


void CommandEvent()
{
    char rspString[MAX_CMD_RSP_LEN+1];
    
    // Process the received command
    if (cmdDispatcher.executeCommand(cmdString, rspString)) {
        // Push response
        rspFifo.PushString(rspString);
    }
}


void RecordEndEvent()
{
    if (enRecording) {
        trigEndRecording = true;
        rspFifo.PushString("Finishing Recording");
    }
}


void ShutdownEvent()
{
    // Stop recording if necessary
    if (enRecording) {
        trigEndRecording = true;
    }
    
    // Wait a moment to let any other threads clean up
    wait_ms(100);
    
    // Power down
    pegasus.max14690.shutdown();
}


void AccelTick()
{
    enSnsTick[ACCEL] = true;
}

void BattTick()
{
    enSnsTick[BATT] = true;
}

void GpsTick()
{
    enSnsTick[GPS] = true;
}

void LightTick()
{
    enSnsTick[LIGHT] = true;
}

void TempTick()
{
    enSnsTick[TEMP] = true;
}



// =============================================================================
// Support subroutines
// =============================================================================

void InitSystem()
{
    // Indicate startup
    SetStatusLed(ST_STARTUP);
    
    // Misc global object initialization
    daplink.baud(115200);
    sw_spi.init();
    i2cBus.frequency(400000);
    
    // Setup the Radio
    if (!InitRadio()) {
        daplink.printf("Radio Init failed\n\r");
        SetStatusLed(ST_ERROR);
    }
    
    // Misc global variable initialization
    strcpy(curRecFileName, DEF_RECORD_FILE);
    for (int i=0; i<5; i++) {
        enSnsLogging[i] = false;
        enSnsTick[i] = false;
    }
    
    // Commands
    cmdDispatcher.addCommand("status", statusCommand);
    cmdDispatcher.addCommand("file", fileCommand);
    cmdDispatcher.addCommand("dump", dumpCommand);
    cmdDispatcher.addCommand("dur", durCommand);
    cmdDispatcher.addCommand("msd", massStorageCommand);
    cmdDispatcher.addCommand("send", sendCommand);
    cmdDispatcher.addCommand("record", recordCommand);
    cmdDispatcher.addCommand("shutdown", shutdownCommand);
    cmdDispatcher.addCommand("help", helpCommand);
    
    // Setup events
    //daplink.attach(&DaplinkRxIsr, Serial::RxIrq);  // NOTE: This hangs the system!
    radioInt.rise(mQueue.event(RadioIsrEvent));
    
    // Start threads
    daplinkRxThread.start(callback(daplinkRxProcess));
    accelThread.start(callback(AccelProcess));
    battThread.start(callback(BattProcess));
    gpsRxThread.start(callback(GpsRxProcess));
    gpsLogThread.start(callback(GpsLogProcess));
    lightThread.start(callback(LightProcess));
    tempThread.start(callback(TempProcess));
    fileWriteThread.start(callback(FileWriteProcess));
    fileReadThread.start(callback(FileReadProcess));
    mQueueThread.start(callback(&mQueue, &EventQueue::dispatch_forever));

    // Start sensor trigger timers (this sets the frequency for each sensor's data
    // collection)
    accelTicker.attach(&AccelTick, 0.01f);
    battTicker.attach(&BattTick, 5.0f);
    gpsTicker.attach(&GpsTick, 1.0f);
    lightTicker.attach(&LightTick, 0.01f);
    tempTicker.attach(&TempTick, 0.1f);
}


bool InitRadio()
{
    // Reset radio
    radioRstN = 0;
    wait_ms(10);
    radioRstN = 1;
    
    // Attempt to initialize it
    if (!radio.init()) {
        return(false);
    }
    
    // Configure
    radio.setFrequency(915.0);
    radio.setTxPower(20);
    
    return(true);
}


void SetStatusLed(uint8_t f)
{
    ledStatus |= f;
    
    UpdateStatusLed();
}


void ClearStatusLed(uint8_t f)
{
    ledStatus &= ~f;
    
    UpdateStatusLed();
}


void UpdateStatusLed()
{
    LedColors c;
    
    // Priority Encoder
    if (ledStatus & ST_ERROR)        c = RED;
    else if (ledStatus & ST_MSD)     c = BLUE;
    else if (ledStatus & ST_RECORD)  c = MAGENTA;
    else if (ledStatus & ST_USB_PWR) c = GREEN;
    else if (ledStatus & ST_GPS_FIX) c = CYAN;
    else if (ledStatus & ST_STARTUP) c = YELLOW;
    else                             c = BLACK;
    
    // Hardware control
    switch (c) {
        case BLACK:
            rLED = 1; gLED = 1; bLED = 1;
            break;
        case RED:
            rLED = 0; gLED = 1; bLED = 1;
            break;
        case YELLOW:
            rLED = 0; gLED = 0; bLED = 1;
            break;
        case GREEN:
            rLED = 1; gLED = 0; bLED = 1;
            break;
        case CYAN:
            rLED = 1; gLED = 0; bLED = 0;
            break;
        case BLUE:
            rLED = 1; gLED = 1; bLED = 0;
            break;
        case MAGENTA:
            rLED = 0; gLED = 1; bLED = 0;
            break;
        case WHITE:
            rLED = 0; gLED = 0; bLED = 0;
            break;
    }
}


void Signon()
{
    char msg[20];
    
    sprintf(msg, "HELLO,%d.%d", VER_MAJOR, VER_MINOR);
    AddChecksum(msg);
    daplink.printf("%s\n\r", msg);
    radio.send((uint8_t *) msg, sizeof(msg));
}


float GpsNMEAtoFrac(float c)
{
    float intPart;
    float fracPart;
    
    // Get the integer part of the coordinate
    intPart = floor(c / 100.0f);
    
    // Convert the fractional part into 60ths
    fracPart = c - (intPart * 100.0f);   // Get rid of the integer part
    fracPart = fracPart / 60.0f;
    
    return(intPart + fracPart);
}


void AddChecksum(char* s)
{
    uint8_t c = 0;
    int i = 0;
    
    while (*(s+i) != 0) {
        c ^= *(s+i++);
    }
    
    sprintf(s+i, ",*%X", c);
}



// =============================================================================
// Main
// =============================================================================
int main()
{    
    char outString[MAX_CMD_RSP_LEN+1];
    
    InitSystem();
    Signon();
    
    while (1) {
        // Look for data to pass back to our user
        rspFifo.PopString(outString);

        if (cmdFromRadio) {
            radio.send((uint8_t *) outString, strlen(outString));
        } else {
            while (!daplink.writeable()) {
                Thread::yield();
            }
            daplink.printf("%s\n", outString);
        } 
    }    
}