update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
IMU/IMU.cpp
- Committer:
- mkelly10
- Date:
- 2017-10-20
- Revision:
- 9:d5fcdcb3c89d
- Child:
- 10:085ab7328054
File content as of revision 9:d5fcdcb3c89d:
#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; }