Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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.cpp@9:d5fcdcb3c89d, 2017-10-20 (annotated)

Committer:
mkelly10
Date:
Fri Oct 20 11:41:22 2017 +0000
Revision:
9:d5fcdcb3c89d
Child:
10:085ab7328054
Tested 10/19/17 Folders

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkelly10 9:d5fcdcb3c89d 1 #include "IMU.h"
mkelly10 9:d5fcdcb3c89d 2
mkelly10 9:d5fcdcb3c89d 3 IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction
mkelly10 9:d5fcdcb3c89d 4 {
mkelly10 9:d5fcdcb3c89d 5
mkelly10 9:d5fcdcb3c89d 6 }
mkelly10 9:d5fcdcb3c89d 7
mkelly10 9:d5fcdcb3c89d 8 void IMU::initialize()
mkelly10 9:d5fcdcb3c89d 9 {
mkelly10 9:d5fcdcb3c89d 10 //set up the spi bus and frequency
mkelly10 9:d5fcdcb3c89d 11 _rs232.baud(115200);
mkelly10 9:d5fcdcb3c89d 12
mkelly10 9:d5fcdcb3c89d 13 // Define IMU packet type
mkelly10 9:d5fcdcb3c89d 14 unsigned char SYNC1 = 0x75; // First sync byte will always be 'u' (0x75)
mkelly10 9:d5fcdcb3c89d 15 unsigned char SYNC2 = 0x65; // Second sync byte will always be 'e' (0x65)
mkelly10 9:d5fcdcb3c89d 16 unsigned char descripter_set = 0x80; // Descriptor set byte for AHRS (0x80)
mkelly10 9:d5fcdcb3c89d 17 int payload_length = 0x0E; // Payload length byte for CF Euler Angles (0x0E)
mkelly10 9:d5fcdcb3c89d 18 int field_length = 0x0E; // Field length byte for CF Euler Angles (0x0E)
mkelly10 9:d5fcdcb3c89d 19 unsigned char data_descriptor = 0x0C; // Data descriptor byte for CF Euler Angles (0x0C)
mkelly10 9:d5fcdcb3c89d 20 unsigned char data[30]; // Data sent CF euler angles rpy [radians]
mkelly10 9:d5fcdcb3c89d 21 int data_offset = 6; // Binary offset
mkelly10 9:d5fcdcb3c89d 22 int roll_offset = 2; // Binary offset
mkelly10 9:d5fcdcb3c89d 23 int pitch_offset = 6; // Binary offset
mkelly10 9:d5fcdcb3c89d 24 int yaw_offset = 10; // Binary offset
mkelly10 9:d5fcdcb3c89d 25 float euler[3];
mkelly10 9:d5fcdcb3c89d 26
mkelly10 9:d5fcdcb3c89d 27 int i = 0;// set packet_length based on field_length (convert from hex to int)
mkelly10 9:d5fcdcb3c89d 28 unsigned char current = 0x00;
mkelly10 9:d5fcdcb3c89d 29 unsigned char last = 0x00;
mkelly10 9:d5fcdcb3c89d 30
mkelly10 9:d5fcdcb3c89d 31 }
mkelly10 9:d5fcdcb3c89d 32
mkelly10 9:d5fcdcb3c89d 33 //This starts an interupt driven trigger of the external ADC
mkelly10 9:d5fcdcb3c89d 34 void IMU::start()
mkelly10 9:d5fcdcb3c89d 35 {
mkelly10 9:d5fcdcb3c89d 36 interval.attach(this, &IMU::update, 1); //this should be a 1 Hz sample rate
mkelly10 9:d5fcdcb3c89d 37 }
mkelly10 9:d5fcdcb3c89d 38
mkelly10 9:d5fcdcb3c89d 39 //This stops it
mkelly10 9:d5fcdcb3c89d 40 void IMU::stop()
mkelly10 9:d5fcdcb3c89d 41 {
mkelly10 9:d5fcdcb3c89d 42 interval.detach();
mkelly10 9:d5fcdcb3c89d 43 }
mkelly10 9:d5fcdcb3c89d 44
mkelly10 9:d5fcdcb3c89d 45
mkelly10 9:d5fcdcb3c89d 46 void IMU::update()
mkelly10 9:d5fcdcb3c89d 47 {
mkelly10 9:d5fcdcb3c89d 48 last = current;
mkelly10 9:d5fcdcb3c89d 49 current = _rs232.getc();
mkelly10 9:d5fcdcb3c89d 50 if (last == SYNC1 && current == SYNC2){ // Detect beginning of packet
mkelly10 9:d5fcdcb3c89d 51 data[0] = last;
mkelly10 9:d5fcdcb3c89d 52 data[0] = current;
mkelly10 9:d5fcdcb3c89d 53 data[0] = _rs232.getc();
mkelly10 9:d5fcdcb3c89d 54 data[0] = _rs232.getc();
mkelly10 9:d5fcdcb3c89d 55 i = 0;
mkelly10 9:d5fcdcb3c89d 56 while(i < field_length){ // Get data
mkelly10 9:d5fcdcb3c89d 57 data[i] = _rs232.getc();
mkelly10 9:d5fcdcb3c89d 58 i += 1;
mkelly10 9:d5fcdcb3c89d 59 }
mkelly10 9:d5fcdcb3c89d 60 data[i + data_offset + 1] = _rs232.getc(); // Check sum 1
mkelly10 9:d5fcdcb3c89d 61 data[i + data_offset + 2] = _rs232.getc(); // Check sum 2
mkelly10 9:d5fcdcb3c89d 62 }
mkelly10 9:d5fcdcb3c89d 63 euler[0] = float_from_char(&data[roll_offset])*180/_PI; // roll Euler angle convert to float
mkelly10 9:d5fcdcb3c89d 64 euler[1] = float_from_char(&data[pitch_offset])*180/_PI; // pitch Euler angle convert to float
mkelly10 9:d5fcdcb3c89d 65 euler[2] = float_from_char(&data[yaw_offset])*180/_PI; // yaw Euler angle convert to float
mkelly10 9:d5fcdcb3c89d 66 return ;
mkelly10 9:d5fcdcb3c89d 67 }
mkelly10 9:d5fcdcb3c89d 68
mkelly10 9:d5fcdcb3c89d 69 float IMU::float_from_char(unsigned char * value)
mkelly10 9:d5fcdcb3c89d 70 {
mkelly10 9:d5fcdcb3c89d 71 unsigned char temp[4];
mkelly10 9:d5fcdcb3c89d 72 temp[0] = value[3];
mkelly10 9:d5fcdcb3c89d 73 temp[1] = value[2];
mkelly10 9:d5fcdcb3c89d 74 temp[2] = value[1];
mkelly10 9:d5fcdcb3c89d 75 temp[3] = value[0];
mkelly10 9:d5fcdcb3c89d 76 return *(float *) temp;
mkelly10 9:d5fcdcb3c89d 77 }
mkelly10 9:d5fcdcb3c89d 78