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.
main.cpp@0:090f184c5b52, 2014-09-08 (annotated)
- Committer:
- LORDTEK
- Date:
- Mon Sep 08 08:43:02 2014 +0000
- Revision:
- 0:090f184c5b52
YEI 3 Space Sensor Embedded over SPI communication Pitch Yaw Roll Euler angle
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LORDTEK | 0:090f184c5b52 | 1 | #include "mbed.h" |
LORDTEK | 0:090f184c5b52 | 2 | |
LORDTEK | 0:090f184c5b52 | 3 | DigitalOut myled(LED1); |
LORDTEK | 0:090f184c5b52 | 4 | SPI spi(p5, p6, p7); // mosi, miso, sclk |
LORDTEK | 0:090f184c5b52 | 5 | DigitalOut cs(p8); // chip sellect |
LORDTEK | 0:090f184c5b52 | 6 | Serial pc(USBTX, USBRX); // tx, rx |
LORDTEK | 0:090f184c5b52 | 7 | |
LORDTEK | 0:090f184c5b52 | 8 | float floatingPointDecoder(uint32_t VALUE) { |
LORDTEK | 0:090f184c5b52 | 9 | uint32_t value_Temp = 0; |
LORDTEK | 0:090f184c5b52 | 10 | float val1 = 0.0f; |
LORDTEK | 0:090f184c5b52 | 11 | float val2 = 0.0f; |
LORDTEK | 0:090f184c5b52 | 12 | float val3 = 0.0f; |
LORDTEK | 0:090f184c5b52 | 13 | ( VALUE & (1 << 31) ) ? val1 = -1.0f : val1 = 1.0f; |
LORDTEK | 0:090f184c5b52 | 14 | value_Temp = VALUE << 1; |
LORDTEK | 0:090f184c5b52 | 15 | value_Temp = value_Temp >> 24; |
LORDTEK | 0:090f184c5b52 | 16 | val2 = (float) value_Temp - 127.0f; |
LORDTEK | 0:090f184c5b52 | 17 | value_Temp = VALUE << 8; |
LORDTEK | 0:090f184c5b52 | 18 | value_Temp = value_Temp >> 8; |
LORDTEK | 0:090f184c5b52 | 19 | for(int i=24-1, j=0; i>=0; i--, j++) { |
LORDTEK | 0:090f184c5b52 | 20 | ( value_Temp & (1 << i) ) ? val3 += 1.0f / ( pow(2.0,j) ) : val3 += 0.0f; |
LORDTEK | 0:090f184c5b52 | 21 | } |
LORDTEK | 0:090f184c5b52 | 22 | return val1 * pow(2.0f,val2) * val3; |
LORDTEK | 0:090f184c5b52 | 23 | } |
LORDTEK | 0:090f184c5b52 | 24 | |
LORDTEK | 0:090f184c5b52 | 25 | int main() { |
LORDTEK | 0:090f184c5b52 | 26 | int i = 0; |
LORDTEK | 0:090f184c5b52 | 27 | uint8_t value[13] = {0}; |
LORDTEK | 0:090f184c5b52 | 28 | uint32_t pyr[3] = {0}; |
LORDTEK | 0:090f184c5b52 | 29 | float yaw = 0.0f, pitch = 0.0f, roll = 0.0f; |
LORDTEK | 0:090f184c5b52 | 30 | |
LORDTEK | 0:090f184c5b52 | 31 | pc.baud(9600);while(1){ |
LORDTEK | 0:090f184c5b52 | 32 | |
LORDTEK | 0:090f184c5b52 | 33 | // Chip must be deselected |
LORDTEK | 0:090f184c5b52 | 34 | cs = 1; |
LORDTEK | 0:090f184c5b52 | 35 | |
LORDTEK | 0:090f184c5b52 | 36 | // Setup the spi for 8 bit data, high steady state clock, |
LORDTEK | 0:090f184c5b52 | 37 | // second edge capture, with a 1MHz clock rate |
LORDTEK | 0:090f184c5b52 | 38 | spi.format(8,0); |
LORDTEK | 0:090f184c5b52 | 39 | spi.frequency(1000000); |
LORDTEK | 0:090f184c5b52 | 40 | // Select the device by seting chip select low |
LORDTEK | 0:090f184c5b52 | 41 | cs = 0; |
LORDTEK | 0:090f184c5b52 | 42 | |
LORDTEK | 0:090f184c5b52 | 43 | // Send 0xF6, to start communication on SPI |
LORDTEK | 0:090f184c5b52 | 44 | value[0] = spi.write(0xF6); |
LORDTEK | 0:090f184c5b52 | 45 | //printf("F6 is sent and 00 is waiting. The return result is: 0x%X\n", value[0]); |
LORDTEK | 0:090f184c5b52 | 46 | |
LORDTEK | 0:090f184c5b52 | 47 | // Send 0x07, to request untared euler angles |
LORDTEK | 0:090f184c5b52 | 48 | value[0] = spi.write(0x07); |
LORDTEK | 0:090f184c5b52 | 49 | //printf("07 is sent and 04 is waiting. The return result is: 0x%X\n", value[0]); |
LORDTEK | 0:090f184c5b52 | 50 | |
LORDTEK | 0:090f184c5b52 | 51 | for(i=0; i<=12; i++) { |
LORDTEK | 0:090f184c5b52 | 52 | value[i] = spi.write(0xFF); |
LORDTEK | 0:090f184c5b52 | 53 | if(value[i] == 1) { |
LORDTEK | 0:090f184c5b52 | 54 | i = 0; |
LORDTEK | 0:090f184c5b52 | 55 | } |
LORDTEK | 0:090f184c5b52 | 56 | wait(0.01); |
LORDTEK | 0:090f184c5b52 | 57 | } |
LORDTEK | 0:090f184c5b52 | 58 | |
LORDTEK | 0:090f184c5b52 | 59 | pyr[0] = value[1]; |
LORDTEK | 0:090f184c5b52 | 60 | pyr[0] = (pyr[0] << 8) + value[2]; |
LORDTEK | 0:090f184c5b52 | 61 | pyr[0] = (pyr[0] << 8) + value[3]; |
LORDTEK | 0:090f184c5b52 | 62 | pyr[0] = (pyr[0] << 8) + value[4]; |
LORDTEK | 0:090f184c5b52 | 63 | pitch = floatingPointDecoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795; |
LORDTEK | 0:090f184c5b52 | 64 | pyr[1] = value[5]; |
LORDTEK | 0:090f184c5b52 | 65 | pyr[1] = (pyr[1] << 8) + value[6]; |
LORDTEK | 0:090f184c5b52 | 66 | pyr[1] = (pyr[1] << 8) + value[7]; |
LORDTEK | 0:090f184c5b52 | 67 | pyr[1] = (pyr[1] << 8) + value[8]; |
LORDTEK | 0:090f184c5b52 | 68 | yaw = floatingPointDecoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795; |
LORDTEK | 0:090f184c5b52 | 69 | pyr[2] = value[9]; |
LORDTEK | 0:090f184c5b52 | 70 | pyr[2] = (pyr[2] << 8) + value[10]; |
LORDTEK | 0:090f184c5b52 | 71 | pyr[2] = (pyr[2] << 8) + value[11]; |
LORDTEK | 0:090f184c5b52 | 72 | pyr[2] = (pyr[2] << 8) + value[12]; |
LORDTEK | 0:090f184c5b52 | 73 | roll = floatingPointDecoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795; |
LORDTEK | 0:090f184c5b52 | 74 | |
LORDTEK | 0:090f184c5b52 | 75 | printf("Pitch: %f Yaw: %f Roll: %f\n", pitch, yaw, roll); |
LORDTEK | 0:090f184c5b52 | 76 | |
LORDTEK | 0:090f184c5b52 | 77 | // Deselect the device |
LORDTEK | 0:090f184c5b52 | 78 | cs = 1; |
LORDTEK | 0:090f184c5b52 | 79 | wait(1);} |
LORDTEK | 0:090f184c5b52 | 80 | } |