most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
IMU/IMU.h
- Committer:
- joel_ssc
- Date:
- 2019-05-13
- Revision:
- 92:52a91656458a
- Parent:
- 66:0f20870117b7
File content as of revision 92:52a91656458a:
#include "mbed.h" #include "MODSERIAL.h" // for Microstrain's MIPS protocol, try this link, or search on microstrain.com // http://www.microstrain.com/sites/default/files/3dm-gx5-45_dcp_manual_8500-0064_0.pdf #define _PI ((float) 3.14159265359) // state machine states #define SYNC0 0 // first sync byte state #define SYNC1 1 // second sync byte state #define SET 2 // aka descriptor set #define LEN 3 // payload length #define PAY 4 // getting the payload data #define CRC0 5 // crc high byte #define CRC1 6 // crc low byte // data set descriptors #define IMU_DATA_SET 0x80 #define GNSS_DATA_SET 0x81 // enumerations for the Euler angle packet we care about #define EULER_CF_LENGTH 14 #define EULER_CF_DESCRIPTOR 0x0C #define ROLL_OFFSET 0 #define PITCH_OFFSET 4 #define YAW_OFFSET 8 // enumerations for the lat-lon-alt packet we care about #define LLH_POSITION_LENGTH 44 #define LLH_POSITION_DESCRIPTOR 0x03 #define LATITUDE_OFFSET 0 #define LONGITUDE_OFFSET 8 #define HEIGHT_MSL_OFFSET 16 #define VALID_FLAG_OFFSET 40 // enumerations for the lat-lon-alt packet we care about #define GNSS_FIX_INFO_LENGTH 8 #define GNSS_FIX_INFO_DESCRIPTOR 0x0B #define FIX_TYPE_OFFSET 0 #define NUM_SV_OFFSET 1 #define VALID_FLAGS_OFFSET 4 class IMU { public: IMU(PinName Tx, PinName Rx); void initialize(); void update(); void start(); void stop(); void runIMU(); float getRoll(); float getPitch(); float getHeading(); bool getIsValid2dFix(); bool getIsValid3dFix(); char getNumSV(); double getLatitude(); double getLongitude(); double getAltitudeMSL(); protected: Ticker interval; MODSERIAL _rs232; char byte; unsigned char state, len, descriptor, i, packet[256]; unsigned int checksum, crc0, crc1; /* unsigned char state, len, descriptor, i, crc0, crc1, payload[30]; unsigned int checksum;*/ float euler[3]; double latLonAlt[3]; bool is2dFixValid; bool is3dFixValid; char numSV; void processPayload(char type, char length, unsigned char * payload); void processEulerCfPacket(char length, unsigned char * payload); void processLatLonAltPacket(char length, unsigned char * payload); void processGnssFixInformation(char length, unsigned char * payload); unsigned int calcChecksum(unsigned char * mip_packet, char checksum_range); float floatFromChar(unsigned char * value); double doubleFromChar(unsigned char * value); };