Andy Rodriguez
/
UM7_USMA
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: UM6.cpp.txt
- Revision:
- 5:a3b249e68e44
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UM6.cpp.txt Tue Aug 25 21:13:06 2020 +0000
@@ -0,0 +1,214 @@
+#include "UM6.h"
+
+Serial um6serial(A0, A1);
+//Serial pc6(USBTX, USBRX);
+//Serial pc6(PC_10, PC_11);
+
+int16_t pitchRaw = 0;//divide by 91.02222
+int16_t rollRaw = 0;
+int16_t yawRaw = 0;
+int16_t pitchDotRaw = 0;//divide by 16
+int16_t rollDotRaw = 0;
+int16_t yawDotRaw = 0;
+int16_t accXRaw = 0; //idk
+int16_t accYRaw = 0;
+int16_t accZRaw = 0;
+
+float eulerTime = 0;
+double pitch = 0.0;
+double roll = 0.0;
+double yaw = 0.0;
+double rollDot = 0.0;
+double pitchDot = 0.0;
+double yawDot = 0.0;
+double accX = 0.0;
+double accY = 0.0;
+double accZ = 0.0;
+
+const int packet_length = 7+4*8;
+uint8_t rx_data6[packet_length];
+int iPack = 0;
+
+// parse_serial_data.This function parses the data in ‘rx_data6’ with length ‘rx_length’ and attempts to find a packet in the data. If a packet is found, the structure ‘packet’ is filled with the packet data.If there is not enough data for a full packet in the provided array, parse_serial_data returns 1. If there is enough data, but no packet header was found, parse_serial_data returns 2.If a packet header was found, but there was insufficient data to parse the whole packet,then parse_serial_data returns 3. This could happen if not all of the serial data has been received when parse_serial_data is called.If a packet was received, but the checksum was bad, parse_serial_data returns 4. If a good packet was received, parse_serial_data fills the UM6_packet structure and returns 0.
+uint8_t parse_serial_data( uint8_t* rx_data6, uint8_t rx_length, UM6_packet* packet ){
+ uint8_t index;
+
+ // Make sure that the data buffer provided is long enough to contain a full packet The minimum packet length is 7 bytes
+ if( rx_length < 7 ){
+ return 1;
+ }
+
+ // Try to find the ‘snp’ start sequence for the packet
+ for( index = 0; index < (rx_length - 2); index++ ){
+ // Check for 'snp'. If found, immediately exit the loop
+ if( rx_data6[index] == 's' && rx_data6[index+1] == 'n' && rx_data6[index+2] == 'p' ){
+ break;
+ }
+ }
+
+ uint8_t packet_index = index;
+
+ // Check to see if the variable ‘packet_index’ is equal to (rx_length - 2). If it is, then the above loop executed to completion and never found a packet header.
+ if( packet_index == (rx_length - 2) )
+ {
+ return 2;
+ }
+
+ // If we get here, a packet header was found. Now check to see if we have enough room left in the buffer to contain a full packet. Note that at this point, the variable ‘packet_index’contains the location of the ‘s’ character in the buffer (the first byte in the header)
+ if( (rx_length - packet_index) < 7 )
+ {
+ return 3;
+ }
+
+ // We’ve found a packet header, and there is enough space left in the buffer for at least the smallest allowable packet length (7 bytes). Pull out the packet type byte to determine the actual length of this packet
+ uint8_t PT = rx_data6[packet_index + 3];
+
+ // Do some bit-level manipulation to determine if the packet contains data and if it is a batch.We have to do this because the individual bits in the PT byte specify the contents of the packet.
+ uint8_t packet_has_data = (PT >> 7) & 0x01; // Check bit 7 (HAS_DATA)
+ uint8_t packet_is_batch = (PT >> 6) & 0x01; // Check bit 6 (IS_BATCH)
+ uint8_t batch_length = (PT >> 2) & 0x0F; // Extract the batch length (bits 2 through 5)
+
+ // Now finally figure out the actual packet length
+ uint8_t data_length = 0;
+
+ if( packet_has_data )
+ {
+ if( packet_is_batch )
+ {
+ // Packet has data and is a batch. This means it contains ‘batch_length' registers, each // of which has a length of 4 bytes
+ data_length = 4*batch_length;
+ }
+ else // Packet has data but is not a batch. This means it contains one register (4 bytes)
+ {
+ data_length = 4;
+ }
+ }
+ else // Packet has no data
+ {
+ data_length = 0;
+ }
+
+ // At this point, we know exactly how long the packet is. Now we can check to make sure we have enough data for the full packet.
+ if( (rx_length - packet_index) < (data_length + 5) )
+ {
+ return 3;
+ }
+
+ // If we get here, we know that we have a full packet in the buffer. All that remains is to pullout the data and make sure the checksum is good. Start by extracting all the data
+ packet->Address = rx_data6[packet_index + 4];
+ packet->PT = PT;
+
+ // Get the data bytes and compute the checksum all in one step
+ packet->data_length = data_length;
+ uint16_t computed_checksum = 's' + 'n' + 'p' + packet->PT + packet->Address;
+
+ for( index = 0; index < data_length; index++ )
+ {
+ // Copy the data into the packet structure’s data array
+ packet->data[index] = rx_data6[packet_index + 5 + index];
+ // Add the new byte to the checksum
+ computed_checksum += packet->data[index];
+ }
+
+ // Now see if our computed checksum matches the received checksum
+ // First extract the checksum from the packet
+ uint16_t received_checksum = (rx_data6[packet_index + 5 + data_length] << 8);
+ received_checksum |= rx_data6[packet_index + 6 + data_length];
+
+ // Now check to see if they don’t match
+ if( received_checksum != computed_checksum )
+ {
+ return 4;
+ }
+ // At this point, we’ve received a full packet with a good checksum. It is already fully parsed and copied to the ‘packet’ structure, so return 0 to indicate that a packet was processed.
+ return 0;
+}
+
+
+// read a float from 4 bytes in buffer,
+// starting at offset, MSB first
+float readFloat(uint8_t * buffer, int offset) {
+ float val=0;
+
+ unsigned long result=0;
+ result |= ((unsigned long)(buffer[offset]) << 0x18);
+ result |= ((unsigned long)(buffer[offset+1]) << 0x10);
+ result |= ((unsigned long)(buffer[offset+2]) << 0x08);
+ result |= ((unsigned long)(buffer[offset+3]));
+ memcpy(&val,&result,4);
+
+ return val;
+}
+
+void setupIMU(){
+ um6serial.baud(115200);
+// pc6.baud(115200);
+// pc6.printf("----------- Start! -----------\n");
+ um6serial.attach(&readIMUAsync);
+ wait(12);
+}
+
+
+void readIMUAsync(){
+ if(iPack < packet_length){
+ rx_data6[iPack] = um6serial.getc();
+// printf("%c",rx_data6[iPack]);
+ iPack++;
+ }
+ if(iPack >= packet_length){
+ iPack = 0;
+ UM6_packet new_packet;
+ // Call the parse_serial_data function to handle the incoming serial data. The serial data should
+ // be placed in ’rx_data6’ and the length in ‘packet_length’ before this function is called.
+ int datastatus = parse_serial_data( rx_data6, packet_length, &new_packet );
+// printf("\n");
+ if( !datastatus){
+ // We got a good packet!
+// for (int i = 0; i<42; i++){
+// printf("%c",rx_data6[i]);
+// }
+// printf("\n");
+
+// pc6.printf("Size of data packet: %i\n\n", NELEMS(new_packet.data));
+// printf("data_length: %i\n\n", new_packet.data_length);
+// printf("Address: %hhx\n\n", new_packet.Address);
+
+ /*
+ #define UM6_GYRO_PROC_XY 0x5C
+ #define UM6_GYRO_PROC_Z 0x5D
+ #define UM6_ACCEL_PROC_XY 0x5E
+ #define UM6_ACCEL_PROC_Z 0x5F
+
+ #define UM6_EULER_PHI_THETA 0x62
+ #define UM6_EULER_PSI 0x63
+ */
+
+ if( new_packet.Address == UM6_GYRO_PROC_XY ){
+ pitchDotRaw = 256*new_packet.data[0] + new_packet.data[1];
+ pitchDot = pitchDotRaw*RAW2ANGDOT;
+ rollDotRaw = 256*new_packet.data[2] + new_packet.data[3];
+ rollDot = rollDotRaw*RAW2ANGDOT+1.5;
+ yawDotRaw = 256*new_packet.data[4] + new_packet.data[5];
+ yawDot = yawDotRaw*RAW2ANGDOT;
+
+ accXRaw = 256*new_packet.data[8] + new_packet.data[9];
+ accX = accXRaw*RAW2ACC;
+ accYRaw = 256*new_packet.data[10] + new_packet.data[11];
+ accY = accYRaw*RAW2ACC;
+ accZRaw = 256*new_packet.data[12] + new_packet.data[13];
+ accZ = accZRaw*RAW2ACC;
+
+ }else if( new_packet.Address == UM6_EULER_PHI_THETA ){
+ pitchRaw = 256*new_packet.data[0] + new_packet.data[1];
+ pitch = pitchRaw*RAW2ANG - 180.0+3.5;
+ rollRaw = 256*new_packet.data[2] + new_packet.data[3];
+ roll = rollRaw*RAW2ANG;
+ yawRaw = 256*new_packet.data[4] + new_packet.data[5];
+ yaw = yawRaw*RAW2ANG;
+ }
+ }else{
+// printf("BAD IMU PACKET: %i\n", datastatus);
+ }
+ }
+
+}
\ No newline at end of file
