LORDTEK LORDTEK / Mbed 2 deprecated YEI-SPI

This program retrieve Euler angles over SPI from YEI 3 Space Sensor

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }