Revision 66:0f20870117b7, committed 2018-06-19

Comitter:

tnhnrl

Date:

Tue Jun 19 20:14:23 2018 +0000

Parent:

65:2ac186553959

Child:

67:c86a4b464682

Commit message:

XBee testing file transmission good.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/IMU.cpp	Tue Jun 19 20:14:23 2018 +0000
@@ -0,0 +1,244 @@
+#include "IMU.h"
+
+IMU::IMU(PinName Tx, PinName Rx): 
+    _rs232(Tx,Rx)
+{
+}
+
+void IMU::initialize() {
+    //set up the spi bus and frequency
+    _rs232.baud(115200);
+
+    // initialize the processing state machine
+    state = SYNC0; 
+    
+    // initialize to zeros
+    euler[0] = 0.0;
+    euler[1] = 0.0;
+    euler[2] = 0.0;
+    
+    // initialize to zeros
+    latLonAlt[0] = 0.0;
+    latLonAlt[1] = 0.0;
+    latLonAlt[2] = 0.0;
+    
+    // initialize to no GNSS fix
+    is2dFixValid = false;
+    is3dFixValid = false;
+    numSV = 0;
+}
+
+// this stops an interval timer trigger of the IMU update function
+void IMU::start() {
+    interval.attach(this, &IMU::update, .05);  //this should be a 1 Hz sample rate
+}
+
+// this stops the interval timer trigger of the IMU update function
+void IMU::stop() {
+    interval.detach();
+}
+
+void IMU::runIMU() {
+    update();
+}
+
+// updated the imu update function with a state machine that doesn't hang if no data is present
+void IMU::update() {
+    
+//    // DEBUGGING an example packet
+//    Serial pc(USBTX, USBRX);
+//    //char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x04,0x3E,0x7A,0x63,0xA0,0xBB,0x8E,0x3B,0x29,0x7F,0xE5,0xBF,0x7F,0x84,0xEE}; // 3d accel
+//    char data[20] = {0x75,0x65,0x80,0x0E,0x0E,0x0C,0xBA,0xE3,0xED,0x9B,0x3C,0x7D,0x6D,0xDF,0xBF,0x85,0x5C,0xF5,0x41,0xBB}; // euler cf
+//    for (int j=0; j<20; j++) {
+        //byte = data[j];
+        
+    while (_rs232.readable()) {
+        // read a single byte
+        byte = _rs232.getc();
+        
+        // state machine to process byte-by-byte
+        switch (state) {
+        case SYNC0 : 
+            if (byte == 0x75) {
+                packet[0] = byte; // save into the packet
+                state = SYNC1;
+            }
+            break;
+            
+        case SYNC1 :
+            if (byte == 0x65) {
+                packet[1] = byte; // save into the packet
+                state = SET;
+            }
+            else {
+                state = SYNC0;
+            }
+            break;
+            
+        case SET :
+            descriptor = byte; // save descriptor set
+            packet[2] = byte; // save into the packet
+            state = LEN;
+            break;
+            
+        case LEN :
+            len = byte; // save payload length
+            packet[3] = byte; // save into the packet
+            state = PAY;
+            i = 0; // reset payload field length counter
+            break;
+            
+        case PAY :
+            if (i < len) { // keep adding until get all the payload length
+                packet[4+i] = byte; // add byte to the packet, skipping over the header 4-bytes
+                i++; // increment payload counter
+            }
+            else {
+                state = CRC0;
+            }
+            if (i >= len) { // not an elseif, since we want to escape when i==len
+                state = CRC0;
+            }
+            break;
+            
+        case CRC0 :
+            crc0 = byte; // save the msb of the checksum
+            state = CRC1;
+            break;
+            
+        case CRC1 :
+            crc1 = byte; // save the lsb of the checksum
+            checksum = ((unsigned int)crc0 << 8) + (unsigned int)crc1; // make checksum into a uint16
+            if (checksum == calcChecksum(packet, len+4)) { // passed checksum, wahoo!
+                processPayload(descriptor, packet[4], &packet[4]); // process the payload part of the packet, starting at byte 4
+            }
+            state = SYNC0; // reset to SYNC0 state
+            break;
+
+        default :
+            state = SYNC0;
+        }
+    }
+    return;
+}
+
+void IMU::processPayload(char descriptor, char length, unsigned char * payload) {
+    if (descriptor == IMU_DATA_SET) { // find an IMU data descriptor set
+        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
+            if (payload[1] == EULER_CF_DESCRIPTOR) { // find an euler CF field descriptor
+                processEulerCfPacket(length, payload);
+            }
+        }
+    }
+    else if (descriptor == GNSS_DATA_SET) { // find a GNSS data descriptor set
+        if (length > 2) { // make sure there are at least two bytes to see the field descriptor
+            if (payload[1] == LLH_POSITION_DESCRIPTOR) { // find a lat-lon-alt field descriptor
+                processLatLonAltPacket(length, payload);
+            }
+            else if (payload[1] == GNSS_FIX_INFO_DESCRIPTOR) { // find a gnss fix field descriptor
+                processGnssFixInformation(length, payload);
+            }
+        }
+    }
+}
+
+void IMU::processEulerCfPacket(char length, unsigned char * payload) {
+    if (length >= EULER_CF_LENGTH) { // make sure correct field length
+        if (payload[0] == EULER_CF_LENGTH) { // make sure field length is as expected
+            euler[0] = floatFromChar(&payload[ROLL_OFFSET+2])*180/_PI;  // roll Euler angle convert in degrees
+            euler[1] = floatFromChar(&payload[PITCH_OFFSET+2])*180/_PI; // pitch Euler angle convert in degrees
+            euler[2] = floatFromChar(&payload[YAW_OFFSET+2])*180/_PI;   // yaw Euler angle convert in degrees
+        }
+    }
+}
+
+void IMU::processLatLonAltPacket(char length, unsigned char * payload) {
+    if (length >= LLH_POSITION_LENGTH) { // make sure correct field length
+        if (payload[0] == LLH_POSITION_LENGTH) { // make sure field length is as expected
+            latLonAlt[0] = doubleFromChar(&payload[LATITUDE_OFFSET+2]);   // latitude in decimal degrees
+            latLonAlt[1] = doubleFromChar(&payload[LONGITUDE_OFFSET+2]);  // longitude in decimal degrees
+            latLonAlt[2] = doubleFromChar(&payload[HEIGHT_MSL_OFFSET+2]); // altitude above mean sea level in meters
+        }
+    }
+}
+
+void IMU::processGnssFixInformation(char length, unsigned char * payload) {
+    if (length >= GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
+        if (payload[0] == GNSS_FIX_INFO_LENGTH) { // make sure field length is as expected
+            is2dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x01; // bitand on LSB to see 2d fix flag
+            is3dFixValid = char(payload[FIX_TYPE_OFFSET+2]) & 0x02; // bitand on LSB to see 3d fix flag
+            numSV = char(payload[NUM_SV_OFFSET+2]); // number of GNSS satellite vehicles
+        }
+    }
+}
+
+float IMU::floatFromChar(unsigned char * value) {
+    unsigned char temp[4];
+    temp[0] = value[3];
+    temp[1] = value[2];
+    temp[2] = value[1];
+    temp[3] = value[0];
+    return *(float *) temp;
+}
+
+double IMU::doubleFromChar(unsigned char * value) {
+    unsigned char temp[8];
+    temp[0] = value[7];
+    temp[1] = value[6];
+    temp[2] = value[5];
+    temp[3] = value[4];
+    temp[4] = value[3];
+    temp[5] = value[2];
+    temp[6] = value[1];
+    temp[7] = value[0];
+    return *(double *) temp;
+}
+
+float IMU::getRoll() {
+    return euler[0];
+}
+
+float IMU::getPitch() {
+    return euler[1];
+}
+
+float IMU::getHeading() {
+    return euler[2];
+}
+
+bool IMU::getIsValid2dFix() {
+    return is2dFixValid;   
+}
+
+bool IMU::getIsValid3dFix() {
+    return is3dFixValid;
+}
+
+char IMU::getNumSV() {
+    return numSV;
+}
+
+double IMU::getLatitude() {
+    return latLonAlt[0];
+}
+
+double IMU::getLongitude() {
+    return latLonAlt[1];
+}
+
+double IMU::getAltitudeMSL() {
+    return latLonAlt[2];
+}
+
+unsigned int IMU::calcChecksum(unsigned char * mip_packet, char checksum_range) {
+    unsigned char checksum_byte1 = 0;
+    unsigned char checksum_byte2 = 0;
+    unsigned int myChecksum = 0;
+    
+    for (int i=0; i<checksum_range; i++) {
+        checksum_byte1 += mip_packet[i];
+        checksum_byte2 += checksum_byte1;
+    }
+    myChecksum = ((unsigned int)checksum_byte1 << 8) + (unsigned int)checksum_byte2;
+    return myChecksum;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/IMU.h	Tue Jun 19 20:14:23 2018 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+// for Microstrain's MIPS protocol, try this link, or search on microstrain.com
+// http://www.microstrain.com/sites/default/files/3dm-gx5-45_dcp_manual_8500-0064_0.pdf
+
+#define _PI ((float) 3.14159265359)
+
+// state machine states
+#define SYNC0 0 // first sync byte state
+#define SYNC1 1 // second sync byte state
+#define SET   2 // aka descriptor set
+#define LEN   3 // payload length
+#define PAY   4 // getting the payload data
+#define CRC0  5 // crc high byte
+#define CRC1  6 // crc low byte
+
+// data set descriptors
+#define IMU_DATA_SET                0x80
+#define GNSS_DATA_SET               0x81
+
+// enumerations for the Euler angle packet we care about
+#define EULER_CF_LENGTH             14
+#define EULER_CF_DESCRIPTOR         0x0C
+#define ROLL_OFFSET                 0
+#define PITCH_OFFSET                4
+#define YAW_OFFSET                  8
+
+// enumerations for the lat-lon-alt packet we care about
+#define LLH_POSITION_LENGTH         44
+#define LLH_POSITION_DESCRIPTOR     0x03
+#define LATITUDE_OFFSET             0
+#define LONGITUDE_OFFSET            8
+#define HEIGHT_MSL_OFFSET           16
+#define VALID_FLAG_OFFSET           40
+
+// enumerations for the lat-lon-alt packet we care about
+#define GNSS_FIX_INFO_LENGTH        8
+#define GNSS_FIX_INFO_DESCRIPTOR    0x0B
+#define FIX_TYPE_OFFSET             0
+#define NUM_SV_OFFSET               1
+#define VALID_FLAGS_OFFSET          4
+
+class IMU {
+public:
+    IMU(PinName Tx, PinName Rx);
+    void initialize();
+    void update();
+    void start();
+    void stop();
+    
+    void runIMU();
+    
+    float getRoll();
+    float getPitch();
+    float getHeading();
+   
+    bool getIsValid2dFix();
+    bool getIsValid3dFix();
+    char getNumSV();
+    double getLatitude();
+    double getLongitude();
+    double getAltitudeMSL();
+    
+    protected:
+    Ticker interval;
+    MODSERIAL _rs232;
+    
+    char byte;
+    
+    unsigned char state, len, descriptor, i, packet[256];
+    unsigned int checksum, crc0, crc1;
+    
+/*    unsigned char state, len, descriptor, i, crc0, crc1, payload[30];
+    unsigned int checksum;*/
+    
+    float euler[3];
+    double latLonAlt[3];
+    bool is2dFixValid;
+    bool is3dFixValid;
+    char numSV;
+
+    void processPayload(char type, char length, unsigned char * payload);
+    void processEulerCfPacket(char length, unsigned char * payload);
+    void processLatLonAltPacket(char length, unsigned char * payload);
+    void processGnssFixInformation(char length, unsigned char * payload);
+
+    unsigned int calcChecksum(unsigned char * mip_packet, char checksum_range);
+
+    float floatFromChar(unsigned char * value);
+    double doubleFromChar(unsigned char * value);
+};
\ No newline at end of file

--- a/StateMachine/StateMachine.cpp	Tue Jun 19 18:10:50 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Tue Jun 19 20:14:23 2018 +0000
@@ -861,7 +861,8 @@
     pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);  
     pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
               
