串口ok

2024-09-26 22:32:20 +08:00
commit 097089ef4e
323 changed files with 135661 additions and 0 deletions
@@ -0,0 +1,187 @@
+/*
+ * Gyro_XV7011BB.c
+ *
+ *  Created on: July 12, 2020
+ *      Author: lqh
+ */
+#include "Gyro_XV7011BB.h"
+#include "main.h"
+
+/* XV7011BB Gyroscope registers addresses */
+#define GYRO_DSP_CTL1            0x01
+#define GYRO_DSP_CTL2            0x02
+#define GYRO_DSP_CTL3            0x03
+#define GYRO_STS_RD              0x04
+#define GYRO_SLP_IN              0x05
+#define GYRO_SLP_OUT             0x06
+#define GYRO_ST_BY               0x07
+#define GYRO_TEMP_RD             0x08
+#define GYRO_SW_RST              0x09
+#define GYRO_DAT_ACCON           0x0A
+#define GYRO_OUT_CTL1            0x0B
+#define GYRO_AUTO_C              0x0C
+#define GYRO_DSP_RES             0x0D
+#define GYRO_MEM_LOAD            0x1B
+#define GYRO_TSDATA_FORMAT       0x1C
+#define GYRO_IF_CTL              0x1F
+
+int Gyro_XV7011BB_init(Gyro_XV7011BB_t *Gyro, const char *name, SPI_DEV_t *dev) {
+	uint8_t TxdBuf[2];
+	uint8_t RxdBuf[2];
+	int rslt;
+
+	Gyro->name = name;
+	Gyro->_dev = dev;
+	Gyro->read_cnt = 0;
+	Gyro->cnt = 0;
+	Gyro->last_cnt = 0;
+	Gyro->pps = 0;
+
+	SPI_DEV_begin(dev, -1);
+
+	//Software Reset Chip(Command: Just Specifying an Address)
+	TxdBuf[0] = GYRO_SW_RST;  //bit7=0,means write; bit7=1, means read
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Waite 200ms To Be sure The GYRO ReStart Up
+	HAL_Delay(200);
+
+	//Disnable IIC, Enable 4-wire SPI
+	TxdBuf[0] = GYRO_IF_CTL; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	TxdBuf[1] = 0;  //SPISel=0(4-wire SPI), IIC_EN=0(Disnable)
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Set LPF(DSP Setting2)
+	TxdBuf[0] = GYRO_DSP_CTL2; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	TxdBuf[1] = 0x23;  //LPF Order = 4, LPF=50Hz
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Set ODR(DSP Setting3)
+	TxdBuf[0] = GYRO_DSP_CTL3; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	TxdBuf[1] = 0x01;  //ODR=FS/2
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Set Angular rate data format:24bit
+	TxdBuf[0] = GYRO_OUT_CTL1; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	TxdBuf[1] = 0x5;  //DataFormat=1(24bit), OutCtl[1:0]=01(Angular rate output)
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Set Temperature data format:12bit
+	TxdBuf[0] = GYRO_TSDATA_FORMAT; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	TxdBuf[1] = 0x4B;  //DataFormat=10(12bit), MISO/SA0=10
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Sleep Out(Start)  (Command: Just Specifying an Address)
+	TxdBuf[0] = GYRO_SLP_OUT; //bit7=0,means write; bit7=1, means read, bit5-bit6 = 00, means all(H/J/K) slave device
+	SPI_DEV_select(dev);
+	SPI_DEV_transfer(dev, TxdBuf, 1, NULL, 0, 2);
+	SPI_DEV_unselect(dev);
+
+	//Init GYRO Global Variable
+	Gyro->Gyro = 0;
+	Gyro->Temp = 0;
+
+	//based on config
+	Gyro->lsb_per_deg_sec = 17920.0f;
+	Gyro->lsb_per_degC = 16.0f;
+
+	//Waite 200ms To Be sure The GYRO Sensor Start Up
+	HAL_Delay(200);
+
+	//verify spi communication and device start
+	TxdBuf[0] = (GYRO_STS_RD | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
+	SPI_DEV_select(Gyro[0]._dev);
+	SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 2, RxdBuf, 2, 2);
+	SPI_DEV_unselect(Gyro[0]._dev);
+
+	if ((RxdBuf[1]&0x07) == 0x01)	{
+		rslt = 0;
+	}
+	else {
+		rslt = -1;
+	}
+
+	SPI_DEV_end(dev);
+
+
+	return rslt;
+}
+
+int Gyro_XV7011BB_update(Gyro_XV7011BB_t *Gyro) {
+	uint8_t TxdBuf[4];
+	uint8_t RxdBuf[4];
+	int32_t Temp;
+	int32_t Gyr;
+
+	if (SPI_DEV_begin(Gyro->_dev, 1)) //½øÈëÁÙ½çÇø
+			{
+		TxdBuf[0] = (GYRO_DAT_ACCON | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
+		SPI_DEV_select(Gyro[0]._dev);
+		SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 4, RxdBuf, 4, 2);
+		SPI_DEV_unselect(Gyro[0]._dev);
+
+		Gyr = ((RxdBuf[1] << 16) | (RxdBuf[2] << 8) | RxdBuf[3]);
+		if ((Gyr & 0x00800000) != 0)   //Sign-extending for raw data
+				{
+			Gyr |= 0xFF000000;
+		}
+
+		//Read Tempture of GYRO1
+		TxdBuf[0] = (GYRO_TEMP_RD | 0xA0); //bit7=1,means Read; bit6/5=01,means 1st GYRO
+		SPI_DEV_select(Gyro[0]._dev);
+		SPI_DEV_transfer(Gyro[0]._dev, TxdBuf, 3, RxdBuf, 3, 2);
+		SPI_DEV_unselect(Gyro[0]._dev);
+
+		Temp = ((RxdBuf[1] << 4) | (RxdBuf[2] >> 4));
+		if ((Temp & 0x00000800) != 0)   //Sign-extending for raw data
+				{
+			Temp |= 0xFFFFF000;
+		}
+		SPI_DEV_end(Gyro->_dev);
+
+		taskENTER_CRITICAL();
+		Gyro[0].Gyro += Gyr;
+		Gyro[0].Temp += Temp;
+		Gyro->read_cnt++;
+		taskEXIT_CRITICAL();
+
+		Gyro->cnt++;
+		return true;
+	}
+
+	return false;
+}
+
+int Gyro_XV7011BB_read(Gyro_XV7011BB_t *Gyro, float rollrate[1], float Temp[1]) {
+	int cnt;
+
+	cnt = Gyro->read_cnt;
+	if (cnt > 0) {
+		taskENTER_CRITICAL();
+		rollrate[0] = Gyro[0].Gyro / (cnt*Gyro->lsb_per_deg_sec);
+		Temp[0] = Gyro[0].Temp / (cnt*Gyro->lsb_per_degC);
+		Gyro->Gyro = 0;
+		Gyro->Temp = 0;
+		Gyro->read_cnt = 0;
+		taskEXIT_CRITICAL();
+	}
+	return cnt;
+}
+
+void Gyro_XV7011BB_stats(Gyro_XV7011BB_t *Gyro) {
+	Gyro->pps = Gyro->cnt - Gyro->last_cnt;
+	Gyro->last_cnt = Gyro->cnt;
+}
+