lhiggs CSUM / Mbed 2 deprecated FLIGHT_CONTROL_AND_COMMUNICATIONS_SYSTEM

Capstone project for Bachelor's in Mechanical Engineering 2011

Dependencies:   FatFileSystem MAX3100 MODGPS MODSERIAL SDFileSystem mbed

main.cpp@0:0529d2d7762f, 2013-05-29 (annotated)

Committer:
lhiggs
Date:
Wed May 29 00:45:41 2013 +0000
Revision:
0:0529d2d7762f
Broken, after updating all the libraries. RPC has issues with new mbed libraries.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lhiggs 0:0529d2d7762f 1 #include "mbed.h" // MBED LIBRARY
lhiggs 0:0529d2d7762f 2
lhiggs 0:0529d2d7762f 3 #include "SerialRPCInterface.h" // MBED RPC LIBRARY (FOR LABVIEW CONTROL OF PROGRAM FUNCTIONS AND VARIABLES)
lhiggs 0:0529d2d7762f 4
lhiggs 0:0529d2d7762f 5 #include "GPS.h" // MBED GPS LIBRARY
lhiggs 0:0529d2d7762f 6
lhiggs 0:0529d2d7762f 7 #include "MODSERIAL.h" // MBED BUFFERED SERIAL
lhiggs 0:0529d2d7762f 8 #include "SDFileSystem.h" // MBED SD LIBRARY
lhiggs 0:0529d2d7762f 9
lhiggs 0:0529d2d7762f 10
lhiggs 0:0529d2d7762f 11 #include "UM6_usart.h" // UM6 USART HEADER
lhiggs 0:0529d2d7762f 12 #include "UM6_config.h" // UM6 CONFIG HEADER
lhiggs 0:0529d2d7762f 13
lhiggs 0:0529d2d7762f 14
lhiggs 0:0529d2d7762f 15 #include "MAX3100.h" // MAX3100 SPI to UART CHIP DRIVER LIBRARY
lhiggs 0:0529d2d7762f 16
lhiggs 0:0529d2d7762f 17 #include "math.h"
lhiggs 0:0529d2d7762f 18
lhiggs 0:0529d2d7762f 19
lhiggs 0:0529d2d7762f 20 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 21 // Program Name: main.cpp
lhiggs 0:0529d2d7762f 22 // Description: This is the main program for an autonomous parafoil undergraduate capstone
lhiggs 0:0529d2d7762f 23 // project in Mechanical Engineering at the California Maritime Academy, Vallejo
lhiggs 0:0529d2d7762f 24 // CA, Spring 2011. As of yet there is no autonomous functionality. However, all
lhiggs 0:0529d2d7762f 25 // hardware has proven functional. The hardware that this program was written for
lhiggs 0:0529d2d7762f 26 // consists of an mbed LPC1768 microcontroller, cool components mbed breakout board
lhiggs 0:0529d2d7762f 27 // with 1GB microSD card, GM862-GPS combined cellular and gps module, DNT900P
lhiggs 0:0529d2d7762f 28 // 900Mhz 1Watt RF modem utilized as a transparent serial link to the application,
lhiggs 0:0529d2d7762f 29 // Hitec HS-785 Winch Servos, Firgelli PQ12 linear actuator with limit switches,
lhiggs 0:0529d2d7762f 30 // and last but by no means least a CH Robotics UM6 IMU/AHRS.
lhiggs 0:0529d2d7762f 31 // Author: Logan Higgs, (portions of UM6 interface adapted from open source UM6 code by CH
lhiggs 0:0529d2d7762f 32 // robotics}
lhiggs 0:0529d2d7762f 33 // Date: 04/2011
lhiggs 0:0529d2d7762f 34 // Special Thanks to Andy Kirkham from mbed who wrote most of the mbed libraries that this
lhiggs 0:0529d2d7762f 35 // program utilizes, and more importantly even edited his MODGPS library to support VTG GPS
lhiggs 0:0529d2d7762f 36 // data specifically from my request.
lhiggs 0:0529d2d7762f 37 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 38
lhiggs 0:0529d2d7762f 39
lhiggs 0:0529d2d7762f 40 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 41 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 42 // THIS IS FOR RECIEVED DATA COUNTING AND BOOLING GOT NEW DATA FLAGS
lhiggs 0:0529d2d7762f 43 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 44 static uint8_t got_data_counter = 0;
lhiggs 0:0529d2d7762f 45
lhiggs 0:0529d2d7762f 46 volatile bool got_data = false;
lhiggs 0:0529d2d7762f 47 volatile bool GOT_UM6 = false;
lhiggs 0:0529d2d7762f 48 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 49
lhiggs 0:0529d2d7762f 50
lhiggs 0:0529d2d7762f 51
lhiggs 0:0529d2d7762f 52
lhiggs 0:0529d2d7762f 53 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 54 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 55 // THIS IS FOR ATTEMPT TO MAKE UM6 DATA ARRAYS FOR LETTING microSD CARD
lhiggs 0:0529d2d7762f 56 // DATA WRITING ONLY ONCE A SECOND, INSTEAD OF EVERY TIME A UM6 DATA
lhiggs 0:0529d2d7762f 57 // SET IS RECIEVED
lhiggs 0:0529d2d7762f 58 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 59
lhiggs 0:0529d2d7762f 60 int UM6_data_counter = 0; // Counter for SD data logging
lhiggs 0:0529d2d7762f 61
lhiggs 0:0529d2d7762f 62 int16_t MY_DATA_GYRO_PROC_X;
lhiggs 0:0529d2d7762f 63 int16_t MY_DATA_GYRO_PROC_Y;
lhiggs 0:0529d2d7762f 64 int16_t MY_DATA_GYRO_PROC_Z;
lhiggs 0:0529d2d7762f 65 int16_t MY_DATA_ACCEL_PROC_X;
lhiggs 0:0529d2d7762f 66 int16_t MY_DATA_ACCEL_PROC_Y;
lhiggs 0:0529d2d7762f 67 int16_t MY_DATA_ACCEL_PROC_Z;
lhiggs 0:0529d2d7762f 68 int16_t MY_DATA_MAG_PROC_X;
lhiggs 0:0529d2d7762f 69 int16_t MY_DATA_MAG_PROC_Y;
lhiggs 0:0529d2d7762f 70 int16_t MY_DATA_MAG_PROC_Z;
lhiggs 0:0529d2d7762f 71 int16_t MY_DATA_EULER_PHI;
lhiggs 0:0529d2d7762f 72 int16_t MY_DATA_EULER_THETA;
lhiggs 0:0529d2d7762f 73 int16_t MY_DATA_EULER_PSI;
lhiggs 0:0529d2d7762f 74
lhiggs 0:0529d2d7762f 75 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 76
lhiggs 0:0529d2d7762f 77
lhiggs 0:0529d2d7762f 78
lhiggs 0:0529d2d7762f 79 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 80 // SETUP (ASSIGN) SERIAL COMMUNICATION PINS ON MBED
lhiggs 0:0529d2d7762f 81 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 82 //MODSERIAL pc(USBTX, USBRX); // PC SERIAL OVER USB PORT ON MBED
lhiggs 0:0529d2d7762f 83 //Create the interface on the USB Serial Port
lhiggs 0:0529d2d7762f 84 //SerialRPCInterface SerialInterface(USBTX, USBRX);
lhiggs 0:0529d2d7762f 85 SerialRPCInterface SerialInterface(p28, p27);
lhiggs 0:0529d2d7762f 86
lhiggs 0:0529d2d7762f 87
lhiggs 0:0529d2d7762f 88 MODSERIAL uart(p26, p25); // UM6 SERIAL OVER UART PINS 28 & 27
lhiggs 0:0529d2d7762f 89
lhiggs 0:0529d2d7762f 90 // SETUP DNT900P RF MODEM SERIAL
lhiggs 0:0529d2d7762f 91 //MODSERIAL DNT900P(p28, p27);
lhiggs 0:0529d2d7762f 92
lhiggs 0:0529d2d7762f 93 // SETUP GM862-GPS MODEM SERIAL PORT THRU MAX3100 SPI to UART IC
lhiggs 0:0529d2d7762f 94 MAX3100 max(p11, p12, p13, p14,p15);
lhiggs 0:0529d2d7762f 95
lhiggs 0:0529d2d7762f 96 // SETUP GM862-GPS GPS SERIAL PORT
lhiggs 0:0529d2d7762f 97 // Serial gps_uart(p9, p10);
lhiggs 0:0529d2d7762f 98
lhiggs 0:0529d2d7762f 99 // Create an instance of the GPS object for MODGPS library
lhiggs 0:0529d2d7762f 100 GPS gps(NC, p10);
lhiggs 0:0529d2d7762f 101
lhiggs 0:0529d2d7762f 102
lhiggs 0:0529d2d7762f 103
lhiggs 0:0529d2d7762f 104
lhiggs 0:0529d2d7762f 105 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 106 // SETUP (ASSIGN) SPI COMMUNICATION PINS ON MBED FOR SD CARD ON COOLCOMPONENTS WORKSHOP BOARD
lhiggs 0:0529d2d7762f 107 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 108 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
lhiggs 0:0529d2d7762f 109
lhiggs 0:0529d2d7762f 110 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 111 // SETUP (ASSIGN) WINCH SERVO#1 (STBD) & WINCH SERVO#2 (PORT)
lhiggs 0:0529d2d7762f 112 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 113 PwmOut servo1(p23);
lhiggs 0:0529d2d7762f 114 PwmOut servo2(p24);
lhiggs 0:0529d2d7762f 115
lhiggs 0:0529d2d7762f 116
lhiggs 0:0529d2d7762f 117 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 118 // SETUP (ASSIGN) DIGITAL OUTPUTS FOR H-BRIDGE ACTUATOR DRIVER FOR PARAFOIL DEPLOYMENT
lhiggs 0:0529d2d7762f 119 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 120 DigitalOut red(p16);
lhiggs 0:0529d2d7762f 121 DigitalOut EN(p17);
lhiggs 0:0529d2d7762f 122 DigitalOut blue(p18);
lhiggs 0:0529d2d7762f 123
lhiggs 0:0529d2d7762f 124
lhiggs 0:0529d2d7762f 125
lhiggs 0:0529d2d7762f 126 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 127 // SETUP (ASSIGN) MBED LED (1 thru 3) FOR VISUAL DEBUGGING ON MBED
lhiggs 0:0529d2d7762f 128 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 129 DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
lhiggs 0:0529d2d7762f 130 DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
lhiggs 0:0529d2d7762f 131 DigitalOut sd_activity(LED3); // LED3 = SD CARD
lhiggs 0:0529d2d7762f 132
lhiggs 0:0529d2d7762f 133
lhiggs 0:0529d2d7762f 134
lhiggs 0:0529d2d7762f 135 // CREATE FLOAT VARIABLES FOR LABVIEW REMOTE CONTROL OF PROGRAM
lhiggs 0:0529d2d7762f 136 float servo1_pw = 0.0015;
lhiggs 0:0529d2d7762f 137 float servo2_pw = 0.0015;
lhiggs 0:0529d2d7762f 138 float Actuator_CMD = 1;
lhiggs 0:0529d2d7762f 139 float Latitude = 0;
lhiggs 0:0529d2d7762f 140 float Longitude = 0;
lhiggs 0:0529d2d7762f 141 float Altitude = 0;
lhiggs 0:0529d2d7762f 142 float Roll = 0;
lhiggs 0:0529d2d7762f 143 float Pitch = 0;
lhiggs 0:0529d2d7762f 144 float Yaw = 0;
lhiggs 0:0529d2d7762f 145 float Control_Mode = 0;
lhiggs 0:0529d2d7762f 146 float CMD_Latitude = 0;
lhiggs 0:0529d2d7762f 147 float CMD_Longitude = 0;
lhiggs 0:0529d2d7762f 148 float CALC_Heading = 0;
lhiggs 0:0529d2d7762f 149 float CMD_Heading = 0;
lhiggs 0:0529d2d7762f 150
lhiggs 0:0529d2d7762f 151
lhiggs 0:0529d2d7762f 152 // FROM UM6 FIRMWARE SOURCE
lhiggs 0:0529d2d7762f 153
lhiggs 0:0529d2d7762f 154 // Flag for storing the current USART state
lhiggs 0:0529d2d7762f 155 uint8_t gUSART_State = USART_STATE_WAIT;
lhiggs 0:0529d2d7762f 156
lhiggs 0:0529d2d7762f 157 /*******************************************************************************
lhiggs 0:0529d2d7762f 158 * Function Name : ComputeChecksum
lhiggs 0:0529d2d7762f 159 * Input : USARTPacket* new_packet
lhiggs 0:0529d2d7762f 160 * Output : None
lhiggs 0:0529d2d7762f 161 * Return : uint16_t
lhiggs 0:0529d2d7762f 162 * Description : Returns the two byte sum of all the individual bytes in the
lhiggs 0:0529d2d7762f 163 given packet.
lhiggs 0:0529d2d7762f 164 *******************************************************************************/
lhiggs 0:0529d2d7762f 165 uint16_t ComputeChecksum( USARTPacket* new_packet ) {
lhiggs 0:0529d2d7762f 166 int32_t index;
lhiggs 0:0529d2d7762f 167 uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
lhiggs 0:0529d2d7762f 168
lhiggs 0:0529d2d7762f 169 for ( index = 0; index < new_packet->data_length; index++ ) {
lhiggs 0:0529d2d7762f 170 checksum += new_packet->packet_data[index];
lhiggs 0:0529d2d7762f 171 }
lhiggs 0:0529d2d7762f 172 return checksum;
lhiggs 0:0529d2d7762f 173 }
lhiggs 0:0529d2d7762f 174
lhiggs 0:0529d2d7762f 175
lhiggs 0:0529d2d7762f 176
lhiggs 0:0529d2d7762f 177
lhiggs 0:0529d2d7762f 178
lhiggs 0:0529d2d7762f 179 //////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 180 //////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 181 // MODSERIAL INTERUPT FUNCTION AND MODSERIAL OVERFLOW INTERUPT FUNCTION FOR DEBUGGING
lhiggs 0:0529d2d7762f 182 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 183
lhiggs 0:0529d2d7762f 184 // MBED MODSERIAL INTERUPT FUNCTION
lhiggs 0:0529d2d7762f 185 // This function is called when a character goes into the RX buffer.
lhiggs 0:0529d2d7762f 186 //void rxCallback(void) {
lhiggs 0:0529d2d7762f 187 // if (uart.rxBufferGetCount() > MAX_PACKET_DATA & got_data == false) {
lhiggs 0:0529d2d7762f 188 // got_data = true;
lhiggs 0:0529d2d7762f 189 // uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
lhiggs 0:0529d2d7762f 190 // }
lhiggs 0:0529d2d7762f 191 //}
lhiggs 0:0529d2d7762f 192
lhiggs 0:0529d2d7762f 193 void rxOvflow(void) {
lhiggs 0:0529d2d7762f 194 error("Ouch, overflowed");
lhiggs 0:0529d2d7762f 195 }
lhiggs 0:0529d2d7762f 196
lhiggs 0:0529d2d7762f 197 //SDFileSystem *sd;
lhiggs 0:0529d2d7762f 198 ///////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 199 ///////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 200
lhiggs 0:0529d2d7762f 201
lhiggs 0:0529d2d7762f 202
lhiggs 0:0529d2d7762f 203
lhiggs 0:0529d2d7762f 204
lhiggs 0:0529d2d7762f 205
lhiggs 0:0529d2d7762f 206 int main() {
lhiggs 0:0529d2d7762f 207
lhiggs 0:0529d2d7762f 208 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 209 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 210 //Make these variables accessible over RPC by attaching them to an RPCVariable
lhiggs 0:0529d2d7762f 211 RPCVariable<float> RPCservo1_pw(&servo1_pw, "servo1_pw");
lhiggs 0:0529d2d7762f 212 RPCVariable<float> RPCservo2_pw(&servo2_pw, "servo2_pw");
lhiggs 0:0529d2d7762f 213 RPCVariable<float> RPCActuator_CMD(&Actuator_CMD, "Actuator_CMD");
lhiggs 0:0529d2d7762f 214
lhiggs 0:0529d2d7762f 215 RPCVariable<float> RPCLatitude(&Latitude, "Latitude");
lhiggs 0:0529d2d7762f 216 RPCVariable<float> RPCLongitude(&Longitude, "Longitude");
lhiggs 0:0529d2d7762f 217 RPCVariable<float> RPCAltitude(&Altitude, "Altitude");
lhiggs 0:0529d2d7762f 218
lhiggs 0:0529d2d7762f 219 RPCVariable<float> RPCRoll(&Roll, "Roll");
lhiggs 0:0529d2d7762f 220 RPCVariable<float> RPCPitch(&Pitch, "Pitch");
lhiggs 0:0529d2d7762f 221 RPCVariable<float> RPCYaw(&Yaw, "Yaw");
lhiggs 0:0529d2d7762f 222
lhiggs 0:0529d2d7762f 223 RPCVariable<float> RPCControl_Mode(&Control_Mode, "Control_Mode");
lhiggs 0:0529d2d7762f 224
lhiggs 0:0529d2d7762f 225 RPCVariable<float> RPCCMD_Latitude(&CMD_Latitude, "CMD_Latitude");
lhiggs 0:0529d2d7762f 226 RPCVariable<float> RPCCMD_Longitude(&CMD_Longitude, "CMD_Lognitude");
lhiggs 0:0529d2d7762f 227
lhiggs 0:0529d2d7762f 228 RPCVariable<float> RPCCALC_Heading(&CALC_Heading, "CALC_Heading");
lhiggs 0:0529d2d7762f 229 RPCVariable<float> RPCCMD_Heading(&CMD_Heading, "CMD_Heading");
lhiggs 0:0529d2d7762f 230 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 231
lhiggs 0:0529d2d7762f 232
lhiggs 0:0529d2d7762f 233
lhiggs 0:0529d2d7762f 234 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 235 GPS_Time q1;
lhiggs 0:0529d2d7762f 236 GPS_VTG v1;
lhiggs 0:0529d2d7762f 237 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 238
lhiggs 0:0529d2d7762f 239
lhiggs 0:0529d2d7762f 240 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 241 // WINCH SERVO INITIALIZATION
lhiggs 0:0529d2d7762f 242 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 243 servo1.period(0.020); // servos requires a 20ms period
lhiggs 0:0529d2d7762f 244 servo2.period(0.020);
lhiggs 0:0529d2d7762f 245
lhiggs 0:0529d2d7762f 246 // initialize servo to center (neutral position)
lhiggs 0:0529d2d7762f 247 servo1.pulsewidth(0.0015);
lhiggs 0:0529d2d7762f 248 servo2.pulsewidth(0.0015);
lhiggs 0:0529d2d7762f 249
lhiggs 0:0529d2d7762f 250 //servo_neutral_activity = !servo_neutral_activity;
lhiggs 0:0529d2d7762f 251
lhiggs 0:0529d2d7762f 252 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 253
lhiggs 0:0529d2d7762f 254
lhiggs 0:0529d2d7762f 255 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 256 // THIS MAKES A DIRECTORY ON THE microSD CARD FOR DATA LOGGING
lhiggs 0:0529d2d7762f 257 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 258 // SD DATA LOGGING
lhiggs 0:0529d2d7762f 259 // mkdir("/sd/", 0777);
lhiggs 0:0529d2d7762f 260 // sd_activity = !sd_activity;
lhiggs 0:0529d2d7762f 261 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 262
lhiggs 0:0529d2d7762f 263
lhiggs 0:0529d2d7762f 264
lhiggs 0:0529d2d7762f 265 /////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 266 // SET SERIAL UART BAUD RATES
lhiggs 0:0529d2d7762f 267 /////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 268
lhiggs 0:0529d2d7762f 269 // set UM6 serial uart baud 115200
lhiggs 0:0529d2d7762f 270 uart.baud(9600);
lhiggs 0:0529d2d7762f 271
lhiggs 0:0529d2d7762f 272
lhiggs 0:0529d2d7762f 273 // pc bauds for usb virtual serial port on mbed for ground use only.
lhiggs 0:0529d2d7762f 274 // pc.baud(9600); // pc baud for GM862-GPS modem to pc interface
lhiggs 0:0529d2d7762f 275 // pc.baud(9600); // pc baud for UM6 to pc interface
lhiggs 0:0529d2d7762f 276
lhiggs 0:0529d2d7762f 277 // set DNT900P baud 9600
lhiggs 0:0529d2d7762f 278 // DNT900P.baud(9600);
lhiggs 0:0529d2d7762f 279
lhiggs 0:0529d2d7762f 280 // set gps baud 4800
lhiggs 0:0529d2d7762f 281 gps.baud(4800);
lhiggs 0:0529d2d7762f 282 gps.format(8, GPS::None, 1);
lhiggs 0:0529d2d7762f 283
lhiggs 0:0529d2d7762f 284
lhiggs 0:0529d2d7762f 285 // enable max3100 interupts for GM862-GPS modem uart thru spi to uart ic
lhiggs 0:0529d2d7762f 286 max.enableRxIrq();
lhiggs 0:0529d2d7762f 287 max.enableTxIrq();
lhiggs 0:0529d2d7762f 288
lhiggs 0:0529d2d7762f 289
lhiggs 0:0529d2d7762f 290
lhiggs 0:0529d2d7762f 291 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 292 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 293 // THIS WAS FOR DEBUGGING MODSERIAL AND SD CARD, STILL USEFUL FOR CHECKING FOR OVERFLOW OF MODSERIAL
lhiggs 0:0529d2d7762f 294 // CIRCULAR BUFFER
lhiggs 0:0529d2d7762f 295 /////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 296 // In main after setting the uart baud rate and before attaching your rxCallback function add:-
lhiggs 0:0529d2d7762f 297 uart.attach(&rxOvflow, MODSERIAL::RxOvIrq);
lhiggs 0:0529d2d7762f 298 // Setup the uart serial port, then...
lhiggs 0:0529d2d7762f 299 // sd = new SDFileSystem(p5, p6, p7, p8, "sd");
lhiggs 0:0529d2d7762f 300
lhiggs 0:0529d2d7762f 301 // attach interupt function to uart
lhiggs 0:0529d2d7762f 302 // uart.attach(&rxCallback, MODSERIAL::RxIrq);
lhiggs 0:0529d2d7762f 303
lhiggs 0:0529d2d7762f 304 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 305 //////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 306
lhiggs 0:0529d2d7762f 307
lhiggs 0:0529d2d7762f 308
lhiggs 0:0529d2d7762f 309
lhiggs 0:0529d2d7762f 310
lhiggs 0:0529d2d7762f 311
lhiggs 0:0529d2d7762f 312 while (1) {
lhiggs 0:0529d2d7762f 313
lhiggs 0:0529d2d7762f 314
lhiggs 0:0529d2d7762f 315 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 316 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 317 // GPS AUTOMATIC DATA PARSING WITH MODGPS LIBRARY
lhiggs 0:0529d2d7762f 318 // Demonstrate how to find out the GPS location co-ords.
lhiggs 0:0529d2d7762f 319 // pc.printf("Lat = %.4f ", gps.latitude());
lhiggs 0:0529d2d7762f 320 // pc.printf("Lon = %.4f ", gps.longitude());
lhiggs 0:0529d2d7762f 321 // pc.printf("Alt = %.4f ", gps.altitude());
lhiggs 0:0529d2d7762f 322
lhiggs 0:0529d2d7762f 323 // Gran a snapshot of the current time.
lhiggs 0:0529d2d7762f 324 gps.timeNow(&q1);
lhiggs 0:0529d2d7762f 325 // pc.printf("%02d:%02d:%02d %02d/%02d/%04d\r\n",
lhiggs 0:0529d2d7762f 326 // q1.hour, q1.minute, q1.second, q1.day, q1.month, q1.year);
lhiggs 0:0529d2d7762f 327
lhiggs 0:0529d2d7762f 328 gps.vtg(&v1);
lhiggs 0:0529d2d7762f 329 // pc.printf("Method 1. Vector data, Speed (knots):%lf, Speed(kph):%lf, Track(true):%lf, Track(mag)%lf\r\n",
lhiggs 0:0529d2d7762f 330 // v1.velocity_knots(), v1.velocity_kph(), v1.track_true(), v1.track_mag());
lhiggs 0:0529d2d7762f 331 //
lhiggs 0:0529d2d7762f 332
lhiggs 0:0529d2d7762f 333
lhiggs 0:0529d2d7762f 334
lhiggs 0:0529d2d7762f 335 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 336
lhiggs 0:0529d2d7762f 337
lhiggs 0:0529d2d7762f 338
lhiggs 0:0529d2d7762f 339
lhiggs 0:0529d2d7762f 340 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 341 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 342 // THIS IS A SERIAL PASS THRU BETWEEN THE GM862-GPS MODEM SERIAL PORT AND THE PC USB VIRTUAL SERIAL PORT.
lhiggs 0:0529d2d7762f 343 // IT ISN'T INTENDED TO BE USED DURING FLIGHT, BUT RATHER FOR CONFIGURATION OF THE GM862-GPS WHILE ON
lhiggs 0:0529d2d7762f 344 // THE GROUND.
lhiggs 0:0529d2d7762f 345 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 346
lhiggs 0:0529d2d7762f 347 // if (pc.readable()) {
lhiggs 0:0529d2d7762f 348 // int c = pc.getc();
lhiggs 0:0529d2d7762f 349 // max.putc(c);
lhiggs 0:0529d2d7762f 350 // }
lhiggs 0:0529d2d7762f 351
lhiggs 0:0529d2d7762f 352 // if (max.readable()) {
lhiggs 0:0529d2d7762f 353 // pc.putc( max.getc() );
lhiggs 0:0529d2d7762f 354 // }
lhiggs 0:0529d2d7762f 355
lhiggs 0:0529d2d7762f 356 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 357
lhiggs 0:0529d2d7762f 358
lhiggs 0:0529d2d7762f 359
lhiggs 0:0529d2d7762f 360 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 361 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 362 // THIS IS A SERIAL PASS THRU FOR THE UM6 AHRS/IMU TO THE PC FOR USING THE CH ROBOTICS SERIAL INTERFACE
lhiggs 0:0529d2d7762f 363 // PC SOFTWARE FOR CALIBRATING AND VIEWING DATA FROM THE UM6. DEFAULT PC BAUD 115200
lhiggs 0:0529d2d7762f 364 // if (pc.readable()) {
lhiggs 0:0529d2d7762f 365 // int c = pc.getc();
lhiggs 0:0529d2d7762f 366 // uart.putc(c);
lhiggs 0:0529d2d7762f 367 // }
lhiggs 0:0529d2d7762f 368
lhiggs 0:0529d2d7762f 369 // if (uart.readable()) {
lhiggs 0:0529d2d7762f 370 // pc.putc( uart.getc() );
lhiggs 0:0529d2d7762f 371 // }
lhiggs 0:0529d2d7762f 372
lhiggs 0:0529d2d7762f 373 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 374
lhiggs 0:0529d2d7762f 375 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 376 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 377 // THIS IS FOR THE UM6 IMU/AHRS SERIAL COMMUNICATIONS, IT REALLY SHOULD BE PUT INTO AT LEAST ITS OWN
lhiggs 0:0529d2d7762f 378 // FUNCTIONS, OR EVEN A UM6 CLASS AND LIBRARY.
lhiggs 0:0529d2d7762f 379 ///////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 380
lhiggs 0:0529d2d7762f 381 if (uart.rxBufferGetCount() > MAX_PACKET_DATA) {
lhiggs 0:0529d2d7762f 382
lhiggs 0:0529d2d7762f 383
lhiggs 0:0529d2d7762f 384
lhiggs 0:0529d2d7762f 385 uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
lhiggs 0:0529d2d7762f 386
lhiggs 0:0529d2d7762f 387 // UM6 Firmware Source Function ProcessNextCharacter()
lhiggs 0:0529d2d7762f 388 //void ProcessNextCharacter( ) {
lhiggs 0:0529d2d7762f 389 static uint8_t data_counter = 0;
lhiggs 0:0529d2d7762f 390
lhiggs 0:0529d2d7762f 391 static USARTPacket new_packet;
lhiggs 0:0529d2d7762f 392
lhiggs 0:0529d2d7762f 393 // The next action should depend on the USART state.
lhiggs 0:0529d2d7762f 394 switch ( gUSART_State ) {
lhiggs 0:0529d2d7762f 395 // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
lhiggs 0:0529d2d7762f 396 // that signals a new incoming packet.
lhiggs 0:0529d2d7762f 397 case USART_STATE_WAIT:
lhiggs 0:0529d2d7762f 398 if ( data_counter == 0 ) { // Waiting on 's' character
lhiggs 0:0529d2d7762f 399 if ( uart.getc() == 's' ) {
lhiggs 0:0529d2d7762f 400
lhiggs 0:0529d2d7762f 401 data_counter++;
lhiggs 0:0529d2d7762f 402 } else {
lhiggs 0:0529d2d7762f 403 data_counter = 0;
lhiggs 0:0529d2d7762f 404 }
lhiggs 0:0529d2d7762f 405 } else if ( data_counter == 1 ) { // Waiting on 'n' character
lhiggs 0:0529d2d7762f 406 if ( uart.getc() == 'n' ) {
lhiggs 0:0529d2d7762f 407 data_counter++;
lhiggs 0:0529d2d7762f 408
lhiggs 0:0529d2d7762f 409 } else {
lhiggs 0:0529d2d7762f 410 data_counter = 0;
lhiggs 0:0529d2d7762f 411 }
lhiggs 0:0529d2d7762f 412 } else if ( data_counter == 2 ) { // Waiting on 'p' character
lhiggs 0:0529d2d7762f 413 if ( uart.getc() == 'p' ) {
lhiggs 0:0529d2d7762f 414 // The full 'snp' sequence was received. Reset data_counter (it will be used again
lhiggs 0:0529d2d7762f 415 // later) and transition to the next state.
lhiggs 0:0529d2d7762f 416 data_counter = 0;
lhiggs 0:0529d2d7762f 417 gUSART_State = USART_STATE_TYPE;
lhiggs 0:0529d2d7762f 418
lhiggs 0:0529d2d7762f 419 } else {
lhiggs 0:0529d2d7762f 420 data_counter = 0;
lhiggs 0:0529d2d7762f 421 }
lhiggs 0:0529d2d7762f 422 }
lhiggs 0:0529d2d7762f 423 break;
lhiggs 0:0529d2d7762f 424
lhiggs 0:0529d2d7762f 425 // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
lhiggs 0:0529d2d7762f 426 // indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
lhiggs 0:0529d2d7762f 427 case USART_STATE_TYPE:
lhiggs 0:0529d2d7762f 428
lhiggs 0:0529d2d7762f 429 new_packet.PT = uart.getc();
lhiggs 0:0529d2d7762f 430
lhiggs 0:0529d2d7762f 431 gUSART_State = USART_STATE_ADDRESS;
lhiggs 0:0529d2d7762f 432
lhiggs 0:0529d2d7762f 433 break;
lhiggs 0:0529d2d7762f 434
lhiggs 0:0529d2d7762f 435 // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
lhiggs 0:0529d2d7762f 436 // the address that the packet applies to
lhiggs 0:0529d2d7762f 437 case USART_STATE_ADDRESS:
lhiggs 0:0529d2d7762f 438 new_packet.address = uart.getc();
lhiggs 0:0529d2d7762f 439 // pc.putc(new_packet.address);
lhiggs 0:0529d2d7762f 440
lhiggs 0:0529d2d7762f 441 // For convenience, identify the type of packet this is and copy to the packet structure
lhiggs 0:0529d2d7762f 442 // (this will be used by the packet handler later)
lhiggs 0:0529d2d7762f 443 if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
lhiggs 0:0529d2d7762f 444 new_packet.address_type = ADDRESS_TYPE_CONFIG;
lhiggs 0:0529d2d7762f 445 } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
lhiggs 0:0529d2d7762f 446 new_packet.address_type = ADDRESS_TYPE_DATA;
lhiggs 0:0529d2d7762f 447 } else {
lhiggs 0:0529d2d7762f 448 new_packet.address_type = ADDRESS_TYPE_COMMAND;
lhiggs 0:0529d2d7762f 449 }
lhiggs 0:0529d2d7762f 450
lhiggs 0:0529d2d7762f 451 // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
lhiggs 0:0529d2d7762f 452 // If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet
lhiggs 0:0529d2d7762f 453 if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
lhiggs 0:0529d2d7762f 454 gUSART_State = USART_STATE_CHECKSUM;
lhiggs 0:0529d2d7762f 455 }
lhiggs 0:0529d2d7762f 456
lhiggs 0:0529d2d7762f 457 // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
lhiggs 0:0529d2d7762f 458 else {
lhiggs 0:0529d2d7762f 459 gUSART_State = USART_STATE_DATA;
lhiggs 0:0529d2d7762f 460 // Determine the expected number of bytes in this data packet based on the packet type. A write operation
lhiggs 0:0529d2d7762f 461 // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
lhiggs 0:0529d2d7762f 462 // where the batch size is also given in the packet type byte.
lhiggs 0:0529d2d7762f 463 if ( new_packet.PT & PACKET_IS_BATCH ) {
lhiggs 0:0529d2d7762f 464 new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
lhiggs 0:0529d2d7762f 465
lhiggs 0:0529d2d7762f 466 } else {
lhiggs 0:0529d2d7762f 467 new_packet.data_length = 4;
lhiggs 0:0529d2d7762f 468 }
lhiggs 0:0529d2d7762f 469 }
lhiggs 0:0529d2d7762f 470 break;
lhiggs 0:0529d2d7762f 471
lhiggs 0:0529d2d7762f 472 // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
lhiggs 0:0529d2d7762f 473 // data.
lhiggs 0:0529d2d7762f 474 case USART_STATE_DATA:
lhiggs 0:0529d2d7762f 475 new_packet.packet_data[data_counter] = uart.getc();
lhiggs 0:0529d2d7762f 476 data_counter++;
lhiggs 0:0529d2d7762f 477
lhiggs 0:0529d2d7762f 478 // If the expected number of bytes has been received, transition to the CHECKSUM state.
lhiggs 0:0529d2d7762f 479 if ( data_counter == new_packet.data_length ) {
lhiggs 0:0529d2d7762f 480 // Reset data_counter, since it will be used in the CHECKSUM state.
lhiggs 0:0529d2d7762f 481 data_counter = 0;
lhiggs 0:0529d2d7762f 482 gUSART_State = USART_STATE_CHECKSUM;
lhiggs 0:0529d2d7762f 483 }
lhiggs 0:0529d2d7762f 484 break;
lhiggs 0:0529d2d7762f 485
lhiggs 0:0529d2d7762f 486
lhiggs 0:0529d2d7762f 487
lhiggs 0:0529d2d7762f 488 // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
lhiggs 0:0529d2d7762f 489 // of the 16-bit checksum.
lhiggs 0:0529d2d7762f 490 case USART_STATE_CHECKSUM:
lhiggs 0:0529d2d7762f 491 // Get the highest-order byte
lhiggs 0:0529d2d7762f 492 if ( data_counter == 0 ) {
lhiggs 0:0529d2d7762f 493 // uint8_t Temp_Packet = uart.getc();
lhiggs 0:0529d2d7762f 494 new_packet.checksum = ((uint16_t)uart.getc() << 8);
lhiggs 0:0529d2d7762f 495 data_counter++;
lhiggs 0:0529d2d7762f 496 } else { // ( data_counter == 1 )
lhiggs 0:0529d2d7762f 497 // Get lower-order byte
lhiggs 0:0529d2d7762f 498 new_packet.checksum = new_packet.checksum | ((uint16_t)uart.getc() & 0x0FF);
lhiggs 0:0529d2d7762f 499
lhiggs 0:0529d2d7762f 500 // Both checksum bytes have been received. Make sure that the checksum is valid.
lhiggs 0:0529d2d7762f 501 uint16_t checksum = ComputeChecksum( &new_packet );
lhiggs 0:0529d2d7762f 502
lhiggs 0:0529d2d7762f 503
lhiggs 0:0529d2d7762f 504
lhiggs 0:0529d2d7762f 505 // If checksum does not match, send a BAD_CHECKSUM packet
lhiggs 0:0529d2d7762f 506 if ( checksum != new_packet.checksum ) {
lhiggs 0:0529d2d7762f 507 got_data = false;
lhiggs 0:0529d2d7762f 508 }
lhiggs 0:0529d2d7762f 509
lhiggs 0:0529d2d7762f 510 else
lhiggs 0:0529d2d7762f 511
lhiggs 0:0529d2d7762f 512 {
lhiggs 0:0529d2d7762f 513
lhiggs 0:0529d2d7762f 514 // Packet was received correctly.
lhiggs 0:0529d2d7762f 515
lhiggs 0:0529d2d7762f 516 //-----------------------------------------------------------------------------------------------
lhiggs 0:0529d2d7762f 517 //-----------------------------------------------------------------------------------------------
lhiggs 0:0529d2d7762f 518 //
lhiggs 0:0529d2d7762f 519 // ADDED: CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
lhiggs 0:0529d2d7762f 520
lhiggs 0:0529d2d7762f 521
lhiggs 0:0529d2d7762f 522 // IF DATA ADDRESS
lhiggs 0:0529d2d7762f 523 if (new_packet.address_type == ADDRESS_TYPE_DATA) {
lhiggs 0:0529d2d7762f 524
lhiggs 0:0529d2d7762f 525 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 526 // UM6_GYRO_PROC_XY (0x5C)
lhiggs 0:0529d2d7762f 527 // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
lhiggs 0:0529d2d7762f 528 // per second, the data should be multiplied by the scale factor 0.0610352 as shown below
lhiggs 0:0529d2d7762f 529 // angular_rate = register_data*0.0610352
lhiggs 0:0529d2d7762f 530
lhiggs 0:0529d2d7762f 531 if (new_packet.address == UM6_GYRO_PROC_XY) {
lhiggs 0:0529d2d7762f 532
lhiggs 0:0529d2d7762f 533 // GYRO_PROC_X
lhiggs 0:0529d2d7762f 534 MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:0529d2d7762f 535 MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
lhiggs 0:0529d2d7762f 536 // pc.printf( "GPx %f deg/s\n",MY_DATA_GYRO_PROC_X*0.0610352);
lhiggs 0:0529d2d7762f 537
lhiggs 0:0529d2d7762f 538
lhiggs 0:0529d2d7762f 539 MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:0529d2d7762f 540 MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:0529d2d7762f 541 // pc.printf( "GPy %f d/s\n",MY_DATA_GYRO_PROC_Y*0.0610352);
lhiggs 0:0529d2d7762f 542
lhiggs 0:0529d2d7762f 543 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 544
lhiggs 0:0529d2d7762f 545
lhiggs 0:0529d2d7762f 546 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 547 // UM6_GYRO_PROC_Z (0x5D)
lhiggs 0:0529d2d7762f 548 // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
lhiggs 0:0529d2d7762f 549 // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
lhiggs 0:0529d2d7762f 550
lhiggs 0:0529d2d7762f 551
lhiggs 0:0529d2d7762f 552 // GYRO_PROC_Z
lhiggs 0:0529d2d7762f 553 MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:0529d2d7762f 554 MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];
lhiggs 0:0529d2d7762f 555 //pc.printf( "GPz %f deg/s\n",MY_DATA_GYRO_PROC_Z*0.0610352);
lhiggs 0:0529d2d7762f 556 }
lhiggs 0:0529d2d7762f 557 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 558
lhiggs 0:0529d2d7762f 559
lhiggs 0:0529d2d7762f 560 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 561 // UM6_ACCEL_PROC_XY (0x5E)
lhiggs 0:0529d2d7762f 562 // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
lhiggs 0:0529d2d7762f 563 // the data should be multiplied by the scale factor 0.000183105 as shown below.
lhiggs 0:0529d2d7762f 564 // acceleration = register_data* 0.000183105
lhiggs 0:0529d2d7762f 565 if (new_packet.address == UM6_ACCEL_PROC_XY) {
lhiggs 0:0529d2d7762f 566
lhiggs 0:0529d2d7762f 567 // ACCEL_PROC_X
lhiggs 0:0529d2d7762f 568 MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:0529d2d7762f 569 MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
lhiggs 0:0529d2d7762f 570 // pc.printf( "Apx %f g \n",MY_DATA_ACCEL_PROC_X*0.000183105);
lhiggs 0:0529d2d7762f 571
lhiggs 0:0529d2d7762f 572 // ACCEL_PROC_Y
lhiggs 0:0529d2d7762f 573 MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:0529d2d7762f 574 MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:0529d2d7762f 575 // pc.printf( "Apy %f g \n",MY_DATA_ACCEL_PROC_Y*0.000183105);
lhiggs 0:0529d2d7762f 576
lhiggs 0:0529d2d7762f 577 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 578
lhiggs 0:0529d2d7762f 579
lhiggs 0:0529d2d7762f 580 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 581 // UM6_ACCEL_PROC_Z (0x5F)
lhiggs 0:0529d2d7762f 582 // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
lhiggs 0:0529d2d7762f 583 // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
lhiggs 0:0529d2d7762f 584
lhiggs 0:0529d2d7762f 585 // ACCEL_PROC_Z
lhiggs 0:0529d2d7762f 586 MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:0529d2d7762f 587 MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
lhiggs 0:0529d2d7762f 588 // pc.printf( "Apz %f g \n",MY_DATA_ACCEL_PROC_Z*0.000183105);
lhiggs 0:0529d2d7762f 589 }
lhiggs 0:0529d2d7762f 590
lhiggs 0:0529d2d7762f 591 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 592
lhiggs 0:0529d2d7762f 593
lhiggs 0:0529d2d7762f 594 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 595 // UM6_MAG_PROC_XY (0x60)
lhiggs 0:0529d2d7762f 596 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
lhiggs 0:0529d2d7762f 597 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
lhiggs 0:0529d2d7762f 598 // shown below.
lhiggs 0:0529d2d7762f 599 // magnetic field = register_data* 0.000305176
lhiggs 0:0529d2d7762f 600 if (new_packet.address == UM6_MAG_PROC_XY) {
lhiggs 0:0529d2d7762f 601
lhiggs 0:0529d2d7762f 602 // MAG_PROC_X
lhiggs 0:0529d2d7762f 603 MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:0529d2d7762f 604 MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
lhiggs 0:0529d2d7762f 605 // pc.printf( "Mpx %f \n",MY_DATA_MAG_PROC_X*0.000305176);
lhiggs 0:0529d2d7762f 606
lhiggs 0:0529d2d7762f 607 // MAG_PROC_Y
lhiggs 0:0529d2d7762f 608 MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:0529d2d7762f 609 MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:0529d2d7762f 610 // pc.printf( "Mpy %f \n",MY_DATA_MAG_PROC_Y*0.000305176);
lhiggs 0:0529d2d7762f 611
lhiggs 0:0529d2d7762f 612 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 613
lhiggs 0:0529d2d7762f 614
lhiggs 0:0529d2d7762f 615 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 616 // UM6_MAG_PROC_Z (0x61)
lhiggs 0:0529d2d7762f 617 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
lhiggs 0:0529d2d7762f 618 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
lhiggs 0:0529d2d7762f 619 // shown below.
lhiggs 0:0529d2d7762f 620 // magnetic field = register_data*0.000305176
lhiggs 0:0529d2d7762f 621
lhiggs 0:0529d2d7762f 622 // MAG_PROC_Z
lhiggs 0:0529d2d7762f 623 MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:0529d2d7762f 624 MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
lhiggs 0:0529d2d7762f 625 // pc.printf( "Mpz %f \n",MY_DATA_MAG_PROC_Z*0.000305176);
lhiggs 0:0529d2d7762f 626
lhiggs 0:0529d2d7762f 627 }
lhiggs 0:0529d2d7762f 628 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 629
lhiggs 0:0529d2d7762f 630
lhiggs 0:0529d2d7762f 631
lhiggs 0:0529d2d7762f 632 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 633 // UM6_EULER_PHI_THETA (0x62)
lhiggs 0:0529d2d7762f 634 // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
lhiggs 0:0529d2d7762f 635 // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
lhiggs 0:0529d2d7762f 636 // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
lhiggs 0:0529d2d7762f 637 // angle estimate = register_data* 0.0109863
lhiggs 0:0529d2d7762f 638 if (new_packet.address == UM6_EULER_PHI_THETA) {
lhiggs 0:0529d2d7762f 639 // EULER_PHI (ROLL)
lhiggs 0:0529d2d7762f 640 MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:0529d2d7762f 641 MY_DATA_EULER_PHI |= new_packet.packet_data[1];
lhiggs 0:0529d2d7762f 642 // pc.printf( "PHI %f deg , ",MY_DATA_EULER_PHI*0.0109863);
lhiggs 0:0529d2d7762f 643
lhiggs 0:0529d2d7762f 644
lhiggs 0:0529d2d7762f 645 // EULER_THETA (PITCH)
lhiggs 0:0529d2d7762f 646 MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:0529d2d7762f 647 MY_DATA_EULER_THETA |= new_packet.packet_data[3];
lhiggs 0:0529d2d7762f 648 // printf( "THETA %f deg , ",MY_DATA_EULER_THETA*0.0109863);
lhiggs 0:0529d2d7762f 649
lhiggs 0:0529d2d7762f 650 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 651
lhiggs 0:0529d2d7762f 652
lhiggs 0:0529d2d7762f 653
lhiggs 0:0529d2d7762f 654
lhiggs 0:0529d2d7762f 655 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 656 // UM6_EULER_PSI (0x63) (YAW)
lhiggs 0:0529d2d7762f 657 // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
lhiggs 0:0529d2d7762f 658 // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
lhiggs 0:0529d2d7762f 659 // should be multiplied by the scale factor 0.0109863 as shown below
lhiggs 0:0529d2d7762f 660
lhiggs 0:0529d2d7762f 661 MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:0529d2d7762f 662 MY_DATA_EULER_PSI |= new_packet.packet_data[5];
lhiggs 0:0529d2d7762f 663 // pc.printf( "PSI %f deg \r\n",MY_DATA_EULER_PSI*0.0109863);
lhiggs 0:0529d2d7762f 664
lhiggs 0:0529d2d7762f 665
lhiggs 0:0529d2d7762f 666 GOT_UM6 = true;
lhiggs 0:0529d2d7762f 667 UM6_data_counter++;
lhiggs 0:0529d2d7762f 668
lhiggs 0:0529d2d7762f 669 }
lhiggs 0:0529d2d7762f 670 //------------------------------------------------------------
lhiggs 0:0529d2d7762f 671
lhiggs 0:0529d2d7762f 672
lhiggs 0:0529d2d7762f 673 }
lhiggs 0:0529d2d7762f 674
lhiggs 0:0529d2d7762f 675 // A full packet has been received.
lhiggs 0:0529d2d7762f 676 // Put the USART back into the WAIT state and reset
lhiggs 0:0529d2d7762f 677 // the data_counter variable so that it can be used to receive the next packet.
lhiggs 0:0529d2d7762f 678 data_counter = 0;
lhiggs 0:0529d2d7762f 679 gUSART_State = USART_STATE_WAIT;
lhiggs 0:0529d2d7762f 680
lhiggs 0:0529d2d7762f 681 // ADDED: This is a counter for the SD data logging
lhiggs 0:0529d2d7762f 682 got_data_counter ++;
lhiggs 0:0529d2d7762f 683 got_data = false;
lhiggs 0:0529d2d7762f 684 }
lhiggs 0:0529d2d7762f 685 }
lhiggs 0:0529d2d7762f 686 break;
lhiggs 0:0529d2d7762f 687
lhiggs 0:0529d2d7762f 688 }
lhiggs 0:0529d2d7762f 689
lhiggs 0:0529d2d7762f 690 }
lhiggs 0:0529d2d7762f 691
lhiggs 0:0529d2d7762f 692
lhiggs 0:0529d2d7762f 693 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 694
lhiggs 0:0529d2d7762f 695
lhiggs 0:0529d2d7762f 696
lhiggs 0:0529d2d7762f 697 // LABVIEW REMOTE CONTROL ENABLE
lhiggs 0:0529d2d7762f 698 if (Control_Mode >= 0.5) {
lhiggs 0:0529d2d7762f 699
lhiggs 0:0529d2d7762f 700 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 701 /////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 702 // LABVIEW REMOTE CONTROL OF SERVOS AND ACTUATOR
lhiggs 0:0529d2d7762f 703 servo1.pulsewidth(servo1_pw); // Turn STBD
lhiggs 0:0529d2d7762f 704 servo2.pulsewidth(servo2_pw);
lhiggs 0:0529d2d7762f 705
lhiggs 0:0529d2d7762f 706 // ACTUATOR RETRACT
lhiggs 0:0529d2d7762f 707 if (Actuator_CMD < 0.5) {
lhiggs 0:0529d2d7762f 708 red = 1;
lhiggs 0:0529d2d7762f 709 EN = 1;
lhiggs 0:0529d2d7762f 710 blue = 0;
lhiggs 0:0529d2d7762f 711 }
lhiggs 0:0529d2d7762f 712
lhiggs 0:0529d2d7762f 713
lhiggs 0:0529d2d7762f 714 // ACTUATOR EXTEND
lhiggs 0:0529d2d7762f 715 if (Actuator_CMD >= 0.5) {
lhiggs 0:0529d2d7762f 716 red = 0;
lhiggs 0:0529d2d7762f 717 EN = 1;
lhiggs 0:0529d2d7762f 718 blue = 1;
lhiggs 0:0529d2d7762f 719 }
lhiggs 0:0529d2d7762f 720
lhiggs 0:0529d2d7762f 721 }
lhiggs 0:0529d2d7762f 722 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 723
lhiggs 0:0529d2d7762f 724
lhiggs 0:0529d2d7762f 725
lhiggs 0:0529d2d7762f 726 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 727 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 728 // THIS IS A SIMPLE TEST FOR CONTROLLING THE WINCH SERVOS FROM THE FLIGHT YAW ANGLE ESTIMATED FROM THE
lhiggs 0:0529d2d7762f 729 // ONBOARD UM6 IMU/AHRS.
lhiggs 0:0529d2d7762f 730 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 731
lhiggs 0:0529d2d7762f 732 if (GOT_UM6 == true) {
lhiggs 0:0529d2d7762f 733
lhiggs 0:0529d2d7762f 734
lhiggs 0:0529d2d7762f 735
lhiggs 0:0529d2d7762f 736
lhiggs 0:0529d2d7762f 737 // if (MY_DATA_EULER_PSI*0.0109863 < 5 && MY_DATA_EULER_PSI*0.0109863 > -5) {
lhiggs 0:0529d2d7762f 738 if (MY_DATA_EULER_PSI*0.0109863 < CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 > CALC_Heading-5) {
lhiggs 0:0529d2d7762f 739
lhiggs 0:0529d2d7762f 740 servo1.pulsewidth(0.0015); // NEUTRAL STBD
lhiggs 0:0529d2d7762f 741 servo2.pulsewidth(0.0015); // NEUTRAL PORT
lhiggs 0:0529d2d7762f 742
lhiggs 0:0529d2d7762f 743 }
lhiggs 0:0529d2d7762f 744
lhiggs 0:0529d2d7762f 745
lhiggs 0:0529d2d7762f 746
lhiggs 0:0529d2d7762f 747
lhiggs 0:0529d2d7762f 748 if (MY_DATA_EULER_PSI*0.0109863 <= CALC_Heading-5 && MY_DATA_EULER_PSI*0.0109863 >= -135) {
lhiggs 0:0529d2d7762f 749
lhiggs 0:0529d2d7762f 750 servo1.pulsewidth(0.0015 + 0.0002); // Turn STBD
lhiggs 0:0529d2d7762f 751 servo2.pulsewidth(0.0015);
lhiggs 0:0529d2d7762f 752
lhiggs 0:0529d2d7762f 753 // servo1.pulsewidth(0.0011);
lhiggs 0:0529d2d7762f 754
lhiggs 0:0529d2d7762f 755 // servo2.pulsewidth(0.0015 + 0.0002);
lhiggs 0:0529d2d7762f 756 //servo_cw_activity = !servo_cw_activity;
lhiggs 0:0529d2d7762f 757 }
lhiggs 0:0529d2d7762f 758
lhiggs 0:0529d2d7762f 759 if (MY_DATA_EULER_PSI*0.0109863 >= CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 <= 135) {
lhiggs 0:0529d2d7762f 760 servo1.pulsewidth(0.0015);
lhiggs 0:0529d2d7762f 761 servo2.pulsewidth(0.0015 - 0.0002); // Turn PORT
lhiggs 0:0529d2d7762f 762 //servo_ccw_activity = !servo_ccw_activity;
lhiggs 0:0529d2d7762f 763 }
lhiggs 0:0529d2d7762f 764
lhiggs 0:0529d2d7762f 765
lhiggs 0:0529d2d7762f 766
lhiggs 0:0529d2d7762f 767 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 768 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 769 // DEPLOY PARAFOIL (RETRACT ACTUATOR FOR DEMONSTRATION PURPOSES ONLY WHEN PITCH ABOVE 85 AND BELOW 95 DEG.
lhiggs 0:0529d2d7762f 770
lhiggs 0:0529d2d7762f 771 if (MY_DATA_EULER_THETA*0.0109863 > 75 ) {
lhiggs 0:0529d2d7762f 772 red = 1; // RETRACTS DEPLOYMENT ACTUATOR
lhiggs 0:0529d2d7762f 773 EN = 1;
lhiggs 0:0529d2d7762f 774 blue = 0;
lhiggs 0:0529d2d7762f 775
lhiggs 0:0529d2d7762f 776 }
lhiggs 0:0529d2d7762f 777
lhiggs 0:0529d2d7762f 778
lhiggs 0:0529d2d7762f 779 if (MY_DATA_EULER_THETA*0.0109863 <= -75) {
lhiggs 0:0529d2d7762f 780 red = 0; // RETRACTS DEPLOYMENT ACTUATOR
lhiggs 0:0529d2d7762f 781 EN = 1;
lhiggs 0:0529d2d7762f 782 blue = 1;
lhiggs 0:0529d2d7762f 783
lhiggs 0:0529d2d7762f 784 }
lhiggs 0:0529d2d7762f 785
lhiggs 0:0529d2d7762f 786 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 787
lhiggs 0:0529d2d7762f 788
lhiggs 0:0529d2d7762f 789
lhiggs 0:0529d2d7762f 790 GOT_UM6 = false;
lhiggs 0:0529d2d7762f 791 } // end if for GOT_EULER == true
lhiggs 0:0529d2d7762f 792
lhiggs 0:0529d2d7762f 793 /////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 794
lhiggs 0:0529d2d7762f 795
lhiggs 0:0529d2d7762f 796
lhiggs 0:0529d2d7762f 797
lhiggs 0:0529d2d7762f 798 /////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 799 /////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 800 // THIS IS A SIMPLE DATA OUTPUT USED TO TEST BASIC FUNCTIONALITY OF RF MODEM
lhiggs 0:0529d2d7762f 801 /////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 802
lhiggs 0:0529d2d7762f 803 if (UM6_data_counter >= 20) {
lhiggs 0:0529d2d7762f 804 UM6_data_counter = 0;
lhiggs 0:0529d2d7762f 805
lhiggs 0:0529d2d7762f 806 Roll = MY_DATA_EULER_PHI*0.0109863;
lhiggs 0:0529d2d7762f 807 Pitch = MY_DATA_EULER_THETA*0.0109863;
lhiggs 0:0529d2d7762f 808 Yaw = MY_DATA_EULER_PSI*0.0109863;
lhiggs 0:0529d2d7762f 809
lhiggs 0:0529d2d7762f 810
lhiggs 0:0529d2d7762f 811 Latitude = gps.latitude();
lhiggs 0:0529d2d7762f 812 Longitude = gps.longitude();
lhiggs 0:0529d2d7762f 813 Altitude = gps.altitude();
lhiggs 0:0529d2d7762f 814
lhiggs 0:0529d2d7762f 815
lhiggs 0:0529d2d7762f 816
lhiggs 0:0529d2d7762f 817
lhiggs 0:0529d2d7762f 818
lhiggs 0:0529d2d7762f 819 CALC_Heading = atan2 (cos(Latitude)*sin(CMD_Latitude)-sin(Latitude)*cos(CMD_Latitude)*cos(CMD_Longitude-CMD_Longitude),sin(CMD_Longitude-Longitude)*cos(CMD_Latitude)) * 180 / 3.14159265;
lhiggs 0:0529d2d7762f 820
lhiggs 0:0529d2d7762f 821
lhiggs 0:0529d2d7762f 822
lhiggs 0:0529d2d7762f 823
lhiggs 0:0529d2d7762f 824
lhiggs 0:0529d2d7762f 825
lhiggs 0:0529d2d7762f 826
lhiggs 0:0529d2d7762f 827
lhiggs 0:0529d2d7762f 828 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 829 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 830 // THIS IS FOR microSD CARD FLIGHT DATA LOGGING, NEEDS TO BE OPTIMIZED AND EXPANDED TO LOG ALL DATA AND
lhiggs 0:0529d2d7762f 831 // RECIEVED FLIGHT COMMANDS.
lhiggs 0:0529d2d7762f 832 //////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 833 // FILE *fp = fopen("/sd/UM6_DATA_GROUND.txt", "a");
lhiggs 0:0529d2d7762f 834 // if (fp == NULL) {
lhiggs 0:0529d2d7762f 835 // error("Could not open file for write\n");
lhiggs 0:0529d2d7762f 836 // }
lhiggs 0:0529d2d7762f 837
lhiggs 0:0529d2d7762f 838 // fprintf(fp,"%02d:%02d:%02d %02d/%02d/%04d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",
lhiggs 0:0529d2d7762f 839 // q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
lhiggs 0:0529d2d7762f 840 // gps.latitude(),gps.longitude(),gps.altitude(),
lhiggs 0:0529d2d7762f 841 // v1.velocity_knots(),v1.track_true(),
lhiggs 0:0529d2d7762f 842 // MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
lhiggs 0:0529d2d7762f 843 // MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
lhiggs 0:0529d2d7762f 844 // MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
lhiggs 0:0529d2d7762f 845 // MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
lhiggs 0:0529d2d7762f 846
lhiggs 0:0529d2d7762f 847 // fclose(fp);
lhiggs 0:0529d2d7762f 848
lhiggs 0:0529d2d7762f 849 sd_activity = !sd_activity;
lhiggs 0:0529d2d7762f 850 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 851
lhiggs 0:0529d2d7762f 852
lhiggs 0:0529d2d7762f 853
lhiggs 0:0529d2d7762f 854 // This sends the GPS:lat,long,alt telemetery to the ground base station
lhiggs 0:0529d2d7762f 855 // with the DNT900 900Mhz 1Watt RF modem.
lhiggs 0:0529d2d7762f 856 // DNT900P.printf("%02d:%02d:%02d %02d/%02d/%04d,\r\nLat %f,Long %f,Alt %f,Roll %f,Pitch %f,Yaw %f \n\r",
lhiggs 0:0529d2d7762f 857 // q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
lhiggs 0:0529d2d7762f 858 // gps.latitude(),gps.longitude(),gps.altitude(),
lhiggs 0:0529d2d7762f 859 // MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
lhiggs 0:0529d2d7762f 860
lhiggs 0:0529d2d7762f 861
lhiggs 0:0529d2d7762f 862
lhiggs 0:0529d2d7762f 863
lhiggs 0:0529d2d7762f 864
lhiggs 0:0529d2d7762f 865 // DNT900P.printf("%02d:%02d:%02d %02d/%02d/%04d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",
lhiggs 0:0529d2d7762f 866 // q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
lhiggs 0:0529d2d7762f 867 // gps.latitude(),gps.longitude(),gps.altitude(),
lhiggs 0:0529d2d7762f 868 // v1.velocity_knots(),v1.track_true(),
lhiggs 0:0529d2d7762f 869 // MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
lhiggs 0:0529d2d7762f 870 // MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
lhiggs 0:0529d2d7762f 871 // MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
lhiggs 0:0529d2d7762f 872 // MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
lhiggs 0:0529d2d7762f 873
lhiggs 0:0529d2d7762f 874
lhiggs 0:0529d2d7762f 875
lhiggs 0:0529d2d7762f 876 } // end UM6_data_counter >= 20 if statement
lhiggs 0:0529d2d7762f 877
lhiggs 0:0529d2d7762f 878 /////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:0529d2d7762f 879
lhiggs 0:0529d2d7762f 880
lhiggs 0:0529d2d7762f 881 } // end while(1) loop
lhiggs 0:0529d2d7762f 882
lhiggs 0:0529d2d7762f 883
lhiggs 0:0529d2d7762f 884 } // end main()
lhiggs 0:0529d2d7762f 885
lhiggs 0:0529d2d7762f 886