-    pc().printf("-/+ to decrease/increase timeout: %d s\r\n",_timeout);        
+    pc().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);    
+    
     pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
     pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
     

--- a/System/StaticDefs.cpp	Tue Jun 19 18:10:50 2018 +0000
+++ b/System/StaticDefs.cpp	Tue Jun 19 20:14:23 2018 +0000
@@ -14,8 +14,8 @@
 }
     
 MODSERIAL & pc() {
-    static MODSERIAL pc(USBTX, USBRX);
-    //static MODSERIAL pc(p9, p10);        //XBee tx, rx pins
+    //static MODSERIAL pc(USBTX, USBRX);
+    static MODSERIAL pc(p9, p10);        //XBee tx, rx pins
     return pc;
 }
 

--- a/main.cpp	Tue Jun 19 18:10:50 2018 +0000
+++ b/main.cpp	Tue Jun 19 20:14:23 2018 +0000
@@ -178,7 +178,7 @@
 
 void setup() {
     pc().baud(57600);
-    pc().printf("\n\n\r 2018_03_28_wireless (FSG bench test)\n\n\r");
+    pc().printf("\n\n\r FSG PCB Bench Test V1.5)\n\n\r");
  
     // start up the system timer
     //systemTimer().start();
@@ -281,19 +281,33 @@
     systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
         
     while (1) {        
-        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz
+        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz (adc timing)
         {
             ++tNow;
 
-            if ( (tNow % 10) == 0 ) {   // 0.001 second intervals               //if ( (tNow % 100) == 0 ) {   // 0.01 second intervals
-                fsm_loop = true;
-                FSM();
+            //run finite state machine fast when transmitting data
+            if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
+                if ( (tNow % 10) == 0 ) {   // 0.001 second intervals  (1000 Hz)
+                    fsm_loop = true;
+                    FSM();
+                }
+                if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+                   log_loop = true;
+                    log_function();
+                }
             }
             
-//            if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
-//                log_loop = true;
-//                log_function();
-//            }
+            else {
+                if ( (tNow % 100) == 0 ) {   // 0.1 second intervals (10 Hz)
+                    fsm_loop = true;
+                    FSM();
+                }
+                
+                if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+                    log_loop = true;
+                    log_function();
+                }
+            }
         }
     }
 }
\ No newline at end of file