Dan Julio
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
Diff: main.cpp
- Revision:
- 0:83f4079b2bac
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jun 11 04:26:22 2017 +0000
@@ -0,0 +1,1315 @@
+/*
+ * 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, ®Val) == 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);
+ }
+ }
+}