lhiggs CSUM
Capstone project for Bachelor's in Mechanical Engineering 2011
Dependencies: FatFileSystem MAX3100 MODGPS MODSERIAL SDFileSystem mbed
Revision 0:0529d2d7762f, committed 2013-05-29
- Comitter:
- lhiggs
- Date:
- Wed May 29 00:45:41 2013 +0000
- Commit message:
- Broken, after updating all the libraries. RPC has issues with new mbed libraries.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufSerialRPC/RPCFunction.cpp Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,115 @@
+ /**
+* @section LICENSE
+*Copyright (c) 2010 ARM Ltd.
+*
+*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 THE
+*AUTHORS OR COPYRIGHT HOLDERS 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.
+*
+* @section Description
+*This class provides an object which can be called over RPC to run the function which is attached to it.
+*
+*/
+#include "RPCFunction.h"
+#include "rpc.h"
+
+//Parse a char argument without delimiting by anything that is non alphanumeric - based on version in rpc.h line 153
+ char *parse_arg_char(const char *arg, const char **next) {
+ const char *ptr = arg;
+ char *res = NULL;
+ if(*arg == '"') {
+ /* quoted string */
+ ptr = ++arg;
+ int len = 0;
+ /* find the end (and length) of the quoted string */
+ for(char c = *ptr; c != 0 && c != '"'; c = *++ptr) {
+ len++;
+ if(c == '\\') {
+ ptr++;
+ }
+ }
+ /* copy the quoted string, and unescape characters */
+ if(len != 0) {
+ res = new char[len+1];
+ char *resptr = res;
+ while(arg != ptr) {
+ *resptr++ = parse_char(arg, &arg);
+ }
+ *resptr = 0;
+ }
+ } else {
+ /* unquoted string */
+ while(isalnum(*ptr) || isgraph(*ptr) || *ptr=='_' || *ptr == ' ') { //Edit this line to change which types of characters are allowed and which delimit
+ ptr++;
+ }
+ int len = ptr-arg;
+ if(len!=0) { //Chnages made to just pass whole string with no next arg or delimiters, these changes just removes space at the beginning
+ res = new char[len]; //was len+1
+ memcpy(res, arg + 1, len - 1); // was arg, len
+ res[len-1] = 0; //was len
+ }
+ }
+
+ if(next != NULL) {
+ *next = ptr;
+ }
+ return res;
+ }
+
+ //Custom rpc method caller for execute so that the string will not be delimited by anything
+ //See line 436 of rpc.h
+ void rpc_method_caller_run(Base *this_ptr, const char *arguments, char *result) {
+
+ const char *next = arguments;
+ char* arg1 = parse_arg_char(next,NULL);
+
+ char * res = (static_cast<RPCFunction*>(this_ptr)->run)(arg1);
+ if(result != NULL) {
+ write_result<char*>(res, result);
+ }
+ delete arg1; // Seems to stop a memory leak issue //From Hexley Ball
+ }
+
+ RPCFunction::RPCFunction(void(*f)(char*, char*), const char* name) : Base(name){
+ _ftr = f;
+ }
+
+
+ //Just run the attached function using the string thats in private memory - or just using null values,
+ char * RPCFunction::run(char * input){
+ strcpy(_input, input);
+ (*_ftr)(_input,_output);
+ return(_output);
+ }
+
+ //Just read the output string
+ char* RPCFunction::read(){
+ return(_output);
+ }
+
+
+ #ifdef MBED_RPC
+ const rpc_method *RPCFunction::get_rpc_methods() {
+ static const rpc_method rpc_methods[] = {
+ { "run", rpc_method_caller_run }, //Run using custom caller, all characters accepted in string
+ { "read", rpc_method_caller<char*, RPCFunction, &RPCFunction::read> },
+ RPC_METHOD_SUPER(Base)
+ };
+ return rpc_methods;
+ }
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufSerialRPC/RPCFunction.h Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,84 @@
+ /**
+* @section LICENSE
+*Copyright (c) 2010 ARM Ltd.
+*
+*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 THE
+*AUTHORS OR COPYRIGHT HOLDERS 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.
+*
+* @section Description
+*This class provides an object which can be called over RPC to run the function which is attached to it.
+*
+*/
+#ifndef RPCFUNCTION_RPC
+#define RPCFUNCTION_RPC
+/**
+*Includes
+*/
+#include "mbed.h"
+#include "platform.h"
+#include "rpc.h"
+#define STR_LEN 64
+#include "platform.h"
+
+#ifdef MBED_RPC
+#include "rpc.h"
+#endif
+/**
+*
+*Class to call custom functions over RPC
+*
+*/
+class RPCFunction : public Base{
+public:
+ /**
+ * Constructor
+ *
+ *@param f Pointer to the function to call. the function must be of the form void foo(char * input, char * output)
+ *@param name The name of this object
+ */
+ RPCFunction(void(*f)(char*, char*), const char* = NULL);
+
+ /**
+ *run
+ *
+ *Calls the attached function passing the string in but doesn't return the result.
+ *@param str The string to be passed into the attached function. This string can consist of any ASCII characters apart from escape codes. The usual limtations on argument content for RPC strings has been removed
+ *@return A string output from the function
+ */
+ char * run(char* str);
+
+ /**
+ *Reads the value of the output string.
+ *
+ *@returns the string outputted from the last time the function was called
+ */
+ char * read();
+
+
+ #ifdef MBED_RPC
+ virtual const struct rpc_method *get_rpc_methods();
+ #endif
+
+private:
+ void (*_ftr)(char*, char*);
+
+ char _input[STR_LEN];
+ char _output[STR_LEN];
+
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufSerialRPC/RPCVariable.h Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,102 @@
+ /**
+* @section LICENSE
+*Copyright (c) 2010 ARM Ltd.
+*
+*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 THE
+*AUTHORS OR COPYRIGHT HOLDERS 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.
+*
+* @section Description
+*This class provides an object to which a variable can be attached. Any type
+*for which a parse_args function specilisation exists can be attached. This includes
+*all of the standard types.
+*
+*/
+ #ifndef RPCVARIABLE_H_
+ #define RPCVARIABLE_H_
+
+#include "mbed.h"
+#include "platform.h"
+#include "rpc.h"
+ /**
+ *Class to read and set an attached variable using the RPC
+ *
+ */
+template<class T>
+class RPCVariable : public Base{
+public:
+ /**
+ * Constructor
+ *
+ *@param ptr Pointer to the variable to make accessible over RPC. Any type of
+ *variable can be connected
+ *@param name The name of that this object will be over RPC
+ */
+ template<class A>
+ RPCVariable(A * ptr, const char * name) : Base(name){
+ _ptr = ptr;
+ }
+ /**
+ *Read the variable over RPC.
+ *
+ *@return The value of the variable
+ */
+ T read(){
+ return(*_ptr);
+ }
+ /**
+ *Write a value to the variable over RPC
+ *
+ *@param The value to be written to the attached variable.
+ */
+ void write(T value){
+ *_ptr = value;
+ }
+
+ #ifdef MBED_RPC
+ virtual const struct rpc_method *get_rpc_methods();
+ static struct rpc_class *get_rpc_class();
+ #endif
+
+private:
+ T * _ptr;
+
+};
+
+//Set up RPC methods
+#ifdef MBED_RPC
+template <class T>
+ const rpc_method *RPCVariable<T>::get_rpc_methods() {
+ static const rpc_method rpc_methods[] = {
+ { "read", rpc_method_caller<T, RPCVariable, &RPCVariable::read> },
+ { "write", rpc_method_caller<RPCVariable, T, &RPCVariable::write> },
+ RPC_METHOD_SUPER(Base)
+ };
+ return rpc_methods;
+ }
+ template <class T>
+ rpc_class *RPCVariable<T>::get_rpc_class() {
+ static const rpc_function funcs[] = {"new", rpc_function_caller<const char*, T,const char* , &Base::construct<RemoteVar, T ,const char*> >,RPC_METHOD_END};
+ static rpc_class c = { "RPCVariable", funcs, NULL };
+ return &c;
+ }
+#endif
+
+//There could be specialisation for integer, to also give increment and decrements
+
+
+#endif //RPCVARIABLE_H_
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufSerialRPC/SerialRPCInterface.cpp Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,77 @@
+ /**
+* @section LICENSE
+*Copyright (c) 2010 ARM Ltd.
+*
+*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 THE
+*AUTHORS OR COPYRIGHT HOLDERS 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.
+*
+*
+* @section DESCRIPTION
+*
+*This class sets up RPC communication. This allows objects on mbed to be controlled. Objects can be created or existing objects can be used
+*/
+#include "SerialRPCInterface.h"
+
+using namespace mbed;
+
+//Requires multiple contstructors for each type, serial to set different pin numbers, TCP for port.
+SerialRPCInterface::SerialRPCInterface(PinName tx, PinName rx, int baud):pc(tx, rx) {
+ _RegClasses();
+ _enabled = true;
+ if (baud != 9600) pc.baud(baud);
+ pc.attach(this, &SerialRPCInterface::_RPCSerial, MODSERIAL::RxAutoDetect);
+ pc.autoDetectChar('\n'); //autoDetectChar
+ pc.rxBufferFlush();
+ }
+
+void SerialRPCInterface::_RegClasses(void){
+ //Register classes with base
+ Base::add_rpc_class<AnalogIn>();
+ Base::add_rpc_class<AnalogOut>();
+ Base::add_rpc_class<DigitalIn>();
+ Base::add_rpc_class<DigitalOut>();
+ Base::add_rpc_class<DigitalInOut>();
+ Base::add_rpc_class<PwmOut>();
+ Base::add_rpc_class<Timer>();
+ Base::add_rpc_class<BusOut>();
+ Base::add_rpc_class<BusIn>();
+ Base::add_rpc_class<BusInOut>();
+ Base::add_rpc_class<Serial>();
+}
+
+void SerialRPCInterface::Disable(void){
+ _enabled = false;
+ }
+void SerialRPCInterface::Enable(void){
+ _enabled = true;
+}
+void SerialRPCInterface::_MsgProcess(void) {
+ if(_enabled == true){
+ rpc(_command, _response);
+ }
+}
+
+void SerialRPCInterface::_RPCSerial() {
+ _RPCflag = true;
+ if(_enabled == true) {
+ pc.gets(_command, 256);
+ _MsgProcess();
+ pc.printf("%s\n", _response);
+ }
+ _RPCflag = false;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufSerialRPC/SerialRPCInterface.h Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,91 @@
+/**
+* @section LICENSE
+*Copyright (c) 2010 ARM Ltd.
+*
+*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 THE
+*AUTHORS OR COPYRIGHT HOLDERS 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.
+*
+*
+* @section DESCRIPTION
+*
+*This class sets up RPC communication over serial.
+*/
+#ifndef INTERFACE
+#define INTERFACE
+
+/**
+*Includes
+*/
+#include "mbed.h"
+#include "platform.h"
+#include "rpc.h"
+#include "RPCFunction.h"
+#include "RPCVariable.h"
+#include "MODSERIAL.h"
+
+
+namespace mbed{
+/**
+*Provides an Interface to mbed over RPC.
+*
+*For the chosen communication type this class sets up the necessary interrupts to receive RPC messages. Receives the messages, passes them to the rpc function and then returns the result.
+*/
+class SerialRPCInterface{
+public:
+ /**
+ *Constructor
+ *
+ *Sets up RPC communication using serial communication.
+ *
+ *@param tx The transmit pin of the serial port.
+ *@param rx The receive pin of the serial port.
+ *@param baud Set the baud rate, default is 9600.
+ */
+ SerialRPCInterface(PinName tx, PinName rx, int baud = 9600);
+
+ /**
+ *Disable the RPC.
+ *
+ *This will stop RPC messages being recevied and interpreted by this library. This might be used to prevent RPC commands interrupting an important piece of code on mbed.
+ */
+ void Disable(void);
+
+ /**
+ *Enable the RPC
+ *
+ *This will set this class to receiving and executing RPC commands. The class starts in this mode so this function only needs to be called if you have previosuly disabled the RPC.
+ *
+ */
+ void Enable(void);
+
+ //The Serial Port
+ MODSERIAL pc;
+
+
+private:
+ //Handle messgaes and take appropriate action
+ void _MsgProcess(void);
+ void _RegClasses(void);
+ void _RPCSerial();
+ bool _enabled;
+ char _command[256];
+ char _response[256];
+ bool _RPCflag;
+};
+}
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FATFileSystem.lib Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/FatFileSystem/#333d6e93e58f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MAX3100.lib Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MAX3100/#2a49171453d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS.lib Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODGPS/#64771e31464e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/SDFileSystem/#c8f66dc765d4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UM6_config.h Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,209 @@
+/* ------------------------------------------------------------------------------
+ File: UM6_config.h
+ Author: CH Robotics
+ Version: 1.0
+
+ Description: Preprocessor definitions and function declarations for UM6 configuration
+------------------------------------------------------------------------------ */
+#ifndef __UM6_CONFIG_H
+#define __UM6_CONFIG_H
+
+//#include "CHR_matrix.h"
+#include "UM6_usart.h"
+
+
+// CONFIG_ARRAY_SIZE and DATA_ARRAY_SIZE specify the number of 32 bit configuration and data registers used by the firmware
+// (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a
+// microcontroller register. They are simply index locations into arrays stored in global memory. Data and configuration
+// parameters are stored in arrays because it allows a common communication protocol to be used to access all data and
+// configuration. The software communicating with the sensor needs only specify the register address, and the communication
+// software running on the sensor knows exactly where to find it - it needn't know what the data is. The software communicatin
+// with the sensor, on the other hand, needs to know what it is asking for (naturally...)
+// This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in
+// the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side.
+#define CONFIG_ARRAY_SIZE 44
+#define DATA_ARRAY_SIZE 33
+#define COMMAND_COUNT 9
+
+//
+#define CONFIG_REG_START_ADDRESS 0
+#define DATA_REG_START_ADDRESS 85
+#define COMMAND_START_ADDRESS 170
+
+// These preprocessor definitions make it easier to access specific configuration parameters in code
+// They specify array locations associated with each register name. Note that in the comments below, many of the values are
+// said to be 32-bit IEEE floating point. Obviously this isn't directly the case, since the arrays are actually 32-bit unsigned
+// integer arrays. Bit for bit, the data does correspond to the correct floating point value. Since you can't cast ints as floats,
+// special conversion has to happen to copy the float data to and from the array.
+// Starting with configuration register locations...
+#define UM6_COMMUNICATION CONFIG_REG_START_ADDRESS // Stores settings in individual bits
+#define UM6_MISC_CONFIG (CONFIG_REG_START_ADDRESS + 1) // Stores settings in individual bits
+#define UM6_MAG_REF_X (CONFIG_REG_START_ADDRESS + 2) // Mag reference values are stored as 32-bit IEEE floating point values (these reflect data AFTER scale factors and compensation are applied)
+#define UM6_MAG_REF_Y (CONFIG_REG_START_ADDRESS + 3)
+#define UM6_MAG_REF_Z (CONFIG_REG_START_ADDRESS + 4)
+#define UM6_ACCEL_REF_X (CONFIG_REG_START_ADDRESS + 5) // Accel reference values are stored as 32-bit IEEE floating point values (these reflect data AFTER scale factors and compensation are applied)
+#define UM6_ACCEL_REF_Y (CONFIG_REG_START_ADDRESS + 6)
+#define UM6_ACCEL_REF_Z (CONFIG_REG_START_ADDRESS + 7)
+#define UM6_EKF_MAG_VARIANCE (CONFIG_REG_START_ADDRESS + 8) // Variances are stored as 32-bit IEEE floating point values.
+#define UM6_EKF_ACCEL_VARIANCE (CONFIG_REG_START_ADDRESS + 9)
+#define UM6_EKF_PROCESS_VARIANCE (CONFIG_REG_START_ADDRESS + 10)
+#define UM6_GYRO_BIAS_XY (CONFIG_REG_START_ADDRESS + 11) // Gyro biases are stored as 16-bit signed integers. Bias correction is applied BEFORE scale factors are applied
+#define UM6_GYRO_BIAS_Z (CONFIG_REG_START_ADDRESS + 12)
+#define UM6_ACCEL_BIAS_XY (CONFIG_REG_START_ADDRESS + 13) // Accel biases are stored as 16-bit signed integers. Bias correction is applied BEFORE scale factors are applied
+#define UM6_ACCEL_BIAS_Z (CONFIG_REG_START_ADDRESS + 14)
+#define UM6_MAG_BIAS_XY (CONFIG_REG_START_ADDRESS + 15) // Mag biases are stored as 16-bit signed integers. Bias correction is applied BEFORE magnetometer adjustment
+#define UM6_MAG_BIAS_Z (CONFIG_REG_START_ADDRESS + 16)
+#define UM6_ACCEL_CAL_00 (CONFIG_REG_START_ADDRESS + 17) // The accelerometer alignment matrix is a 3x3 matrix with 32-bit IEEE floating point entries
+#define UM6_ACCEL_CAL_01 (CONFIG_REG_START_ADDRESS + 18)
+#define UM6_ACCEL_CAL_02 (CONFIG_REG_START_ADDRESS + 19)
+#define UM6_ACCEL_CAL_10 (CONFIG_REG_START_ADDRESS + 20)
+#define UM6_ACCEL_CAL_11 (CONFIG_REG_START_ADDRESS + 21)
+#define UM6_ACCEL_CAL_12 (CONFIG_REG_START_ADDRESS + 22)
+#define UM6_ACCEL_CAL_20 (CONFIG_REG_START_ADDRESS + 23)
+#define UM6_ACCEL_CAL_21 (CONFIG_REG_START_ADDRESS + 24)
+#define UM6_ACCEL_CAL_22 (CONFIG_REG_START_ADDRESS + 25)
+#define UM6_GYRO_CAL_00 (CONFIG_REG_START_ADDRESS + 26) // The gyro alignment matrix is a 3x3 matrix with 32-bit IEEE floating point entries
+#define UM6_GYRO_CAL_01 (CONFIG_REG_START_ADDRESS + 27)
+#define UM6_GYRO_CAL_02 (CONFIG_REG_START_ADDRESS + 28)
+#define UM6_GYRO_CAL_10 (CONFIG_REG_START_ADDRESS + 29)
+#define UM6_GYRO_CAL_11 (CONFIG_REG_START_ADDRESS + 30)
+#define UM6_GYRO_CAL_12 (CONFIG_REG_START_ADDRESS + 31)
+#define UM6_GYRO_CAL_20 (CONFIG_REG_START_ADDRESS + 32)
+#define UM6_GYRO_CAL_21 (CONFIG_REG_START_ADDRESS + 33)
+#define UM6_GYRO_CAL_22 (CONFIG_REG_START_ADDRESS + 34)
+#define UM6_MAG_CAL_00 (CONFIG_REG_START_ADDRESS + 35) // The magnetometer calibration matrix is a 3x3 matrix with 32-bit IEEE floating point entries
+#define UM6_MAG_CAL_01 (CONFIG_REG_START_ADDRESS + 36)
+#define UM6_MAG_CAL_02 (CONFIG_REG_START_ADDRESS + 37)
+#define UM6_MAG_CAL_10 (CONFIG_REG_START_ADDRESS + 38)
+#define UM6_MAG_CAL_11 (CONFIG_REG_START_ADDRESS + 39)
+#define UM6_MAG_CAL_12 (CONFIG_REG_START_ADDRESS + 40)
+#define UM6_MAG_CAL_20 (CONFIG_REG_START_ADDRESS + 41)
+#define UM6_MAG_CAL_21 (CONFIG_REG_START_ADDRESS + 42)
+#define UM6_MAG_CAL_22 (CONFIG_REG_START_ADDRESS + 43)
+
+// Now for data register locations.
+// In the communication protocol, data registers are labeled with number ranging from 128 to 255. The value of 128 will be subtracted from these numbers
+// to produce the actual array index labeled below
+#define UM6_STATUS DATA_REG_START_ADDRESS // Status register defines error codes with individual bits
+#define UM6_GYRO_RAW_XY (DATA_REG_START_ADDRESS + 1) // Raw gyro data is stored in 16-bit signed integers
+#define UM6_GYRO_RAW_Z (DATA_REG_START_ADDRESS + 2)
+#define UM6_ACCEL_RAW_XY (DATA_REG_START_ADDRESS + 3) // Raw accel data is stored in 16-bit signed integers
+#define UM6_ACCEL_RAW_Z (DATA_REG_START_ADDRESS + 4)
+#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
+#define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6)
+#define UM6_GYRO_PROC_XY (DATA_REG_START_ADDRESS + 7) // Processed gyro data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
+#define UM6_GYRO_PROC_Z (DATA_REG_START_ADDRESS + 8)
+#define UM6_ACCEL_PROC_XY (DATA_REG_START_ADDRESS + 9) // Processed accel data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
+#define UM6_ACCEL_PROC_Z (DATA_REG_START_ADDRESS + 10)
+#define UM6_MAG_PROC_XY (DATA_REG_START_ADDRESS + 11) // Processed mag data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
+#define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12)
+#define UM6_EULER_PHI_THETA (DATA_REG_START_ADDRESS + 13) // Euler angles are 32-bit IEEE floating point
+#define UM6_EULER_PSI (DATA_REG_START_ADDRESS + 14)
+#define UM6_QUAT_AB (DATA_REG_START_ADDRESS + 15) // Quaternions are 16-bit signed integers.
+#define UM6_QUAT_CD (DATA_REG_START_ADDRESS + 16)
+#define UM6_ERROR_COV_00 (DATA_REG_START_ADDRESS + 17) // Error covariance is a 4x4 matrix of 32-bit IEEE floating point values
+#define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18)
+#define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19)
+#define UM6_ERROR_COV_03 (DATA_REG_START_ADDRESS + 20)
+#define UM6_ERROR_COV_10 (DATA_REG_START_ADDRESS + 21)
+#define UM6_ERROR_COV_11 (DATA_REG_START_ADDRESS + 22)
+#define UM6_ERROR_COV_12 (DATA_REG_START_ADDRESS + 23)
+#define UM6_ERROR_COV_13 (DATA_REG_START_ADDRESS + 24)
+#define UM6_ERROR_COV_20 (DATA_REG_START_ADDRESS + 25)
+#define UM6_ERROR_COV_21 (DATA_REG_START_ADDRESS + 26)
+#define UM6_ERROR_COV_22 (DATA_REG_START_ADDRESS + 27)
+#define UM6_ERROR_COV_23 (DATA_REG_START_ADDRESS + 28)
+#define UM6_ERROR_COV_30 (DATA_REG_START_ADDRESS + 29)
+#define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30)
+#define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31)
+#define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32)
+
+// Finally, define some non-register registers... sometimes commands must be sent to the sensor - commands that don't involve the transmission of any data.
+// Like, for example, a command to zero rate gyros. Or whatever. These commands are given "register" addresses so that they can be sent using the same
+// communication framework used to set and read registers. The only difference is that when a command is received and no data is attached, the communication
+// code doesn't set any registers.
+//
+// The communication code will do two things for every packet received:
+// 1. Copy data to the relevant register if data was provided in the packet
+// 2. Call a "dispatch packet" function that performs additional functions if the packet requires it.
+// Step 2 is what handles commands and causes status packets to be returned.
+#define UM6_GET_FW_VERSION COMMAND_START_ADDRESS // Causes the UM6 to report the firmware revision
+#define UM6_FLASH_COMMIT (COMMAND_START_ADDRESS + 1) // Causes the UM6 to write all configuration values to FLASH
+#define UM6_ZERO_GYROS (COMMAND_START_ADDRESS + 2) // Causes the UM6 to start a zero gyros command
+#define UM6_RESET_EKF (COMMAND_START_ADDRESS + 3) // Causes the UM6 to reset the EKF
+#define UM6_GET_DATA (COMMAND_START_ADDRESS + 4) // Causes the UM6 to transmit a data packet containing data from all enabled channels
+#define UM6_SET_ACCEL_REF (COMMAND_START_ADDRESS + 5) // Causes the UM6 to set the current measured accel data to the reference vector
+#define UM6_SET_MAG_REF (COMMAND_START_ADDRESS + 6) // Causes the UM6 to set the current measured magnetometer data to the reference vector
+#define UM6_RESET_TO_FACTORY (COMMAND_START_ADDRESS + 7) // Causes the UM6 to load default factory settings
+
+#define UM6_SAVE_FACTORY (COMMAND_START_ADDRESS + 8) // Causes the UM6 to save the current settings to the factory flash location
+
+#define UM6_USE_CONFIG_ADDRESS 0
+#define UM6_USE_FACTORY_ADDRESS 1
+
+#define UM6_BAD_CHECKSUM 253 // Sent if the UM6 receives a packet with a bad checksum
+#define UM6_UNKNOWN_ADDRESS 254 // Sent if the UM6 receives a packet with an unknown address
+#define UM6_INVALID_BATCH_SIZE 255 // Sent if a requested batch read or write operation would go beyond the bounds of the config or data array
+
+// Now make even more definitions for writing data to specific registers
+// Start with the UM6_COMMUNICATION register. These definitions specify what individual bits in the regist mean
+#define UM6_BROADCAST_ENABLED (1 << 30) // Enable serial data transmission
+#define UM6_GYROS_RAW_ENABLED (1 << 29) // Enable transmission of raw gyro data
+#define UM6_ACCELS_RAW_ENABLED (1 << 28) // Enable transmission of raw accelerometer data
+#define UM6_MAG_RAW_ENABLED (1 << 27) // Enable transmission of raw magnetometer data
+#define UM6_GYROS_PROC_ENABLED (1 << 26) // Enable transmission of processed gyro data (biases removed, scale factor applied, rotation correction applied)
+#define UM6_ACCELS_PROC_ENABLED (1 << 25) // Enable transmission of processed accel data (biases removed, scale factor applied, rotation correction applied)
+#define UM6_MAG_PROC_ENABLED (1 << 24) // Enable transmission of processed mag data (biases removed, scale factor applied, rotation correction applied)
+#define UM6_QUAT_ENABLED (1 << 23) // Enable transmission of quaternion data
+#define UM6_EULER_ENABLED (1 << 22) // Enable transmission of euler angle data
+#define UM6_COV_ENABLED (1 << 21) // Enable transmission of state covariance data
+
+#define UM6_BAUD_RATE_MASK (0x07) // Mask specifying the number of bits used to set the serial baud rate
+#define UM6_BAUD_START_BIT 8 // Specifies the start location of the baud rate bits
+
+#define UM6_SERIAL_RATE_MASK (0x000FF) // Mask specifying which bits in this register are used to indicate the broadcast frequency
+
+// MISC Configuration register
+#define UM6_MAG_UPDATE_ENABLED (1 << 31) // Enable magnetometer-based updates in the EKF
+#define UM6_ACCEL_UPDATE_ENABLED (1 << 30) // Enable accelerometer-based updates in the EKF
+#define UM6_GYRO_STARTUP_CAL (1 << 29) // Enable automatic gyro calibration on startup
+#define UM6_QUAT_ESTIMATE_ENABLED (1 << 28) // Enable quaternion-based state estimation
+#define UM6_PPS_ENABLED (1 << 27) // Enable PPS support (requires connection to external PPS signal)
+
+// UM6 Status Register
+#define UM6_MAG_INIT_FAILED (1 << 31) // Indicates magnetometer initialization failed
+#define UM6_ACCEL_INIT_FAILED (1 << 30) // Indicates accelerometer initialization failed
+#define UM6_GYRO_INIT_FAILED (1 << 29) // Indicates gyro initialization failed
+#define UM6_GYRO_ST_FAILED_X (1 << 28) // Indicates that the x-axis gyro self test failed
+#define UM6_GYRO_ST_FAILED_Y (1 << 27) // Indicates that the y-axis gyro self test failed
+#define UM6_GYRO_ST_FAILED_Z (1 << 26) // Indicates that the z-axis gyro self test failed
+#define UM6_ACCEL_ST_FAILED_X (1 << 25) // Indicates that the x-axis accel self test failed
+#define UM6_ACCEL_ST_FAILED_Y (1 << 24) // Indicates that the y-axis accel self test failed
+#define UM6_ACCEL_ST_FAILED_Z (1 << 23) // Indicates that the z-axis accel self test failed
+#define UM6_MAG_ST_FAILED_X (1 << 22) // Indicates that the x-axis mag self test failed
+#define UM6_MAG_ST_FAILED_Y (1 << 21) // Indicates that the y-axis mag self test failed
+#define UM6_MAG_ST_FAILED_Z (1 << 20) // Indicates that the z-axis mag self test failed
+#define UM6_I2C_GYRO_BUS_ERROR (1 << 19) // Indicates that there was an i2c bus error while communicating with the rate gyros
+#define UM6_I2C_ACCEL_BUS_ERROR (1 << 18) // Indicates that there was an i2c bus error while communicating with the accelerometers
+#define UM6_I2C_MAG_BUS_ERROR (1 << 17) // Indicates that there was an i2c bus error while communicating with the magnetometer
+#define UM6_EKF_DIVERGENT (1 << 16) // Indicates that the EKF estimate failed and had to be restarted
+#define UM6_GYRO_UNRESPONSIVE (1 << 15) // Inidicates that the rate gyros failed to signal new data for longer than expected
+#define UM6_ACCEL_UNRESPONSIVE (1 << 14) // Indicates that the accelerometer failed to signal new data for longer than expected
+#define UM6_MAG_UNRESPONSIVE (1 << 13) // Indicates that the magnetometer failed to signal new data for longer than expected
+#define UM6_FLASH_WRITE_FAILED (1 << 12) // Indicates that a write to flash command failed to complete properly
+
+#define UM6_SELF_TEST_COMPLETE (1 << 0) // Indicates that a self-test was completed
+
+
+#define GYRO_ZERO_SAMPLE_SIZE 500
+
+typedef struct __UM6_config {
+ uint32_t r[CONFIG_ARRAY_SIZE];
+} UM6_config;
+
+typedef struct __UM6_data {
+ uint32_t r[DATA_ARRAY_SIZE];
+} UM6_data;
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UM6_usart.h Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,81 @@
+/* ______________________________________________________________________________________
+ File: UM6_usart.h
+ Author: CH Robotics, adapted for mbed by lhiggs
+ Version: 1.0
+
+ Description: Function declarations for USART communucation
+ -------------------------------------------------------------------------------------- */
+
+ #include "mbed.h"
+
+ #ifndef _CHR_USART_H
+ #define _CHR_USART_H
+
+#define MAX_PACKET_DATA 40
+
+
+ // Definitions of states for USART receiver state machine (for receiving packets)
+ #define USART_STATE_WAIT 1
+ #define USART_STATE_TYPE 2
+ #define USART_STATE_ADDRESS 3
+ #define USART_STATE_DATA 4
+ #define USART_STATE_CHECKSUM 5
+
+
+
+ // Flags for interpreting the packet type byte in communication packets
+ #define PACKET_HAS_DATA (1 << 7)
+ #define PACKET_IS_BATCH (1 << 6)
+ #define PACKET_BATCH_LENGTH_MASK ( 0x0F )
+
+ #define PACKET_BATCH_LENGTH_OFFSET 2
+
+ #define BATCH_SIZE_2 2
+ #define BATCH_SIZE_3 3
+
+ #define PACKET_NO_DATA 0
+ #define PACKET_COMMAND_FAILED (1 << 0)
+
+
+ // Define flags for identifying the type of packet address received
+ #define ADDRESS_TYPE_CONFIG 0
+ #define ADDRESS_TYPE_DATA 1
+ #define ADDRESS_TYPE_COMMAND 2
+
+
+// Union for working with floats at the byte level
+//typedef union _fconvert
+//{
+// uint32_t uint32_val;
+// int32_t int32_val;
+// float float_val;
+// } fConvert;
+
+
+
+ // Buffer, buffer index, and TX status flag for USART transmit
+
+extern uint8_t gUSART_State;
+
+ // Structure for storing TX and RX packet data
+ typedef struct _USARTPacket
+ {
+ uint8_t PT; // Packet type
+ uint8_t address; // Packet address
+ uint16_t checksum; // Checksum
+
+ // Data included for convenience, but that isn't stored in the packet itself
+ uint8_t data_length; // Number of bytes in data section
+ uint8_t address_type; // Specified the address type (DATA, CONFIG, OR COMMAND)
+
+ uint8_t packet_data[MAX_PACKET_DATA];
+
+ } USARTPacket;
+
+
+
+uint16_t ComputeChecksum( USARTPacket* new_packet );
+
+
+
+ #endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed May 29 00:45:41 2013 +0000
@@ -0,0 +1,886 @@
+#include "mbed.h" // MBED LIBRARY
+
+#include "SerialRPCInterface.h" // MBED RPC LIBRARY (FOR LABVIEW CONTROL OF PROGRAM FUNCTIONS AND VARIABLES)
+
+#include "GPS.h" // MBED GPS LIBRARY
+
+#include "MODSERIAL.h" // MBED BUFFERED SERIAL
+#include "SDFileSystem.h" // MBED SD LIBRARY
+
+
+#include "UM6_usart.h" // UM6 USART HEADER
+#include "UM6_config.h" // UM6 CONFIG HEADER
+
+
+#include "MAX3100.h" // MAX3100 SPI to UART CHIP DRIVER LIBRARY
+
+#include "math.h"
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+// Program Name: main.cpp
+// Description: This is the main program for an autonomous parafoil undergraduate capstone
+// project in Mechanical Engineering at the California Maritime Academy, Vallejo
+// CA, Spring 2011. As of yet there is no autonomous functionality. However, all
+// hardware has proven functional. The hardware that this program was written for
+// consists of an mbed LPC1768 microcontroller, cool components mbed breakout board
+// with 1GB microSD card, GM862-GPS combined cellular and gps module, DNT900P
+// 900Mhz 1Watt RF modem utilized as a transparent serial link to the application,
+// Hitec HS-785 Winch Servos, Firgelli PQ12 linear actuator with limit switches,
+// and last but by no means least a CH Robotics UM6 IMU/AHRS.
+// Author: Logan Higgs, (portions of UM6 interface adapted from open source UM6 code by CH
+// robotics}
+// Date: 04/2011
+// Special Thanks to Andy Kirkham from mbed who wrote most of the mbed libraries that this
+// program utilizes, and more importantly even edited his MODGPS library to support VTG GPS
+// data specifically from my request.
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS FOR RECIEVED DATA COUNTING AND BOOLING GOT NEW DATA FLAGS
+/////////////////////////////////////////////////////////////////////////////////////////////
+static uint8_t got_data_counter = 0;
+
+volatile bool got_data = false;
+volatile bool GOT_UM6 = false;
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS FOR ATTEMPT TO MAKE UM6 DATA ARRAYS FOR LETTING microSD CARD
+// DATA WRITING ONLY ONCE A SECOND, INSTEAD OF EVERY TIME A UM6 DATA
+// SET IS RECIEVED
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+int UM6_data_counter = 0; // Counter for SD data logging
+
+int16_t MY_DATA_GYRO_PROC_X;
+int16_t MY_DATA_GYRO_PROC_Y;
+int16_t MY_DATA_GYRO_PROC_Z;
+int16_t MY_DATA_ACCEL_PROC_X;
+int16_t MY_DATA_ACCEL_PROC_Y;
+int16_t MY_DATA_ACCEL_PROC_Z;
+int16_t MY_DATA_MAG_PROC_X;
+int16_t MY_DATA_MAG_PROC_Y;
+int16_t MY_DATA_MAG_PROC_Z;
+int16_t MY_DATA_EULER_PHI;
+int16_t MY_DATA_EULER_THETA;
+int16_t MY_DATA_EULER_PSI;
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+// SETUP (ASSIGN) SERIAL COMMUNICATION PINS ON MBED
+/////////////////////////////////////////////////////////////////////////////////////////////
+//MODSERIAL pc(USBTX, USBRX); // PC SERIAL OVER USB PORT ON MBED
+//Create the interface on the USB Serial Port
+//SerialRPCInterface SerialInterface(USBTX, USBRX);
+SerialRPCInterface SerialInterface(p28, p27);
+
+
+MODSERIAL uart(p26, p25); // UM6 SERIAL OVER UART PINS 28 & 27
+
+// SETUP DNT900P RF MODEM SERIAL
+//MODSERIAL DNT900P(p28, p27);
+
+// SETUP GM862-GPS MODEM SERIAL PORT THRU MAX3100 SPI to UART IC
+MAX3100 max(p11, p12, p13, p14,p15);
+
+// SETUP GM862-GPS GPS SERIAL PORT
+// Serial gps_uart(p9, p10);
+
+// Create an instance of the GPS object for MODGPS library
+GPS gps(NC, p10);
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// SETUP (ASSIGN) SPI COMMUNICATION PINS ON MBED FOR SD CARD ON COOLCOMPONENTS WORKSHOP BOARD
+////////////////////////////////////////////////////////////////////////////////////////////////
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// SETUP (ASSIGN) WINCH SERVO#1 (STBD) & WINCH SERVO#2 (PORT)
+////////////////////////////////////////////////////////////////////////////////////////////////
+PwmOut servo1(p23);
+PwmOut servo2(p24);
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// SETUP (ASSIGN) DIGITAL OUTPUTS FOR H-BRIDGE ACTUATOR DRIVER FOR PARAFOIL DEPLOYMENT
+////////////////////////////////////////////////////////////////////////////////////////////////
+DigitalOut red(p16);
+DigitalOut EN(p17);
+DigitalOut blue(p18);
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// SETUP (ASSIGN) MBED LED (1 thru 3) FOR VISUAL DEBUGGING ON MBED
+////////////////////////////////////////////////////////////////////////////////////////////////
+DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
+DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
+DigitalOut sd_activity(LED3); // LED3 = SD CARD
+
+
+
+// CREATE FLOAT VARIABLES FOR LABVIEW REMOTE CONTROL OF PROGRAM
+float servo1_pw = 0.0015;
+float servo2_pw = 0.0015;
+float Actuator_CMD = 1;
+float Latitude = 0;
+float Longitude = 0;
+float Altitude = 0;
+float Roll = 0;
+float Pitch = 0;
+float Yaw = 0;
+float Control_Mode = 0;
+float CMD_Latitude = 0;
+float CMD_Longitude = 0;
+float CALC_Heading = 0;
+float CMD_Heading = 0;
+
+
+// FROM UM6 FIRMWARE SOURCE
+
+// Flag for storing the current USART state
+uint8_t gUSART_State = USART_STATE_WAIT;
+
+/*******************************************************************************
+* Function Name : ComputeChecksum
+* Input : USARTPacket* new_packet
+* Output : None
+* Return : uint16_t
+* Description : Returns the two byte sum of all the individual bytes in the
+ given packet.
+*******************************************************************************/
+uint16_t ComputeChecksum( USARTPacket* new_packet ) {
+ int32_t index;
+ uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
+
+ for ( index = 0; index < new_packet->data_length; index++ ) {
+ checksum += new_packet->packet_data[index];
+ }
+ return checksum;
+}
+
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+// MODSERIAL INTERUPT FUNCTION AND MODSERIAL OVERFLOW INTERUPT FUNCTION FOR DEBUGGING
+/////////////////////////////////////////////////////////////////////////////////////////////
+
+// MBED MODSERIAL INTERUPT FUNCTION
+// This function is called when a character goes into the RX buffer.
+//void rxCallback(void) {
+// if (uart.rxBufferGetCount() > MAX_PACKET_DATA & got_data == false) {
+// got_data = true;
+// uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
+// }
+//}
+
+void rxOvflow(void) {
+ error("Ouch, overflowed");
+}
+
+//SDFileSystem *sd;
+///////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+int main() {
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+//Make these variables accessible over RPC by attaching them to an RPCVariable
+ RPCVariable<float> RPCservo1_pw(&servo1_pw, "servo1_pw");
+ RPCVariable<float> RPCservo2_pw(&servo2_pw, "servo2_pw");
+ RPCVariable<float> RPCActuator_CMD(&Actuator_CMD, "Actuator_CMD");
+
+ RPCVariable<float> RPCLatitude(&Latitude, "Latitude");
+ RPCVariable<float> RPCLongitude(&Longitude, "Longitude");
+ RPCVariable<float> RPCAltitude(&Altitude, "Altitude");
+
+ RPCVariable<float> RPCRoll(&Roll, "Roll");
+ RPCVariable<float> RPCPitch(&Pitch, "Pitch");
+ RPCVariable<float> RPCYaw(&Yaw, "Yaw");
+
+ RPCVariable<float> RPCControl_Mode(&Control_Mode, "Control_Mode");
+
+ RPCVariable<float> RPCCMD_Latitude(&CMD_Latitude, "CMD_Latitude");
+ RPCVariable<float> RPCCMD_Longitude(&CMD_Longitude, "CMD_Lognitude");
+
+ RPCVariable<float> RPCCALC_Heading(&CALC_Heading, "CALC_Heading");
+ RPCVariable<float> RPCCMD_Heading(&CMD_Heading, "CMD_Heading");
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+ GPS_Time q1;
+ GPS_VTG v1;
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+// WINCH SERVO INITIALIZATION
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+ servo1.period(0.020); // servos requires a 20ms period
+ servo2.period(0.020);
+
+ // initialize servo to center (neutral position)
+ servo1.pulsewidth(0.0015);
+ servo2.pulsewidth(0.0015);
+
+ //servo_neutral_activity = !servo_neutral_activity;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS MAKES A DIRECTORY ON THE microSD CARD FOR DATA LOGGING
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+ // SD DATA LOGGING
+ // mkdir("/sd/", 0777);
+ // sd_activity = !sd_activity;
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+// SET SERIAL UART BAUD RATES
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ // set UM6 serial uart baud 115200
+ uart.baud(9600);
+
+
+ // pc bauds for usb virtual serial port on mbed for ground use only.
+ // pc.baud(9600); // pc baud for GM862-GPS modem to pc interface
+// pc.baud(9600); // pc baud for UM6 to pc interface
+
+ // set DNT900P baud 9600
+ // DNT900P.baud(9600);
+
+ // set gps baud 4800
+ gps.baud(4800);
+ gps.format(8, GPS::None, 1);
+
+
+ // enable max3100 interupts for GM862-GPS modem uart thru spi to uart ic
+ max.enableRxIrq();
+ max.enableTxIrq();
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS WAS FOR DEBUGGING MODSERIAL AND SD CARD, STILL USEFUL FOR CHECKING FOR OVERFLOW OF MODSERIAL
+// CIRCULAR BUFFER
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+ // In main after setting the uart baud rate and before attaching your rxCallback function add:-
+ uart.attach(&rxOvflow, MODSERIAL::RxOvIrq);
+ // Setup the uart serial port, then...
+ // sd = new SDFileSystem(p5, p6, p7, p8, "sd");
+
+ // attach interupt function to uart
+ // uart.attach(&rxCallback, MODSERIAL::RxIrq);
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+ while (1) {
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+// GPS AUTOMATIC DATA PARSING WITH MODGPS LIBRARY
+ // Demonstrate how to find out the GPS location co-ords.
+ // pc.printf("Lat = %.4f ", gps.latitude());
+ // pc.printf("Lon = %.4f ", gps.longitude());
+ // pc.printf("Alt = %.4f ", gps.altitude());
+
+ // Gran a snapshot of the current time.
+ gps.timeNow(&q1);
+ // pc.printf("%02d:%02d:%02d %02d/%02d/%04d\r\n",
+ // q1.hour, q1.minute, q1.second, q1.day, q1.month, q1.year);
+
+ gps.vtg(&v1);
+ // pc.printf("Method 1. Vector data, Speed (knots):%lf, Speed(kph):%lf, Track(true):%lf, Track(mag)%lf\r\n",
+ // v1.velocity_knots(), v1.velocity_kph(), v1.track_true(), v1.track_mag());
+ //
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS A SERIAL PASS THRU BETWEEN THE GM862-GPS MODEM SERIAL PORT AND THE PC USB VIRTUAL SERIAL PORT.
+// IT ISN'T INTENDED TO BE USED DURING FLIGHT, BUT RATHER FOR CONFIGURATION OF THE GM862-GPS WHILE ON
+// THE GROUND.
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ // if (pc.readable()) {
+ // int c = pc.getc();
+ // max.putc(c);
+ // }
+
+ // if (max.readable()) {
+ // pc.putc( max.getc() );
+ // }
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS A SERIAL PASS THRU FOR THE UM6 AHRS/IMU TO THE PC FOR USING THE CH ROBOTICS SERIAL INTERFACE
+// PC SOFTWARE FOR CALIBRATING AND VIEWING DATA FROM THE UM6. DEFAULT PC BAUD 115200
+ // if (pc.readable()) {
+ // int c = pc.getc();
+ // uart.putc(c);
+ // }
+
+ // if (uart.readable()) {
+ // pc.putc( uart.getc() );
+ // }
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS FOR THE UM6 IMU/AHRS SERIAL COMMUNICATIONS, IT REALLY SHOULD BE PUT INTO AT LEAST ITS OWN
+// FUNCTIONS, OR EVEN A UM6 CLASS AND LIBRARY.
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ if (uart.rxBufferGetCount() > MAX_PACKET_DATA) {
+
+
+
+ uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
+
+ // UM6 Firmware Source Function ProcessNextCharacter()
+ //void ProcessNextCharacter( ) {
+ static uint8_t data_counter = 0;
+
+ static USARTPacket new_packet;
+
+ // The next action should depend on the USART state.
+ switch ( gUSART_State ) {
+ // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
+ // that signals a new incoming packet.
+ case USART_STATE_WAIT:
+ if ( data_counter == 0 ) { // Waiting on 's' character
+ if ( uart.getc() == 's' ) {
+
+ data_counter++;
+ } else {
+ data_counter = 0;
+ }
+ } else if ( data_counter == 1 ) { // Waiting on 'n' character
+ if ( uart.getc() == 'n' ) {
+ data_counter++;
+
+ } else {
+ data_counter = 0;
+ }
+ } else if ( data_counter == 2 ) { // Waiting on 'p' character
+ if ( uart.getc() == 'p' ) {
+ // The full 'snp' sequence was received. Reset data_counter (it will be used again
+ // later) and transition to the next state.
+ data_counter = 0;
+ gUSART_State = USART_STATE_TYPE;
+
+ } else {
+ data_counter = 0;
+ }
+ }
+ break;
+
+ // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
+ // indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
+ case USART_STATE_TYPE:
+
+ new_packet.PT = uart.getc();
+
+ gUSART_State = USART_STATE_ADDRESS;
+
+ break;
+
+ // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
+ // the address that the packet applies to
+ case USART_STATE_ADDRESS:
+ new_packet.address = uart.getc();
+ // pc.putc(new_packet.address);
+
+ // For convenience, identify the type of packet this is and copy to the packet structure
+ // (this will be used by the packet handler later)
+ if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
+ new_packet.address_type = ADDRESS_TYPE_CONFIG;
+ } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
+ new_packet.address_type = ADDRESS_TYPE_DATA;
+ } else {
+ new_packet.address_type = ADDRESS_TYPE_COMMAND;
+ }
+
+ // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
+ // If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet
+ if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
+ gUSART_State = USART_STATE_CHECKSUM;
+ }
+
+ // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
+ else {
+ gUSART_State = USART_STATE_DATA;
+ // Determine the expected number of bytes in this data packet based on the packet type. A write operation
+ // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
+ // where the batch size is also given in the packet type byte.
+ if ( new_packet.PT & PACKET_IS_BATCH ) {
+ new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
+
+ } else {
+ new_packet.data_length = 4;
+ }
+ }
+ break;
+
+ // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
+ // data.
+ case USART_STATE_DATA:
+ new_packet.packet_data[data_counter] = uart.getc();
+ data_counter++;
+
+ // If the expected number of bytes has been received, transition to the CHECKSUM state.
+ if ( data_counter == new_packet.data_length ) {
+ // Reset data_counter, since it will be used in the CHECKSUM state.
+ data_counter = 0;
+ gUSART_State = USART_STATE_CHECKSUM;
+ }
+ break;
+
+
+
+ // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
+ // of the 16-bit checksum.
+ case USART_STATE_CHECKSUM:
+ // Get the highest-order byte
+ if ( data_counter == 0 ) {
+ // uint8_t Temp_Packet = uart.getc();
+ new_packet.checksum = ((uint16_t)uart.getc() << 8);
+ data_counter++;
+ } else { // ( data_counter == 1 )
+ // Get lower-order byte
+ new_packet.checksum = new_packet.checksum | ((uint16_t)uart.getc() & 0x0FF);
+
+ // Both checksum bytes have been received. Make sure that the checksum is valid.
+ uint16_t checksum = ComputeChecksum( &new_packet );
+
+
+
+ // If checksum does not match, send a BAD_CHECKSUM packet
+ if ( checksum != new_packet.checksum ) {
+ got_data = false;
+ }
+
+ else
+
+ {
+
+ // Packet was received correctly.
+
+ //-----------------------------------------------------------------------------------------------
+ //-----------------------------------------------------------------------------------------------
+ //
+ // ADDED: CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
+
+
+ // IF DATA ADDRESS
+ if (new_packet.address_type == ADDRESS_TYPE_DATA) {
+
+ //------------------------------------------------------------
+ // UM6_GYRO_PROC_XY (0x5C)
+ // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
+ // per second, the data should be multiplied by the scale factor 0.0610352 as shown below
+ // angular_rate = register_data*0.0610352
+
+ if (new_packet.address == UM6_GYRO_PROC_XY) {
+
+ // GYRO_PROC_X
+ MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
+ // pc.printf( "GPx %f deg/s\n",MY_DATA_GYRO_PROC_X*0.0610352);
+
+
+ MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
+ // pc.printf( "GPy %f d/s\n",MY_DATA_GYRO_PROC_Y*0.0610352);
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_GYRO_PROC_Z (0x5D)
+ // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
+ // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
+
+
+ // GYRO_PROC_Z
+ MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];
+ //pc.printf( "GPz %f deg/s\n",MY_DATA_GYRO_PROC_Z*0.0610352);
+ }
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_ACCEL_PROC_XY (0x5E)
+ // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
+ // the data should be multiplied by the scale factor 0.000183105 as shown below.
+ // acceleration = register_data* 0.000183105
+ if (new_packet.address == UM6_ACCEL_PROC_XY) {
+
+ // ACCEL_PROC_X
+ MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
+ // pc.printf( "Apx %f g \n",MY_DATA_ACCEL_PROC_X*0.000183105);
+
+ // ACCEL_PROC_Y
+ MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
+ // pc.printf( "Apy %f g \n",MY_DATA_ACCEL_PROC_Y*0.000183105);
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_ACCEL_PROC_Z (0x5F)
+ // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
+ // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
+
+ // ACCEL_PROC_Z
+ MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
+ // pc.printf( "Apz %f g \n",MY_DATA_ACCEL_PROC_Z*0.000183105);
+ }
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_MAG_PROC_XY (0x60)
+ // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
+ // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
+ // shown below.
+ // magnetic field = register_data* 0.000305176
+ if (new_packet.address == UM6_MAG_PROC_XY) {
+
+ // MAG_PROC_X
+ MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
+ // pc.printf( "Mpx %f \n",MY_DATA_MAG_PROC_X*0.000305176);
+
+ // MAG_PROC_Y
+ MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
+ // pc.printf( "Mpy %f \n",MY_DATA_MAG_PROC_Y*0.000305176);
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_MAG_PROC_Z (0x61)
+ // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
+ // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
+ // shown below.
+ // magnetic field = register_data*0.000305176
+
+ // MAG_PROC_Z
+ MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
+ // pc.printf( "Mpz %f \n",MY_DATA_MAG_PROC_Z*0.000305176);
+
+ }
+ //------------------------------------------------------------
+
+
+
+ //------------------------------------------------------------
+ // UM6_EULER_PHI_THETA (0x62)
+ // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
+ // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
+ // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
+ // angle estimate = register_data* 0.0109863
+ if (new_packet.address == UM6_EULER_PHI_THETA) {
+ // EULER_PHI (ROLL)
+ MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_EULER_PHI |= new_packet.packet_data[1];
+ // pc.printf( "PHI %f deg , ",MY_DATA_EULER_PHI*0.0109863);
+
+
+ // EULER_THETA (PITCH)
+ MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_EULER_THETA |= new_packet.packet_data[3];
+ // printf( "THETA %f deg , ",MY_DATA_EULER_THETA*0.0109863);
+
+ //------------------------------------------------------------
+
+
+
+
+ //------------------------------------------------------------
+ // UM6_EULER_PSI (0x63) (YAW)
+ // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
+ // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
+ // should be multiplied by the scale factor 0.0109863 as shown below
+
+ MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_EULER_PSI |= new_packet.packet_data[5];
+ // pc.printf( "PSI %f deg \r\n",MY_DATA_EULER_PSI*0.0109863);
+
+
+ GOT_UM6 = true;
+ UM6_data_counter++;
+
+ }
+ //------------------------------------------------------------
+
+
+ }
+
+ // A full packet has been received.
+ // Put the USART back into the WAIT state and reset
+ // the data_counter variable so that it can be used to receive the next packet.
+ data_counter = 0;
+ gUSART_State = USART_STATE_WAIT;
+
+ // ADDED: This is a counter for the SD data logging
+ got_data_counter ++;
+ got_data = false;
+ }
+ }
+ break;
+
+ }
+
+ }
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+// LABVIEW REMOTE CONTROL ENABLE
+if (Control_Mode >= 0.5) {
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+// LABVIEW REMOTE CONTROL OF SERVOS AND ACTUATOR
+ servo1.pulsewidth(servo1_pw); // Turn STBD
+ servo2.pulsewidth(servo2_pw);
+
+ // ACTUATOR RETRACT
+ if (Actuator_CMD < 0.5) {
+ red = 1;
+ EN = 1;
+ blue = 0;
+ }
+
+
+ // ACTUATOR EXTEND
+ if (Actuator_CMD >= 0.5) {
+ red = 0;
+ EN = 1;
+ blue = 1;
+ }
+
+}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS A SIMPLE TEST FOR CONTROLLING THE WINCH SERVOS FROM THE FLIGHT YAW ANGLE ESTIMATED FROM THE
+// ONBOARD UM6 IMU/AHRS.
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ if (GOT_UM6 == true) {
+
+
+
+
+ // if (MY_DATA_EULER_PSI*0.0109863 < 5 && MY_DATA_EULER_PSI*0.0109863 > -5) {
+if (MY_DATA_EULER_PSI*0.0109863 < CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 > CALC_Heading-5) {
+
+ servo1.pulsewidth(0.0015); // NEUTRAL STBD
+ servo2.pulsewidth(0.0015); // NEUTRAL PORT
+
+ }
+
+
+
+
+ if (MY_DATA_EULER_PSI*0.0109863 <= CALC_Heading-5 && MY_DATA_EULER_PSI*0.0109863 >= -135) {
+
+ servo1.pulsewidth(0.0015 + 0.0002); // Turn STBD
+ servo2.pulsewidth(0.0015);
+
+ // servo1.pulsewidth(0.0011);
+
+ // servo2.pulsewidth(0.0015 + 0.0002);
+ //servo_cw_activity = !servo_cw_activity;
+ }
+
+ if (MY_DATA_EULER_PSI*0.0109863 >= CALC_Heading+5 && MY_DATA_EULER_PSI*0.0109863 <= 135) {
+ servo1.pulsewidth(0.0015);
+ servo2.pulsewidth(0.0015 - 0.0002); // Turn PORT
+ //servo_ccw_activity = !servo_ccw_activity;
+ }
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+// DEPLOY PARAFOIL (RETRACT ACTUATOR FOR DEMONSTRATION PURPOSES ONLY WHEN PITCH ABOVE 85 AND BELOW 95 DEG.
+
+ if (MY_DATA_EULER_THETA*0.0109863 > 75 ) {
+ red = 1; // RETRACTS DEPLOYMENT ACTUATOR
+ EN = 1;
+ blue = 0;
+
+ }
+
+
+ if (MY_DATA_EULER_THETA*0.0109863 <= -75) {
+ red = 0; // RETRACTS DEPLOYMENT ACTUATOR
+ EN = 1;
+ blue = 1;
+
+ }
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+ GOT_UM6 = false;
+ } // end if for GOT_EULER == true
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS A SIMPLE DATA OUTPUT USED TO TEST BASIC FUNCTIONALITY OF RF MODEM
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+ if (UM6_data_counter >= 20) {
+ UM6_data_counter = 0;
+
+Roll = MY_DATA_EULER_PHI*0.0109863;
+Pitch = MY_DATA_EULER_THETA*0.0109863;
+Yaw = MY_DATA_EULER_PSI*0.0109863;
+
+
+ Latitude = gps.latitude();
+ Longitude = gps.longitude();
+ Altitude = gps.altitude();
+
+
+
+
+
+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;
+
+
+
+
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+// THIS IS FOR microSD CARD FLIGHT DATA LOGGING, NEEDS TO BE OPTIMIZED AND EXPANDED TO LOG ALL DATA AND
+// RECIEVED FLIGHT COMMANDS.
+//////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // FILE *fp = fopen("/sd/UM6_DATA_GROUND.txt", "a");
+ // if (fp == NULL) {
+ // error("Could not open file for write\n");
+ // }
+
+ // 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",
+ // q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
+ // gps.latitude(),gps.longitude(),gps.altitude(),
+ // v1.velocity_knots(),v1.track_true(),
+ // MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
+ // MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
+ // MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
+ // MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
+
+ // fclose(fp);
+
+ sd_activity = !sd_activity;
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+// This sends the GPS:lat,long,alt telemetery to the ground base station
+// with the DNT900 900Mhz 1Watt RF modem.
+ // DNT900P.printf("%02d:%02d:%02d %02d/%02d/%04d,\r\nLat %f,Long %f,Alt %f,Roll %f,Pitch %f,Yaw %f \n\r",
+ // q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
+ // gps.latitude(),gps.longitude(),gps.altitude(),
+ // MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
+
+
+
+
+
+// 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",
+// q1.hour, q1.minute,q1.second,q1.month,q1.day,q1.year,
+// gps.latitude(),gps.longitude(),gps.altitude(),
+// v1.velocity_knots(),v1.track_true(),
+// MY_DATA_GYRO_PROC_X*0.0610352,MY_DATA_GYRO_PROC_Y*0.0610352,MY_DATA_GYRO_PROC_Z*0.0610352,
+// MY_DATA_ACCEL_PROC_X*0.000183105,MY_DATA_ACCEL_PROC_Y*0.000183105,MY_DATA_ACCEL_PROC_Z*0.000183105,
+// MY_DATA_MAG_PROC_X*0.000305176,MY_DATA_MAG_PROC_Y*0.000305176,MY_DATA_MAG_PROC_Z*0.000305176,
+// MY_DATA_EULER_PHI*0.0109863,MY_DATA_EULER_THETA*0.0109863,MY_DATA_EULER_PSI*0.0109863);
+
+
+
+ } // end UM6_data_counter >= 20 if statement
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+ } // end while(1) loop
+
+
+} // end main()
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed May 29 00:45:41 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17 \ No newline at end of file