This program retrieve Euler angles over SPI from YEI 3 Space Sensor
Hello!
This program fetch Euler angles from YEI 3-Space Sensor Embedded over SPI. Then decode the unreadable data to float.
Diff: main.cpp
- Revision:
- 0:090f184c5b52
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Sep 08 08:43:02 2014 +0000 @@ -0,0 +1,80 @@ +#include "mbed.h" + +DigitalOut myled(LED1); +SPI spi(p5, p6, p7); // mosi, miso, sclk +DigitalOut cs(p8); // chip sellect +Serial pc(USBTX, USBRX); // tx, rx + +float floatingPointDecoder(uint32_t VALUE) { + uint32_t value_Temp = 0; + float val1 = 0.0f; + float val2 = 0.0f; + float val3 = 0.0f; + ( VALUE & (1 << 31) ) ? val1 = -1.0f : val1 = 1.0f; + value_Temp = VALUE << 1; + value_Temp = value_Temp >> 24; + val2 = (float) value_Temp - 127.0f; + value_Temp = VALUE << 8; + value_Temp = value_Temp >> 8; + for(int i=24-1, j=0; i>=0; i--, j++) { + ( value_Temp & (1 << i) ) ? val3 += 1.0f / ( pow(2.0,j) ) : val3 += 0.0f; + } + return val1 * pow(2.0f,val2) * val3; +} + +int main() { + int i = 0; + uint8_t value[13] = {0}; + uint32_t pyr[3] = {0}; + float yaw = 0.0f, pitch = 0.0f, roll = 0.0f; + + pc.baud(9600);while(1){ + + // Chip must be deselected + cs = 1; + + // Setup the spi for 8 bit data, high steady state clock, + // second edge capture, with a 1MHz clock rate + spi.format(8,0); + spi.frequency(1000000); + // Select the device by seting chip select low + cs = 0; + + // Send 0xF6, to start communication on SPI + value[0] = spi.write(0xF6); + //printf("F6 is sent and 00 is waiting. The return result is: 0x%X\n", value[0]); + + // Send 0x07, to request untared euler angles + value[0] = spi.write(0x07); + //printf("07 is sent and 04 is waiting. The return result is: 0x%X\n", value[0]); + + for(i=0; i<=12; i++) { + value[i] = spi.write(0xFF); + if(value[i] == 1) { + i = 0; + } + wait(0.01); + } + + pyr[0] = value[1]; + pyr[0] = (pyr[0] << 8) + value[2]; + pyr[0] = (pyr[0] << 8) + value[3]; + pyr[0] = (pyr[0] << 8) + value[4]; + pitch = floatingPointDecoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795; + pyr[1] = value[5]; + pyr[1] = (pyr[1] << 8) + value[6]; + pyr[1] = (pyr[1] << 8) + value[7]; + pyr[1] = (pyr[1] << 8) + value[8]; + yaw = floatingPointDecoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795; + pyr[2] = value[9]; + pyr[2] = (pyr[2] << 8) + value[10]; + pyr[2] = (pyr[2] << 8) + value[11]; + pyr[2] = (pyr[2] << 8) + value[12]; + roll = floatingPointDecoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795; + + printf("Pitch: %f Yaw: %f Roll: %f\n", pitch, yaw, roll); + + // Deselect the device + cs = 1; + wait(1);} +} \ No newline at end of file