LabRobocon / Mbed 2 deprecated LabRobocon_2017

Added one task

Dependencies:   mbed

mylib/devices/src/L3GD20.cpp@0:fb4269aa5fb4, 2017-05-26 (annotated)

Committer:
PicYusuke
Date:
Fri May 26 03:51:19 2017 +0000
Revision:
0:fb4269aa5fb4
hoge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PicYusuke 0:fb4269aa5fb4 1 #include "L3GD20.h"
PicYusuke 0:fb4269aa5fb4 2
PicYusuke 0:fb4269aa5fb4 3 #define CS_GPIOx GPIOA
PicYusuke 0:fb4269aa5fb4 4 #define CS_GPIO_Pin GPIO_PIN_3
PicYusuke 0:fb4269aa5fb4 5
PicYusuke 0:fb4269aa5fb4 6 gyro_t l3gd20;
PicYusuke 0:fb4269aa5fb4 7
PicYusuke 0:fb4269aa5fb4 8 /*----------------SPI module-----------------*/
PicYusuke 0:fb4269aa5fb4 9 static SPI *spi_ptr = &spi1; //use SPI1
PicYusuke 0:fb4269aa5fb4 10 static DigitalOut *cs_ptr = &spi1_cs1; //CS pin
PicYusuke 0:fb4269aa5fb4 11 /*-------------------------------------------*/
PicYusuke 0:fb4269aa5fb4 12
PicYusuke 0:fb4269aa5fb4 13 void L3GD20_Init()
PicYusuke 0:fb4269aa5fb4 14 {
PicYusuke 0:fb4269aa5fb4 15 *cs_ptr = 1; //CS high
PicYusuke 0:fb4269aa5fb4 16 HAL_Delay(10);
PicYusuke 0:fb4269aa5fb4 17
PicYusuke 0:fb4269aa5fb4 18 /*-----configuration-----*/
PicYusuke 0:fb4269aa5fb4 19 *cs_ptr = 0; //CS low
PicYusuke 0:fb4269aa5fb4 20 (*spi_ptr).write(CTRL_REG1);
PicYusuke 0:fb4269aa5fb4 21 (*spi_ptr).write(0xCF);
PicYusuke 0:fb4269aa5fb4 22 *cs_ptr = 1; //CS high
PicYusuke 0:fb4269aa5fb4 23 HAL_Delay(50);
PicYusuke 0:fb4269aa5fb4 24
PicYusuke 0:fb4269aa5fb4 25 *cs_ptr = 0; //CS low
PicYusuke 0:fb4269aa5fb4 26 (*spi_ptr).write(CTRL_REG4);
PicYusuke 0:fb4269aa5fb4 27 (*spi_ptr).write(0x30);
PicYusuke 0:fb4269aa5fb4 28 *cs_ptr = 1; //CS high
PicYusuke 0:fb4269aa5fb4 29 /*-----------------------*/
PicYusuke 0:fb4269aa5fb4 30
PicYusuke 0:fb4269aa5fb4 31 #if 0
PicYusuke 0:fb4269aa5fb4 32 spi_tx[0] = WHO_AM_I | L3GD20_RW;
PicYusuke 0:fb4269aa5fb4 33 spi_tx[1] = 0x00;
PicYusuke 0:fb4269aa5fb4 34 HAL_GPIO_WritePin(CS_GPIOx, CS_GPIO_Pin, GPIO_PIN_RESET);
PicYusuke 0:fb4269aa5fb4 35 HAL_SPI_TransmitReceive(&hspi1, spi_tx, spi_rx, 2, 10);
PicYusuke 0:fb4269aa5fb4 36 HAL_GPIO_WritePin(CS_GPIOx, CS_GPIO_Pin, GPIO_PIN_SET);
PicYusuke 0:fb4269aa5fb4 37
PicYusuke 0:fb4269aa5fb4 38 //printf("who am i = 0x%x\n", spi_rx[1]);
PicYusuke 0:fb4269aa5fb4 39 //while(spi_rx[1] == 0xFF);
PicYusuke 0:fb4269aa5fb4 40 #endif
PicYusuke 0:fb4269aa5fb4 41
PicYusuke 0:fb4269aa5fb4 42 }
PicYusuke 0:fb4269aa5fb4 43
PicYusuke 0:fb4269aa5fb4 44 void L3GD20_Read()
PicYusuke 0:fb4269aa5fb4 45 {
PicYusuke 0:fb4269aa5fb4 46 uint32_t i;
PicYusuke 0:fb4269aa5fb4 47
PicYusuke 0:fb4269aa5fb4 48 *cs_ptr = 0; //CS low
PicYusuke 0:fb4269aa5fb4 49 //"0x40" MUST be added to increment the resister address
PicYusuke 0:fb4269aa5fb4 50 (*spi_ptr).write(OUT_X_L | L3GD20_RW | 0x40);
PicYusuke 0:fb4269aa5fb4 51
PicYusuke 0:fb4269aa5fb4 52 for(i = 0; i < 6; i ++)
PicYusuke 0:fb4269aa5fb4 53 {
PicYusuke 0:fb4269aa5fb4 54 //spi_rx[0] is an invalid byte
PicYusuke 0:fb4269aa5fb4 55 //so this process starts with spi_rx[1]
PicYusuke 0:fb4269aa5fb4 56 spi_rx[i+1] = (*spi_ptr).write(0x00); //dummy data
PicYusuke 0:fb4269aa5fb4 57 }
PicYusuke 0:fb4269aa5fb4 58
PicYusuke 0:fb4269aa5fb4 59 *cs_ptr = 1; //CS high
PicYusuke 0:fb4269aa5fb4 60
PicYusuke 0:fb4269aa5fb4 61 l3gd20.x = ((int32_t)spi_rx[1]+((int32_t)spi_rx[2]<<8));
PicYusuke 0:fb4269aa5fb4 62 l3gd20.y = ((int32_t)spi_rx[3]+((int32_t)spi_rx[4]<<8));
PicYusuke 0:fb4269aa5fb4 63 l3gd20.z = ((int32_t)spi_rx[5]+((int32_t)spi_rx[6]<<8));
PicYusuke 0:fb4269aa5fb4 64
PicYusuke 0:fb4269aa5fb4 65 if(l3gd20.z > 32768)
PicYusuke 0:fb4269aa5fb4 66 {
PicYusuke 0:fb4269aa5fb4 67 l3gd20.z = l3gd20.z - 65535 + 6;
PicYusuke 0:fb4269aa5fb4 68 }
PicYusuke 0:fb4269aa5fb4 69 }