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
Diff: IMU/IMU.cpp
- Revision:
- 9:d5fcdcb3c89d
- Child:
- 10:085ab7328054
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU/IMU.cpp Fri Oct 20 11:41:22 2017 +0000 @@ -0,0 +1,78 @@ +#include "IMU.h" + +IMU::IMU(): _rs232(p13,p14) //maybe this should configurable on construction +{ + +} + +void IMU::initialize() +{ + //set up the spi bus and frequency + _rs232.baud(115200); + + // Define IMU packet type + unsigned char SYNC1 = 0x75; // First sync byte will always be 'u' (0x75) + unsigned char SYNC2 = 0x65; // Second sync byte will always be 'e' (0x65) + unsigned char descripter_set = 0x80; // Descriptor set byte for AHRS (0x80) + int payload_length = 0x0E; // Payload length byte for CF Euler Angles (0x0E) + int field_length = 0x0E; // Field length byte for CF Euler Angles (0x0E) + unsigned char data_descriptor = 0x0C; // Data descriptor byte for CF Euler Angles (0x0C) + unsigned char data[30]; // Data sent CF euler angles rpy [radians] + int data_offset = 6; // Binary offset + int roll_offset = 2; // Binary offset + int pitch_offset = 6; // Binary offset + int yaw_offset = 10; // Binary offset + float euler[3]; + + int i = 0;// set packet_length based on field_length (convert from hex to int) + unsigned char current = 0x00; + unsigned char last = 0x00; + +} + +//This starts an interupt driven trigger of the external ADC +void IMU::start() +{ + interval.attach(this, &IMU::update, 1); //this should be a 1 Hz sample rate +} + +//This stops it +void IMU::stop() +{ + interval.detach(); +} + + +void IMU::update() +{ + last = current; + current = _rs232.getc(); + if (last == SYNC1 && current == SYNC2){ // Detect beginning of packet + data[0] = last; + data[0] = current; + data[0] = _rs232.getc(); + data[0] = _rs232.getc(); + i = 0; + while(i < field_length){ // Get data + data[i] = _rs232.getc(); + i += 1; + } + data[i + data_offset + 1] = _rs232.getc(); // Check sum 1 + data[i + data_offset + 2] = _rs232.getc(); // Check sum 2 + } + euler[0] = float_from_char(&data[roll_offset])*180/_PI; // roll Euler angle convert to float + euler[1] = float_from_char(&data[pitch_offset])*180/_PI; // pitch Euler angle convert to float + euler[2] = float_from_char(&data[yaw_offset])*180/_PI; // yaw Euler angle convert to float + return ; +} + +float IMU::float_from_char(unsigned char * value) +{ + unsigned char temp[4]; + temp[0] = value[3]; + temp[1] = value[2]; + temp[2] = value[1]; + temp[3] = value[0]; + return *(float *) temp; +} +