串口ok

2024-09-26 22:32:20 +08:00
commit 097089ef4e
323 changed files with 135661 additions and 0 deletions
@@ -0,0 +1,140 @@
+#include "math.h"
+#include "Mag_HMC5883L.h"
+#include <stdio.h>
+
+int Mag_HMC5883_init(Mag_HMC5883_t *mag, const char *name, I2C_DEV_t *dev,
+		GPIO_EXIT_t *exti) {
+	uint8_t RxdBuf[3];
+	int rslt;
+
+	mag->name = name;
+	mag->_dev = dev;
+	mag->exti = exti;
+
+	mag->cnt = 0;
+	mag->last_cnt = 0;
+	mag->pps = 0;
+
+	I2C_DEV_begin(mag->_dev, -1);
+
+	rslt = 0;
+	if (I2C_DEV_read_registers(mag->_dev, 0x0A, RxdBuf, 3)) {
+		if (RxdBuf[0] == 'H' && RxdBuf[1] == '4' && RxdBuf[2] == '3') {
+			//TxdBuf[0] = 0x01; //Register B
+			//TxdBuf[1] = 0x20; // default 1.3Ga 1090LSB/Gause, 0xF800-0x7FF
+			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
+			I2C_DEV_write_register(mag->_dev, 0x01, 0x20);
+
+			I2C_DEV_write_register(mag->_dev, 0x00, 0x71);
+			I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
+			HAL_Delay(10);
+			I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
+			mag->magnet_xyz[0] =
+					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
+			mag->magnet_xyz[2] =
+					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
+			mag->magnet_xyz[1] =
+					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
+			for (int i=0;i<3;++i)
+			{
+				if (mag->magnet_xyz[i]<900)
+				{
+					rslt = i+1;
+				}
+			}
+
+			I2C_DEV_write_register(mag->_dev, 0x00, 0x72);
+			I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
+			HAL_Delay(10);
+			I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
+			mag->magnet_xyz[0] =
+					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
+			mag->magnet_xyz[2] =
+					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
+			mag->magnet_xyz[1] =
+					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
+			for (int i=0;i<3;++i)
+			{
+				if (mag->magnet_xyz[i]>-900)
+				{
+					rslt = i+1;
+				}
+			}
+
+			//TxdBuf[0] = 0x00; //Register A
+			//TxdBuf[1] = 0x70; // 8 sample averge, 15Hz output, normal config
+			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
+			I2C_DEV_write_register(mag->_dev, 0x00, 0x70);
+
+			//TxdBuf[0] = 0x02; // Mode Register
+			//TxdBuf[1] = 0x00; // continuse mode
+			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
+			I2C_DEV_write_register(mag->_dev, 0x02, 0x00);
+
+			HAL_Delay(10);
+
+			if (I2C_DEV_read_registers(mag->_dev, 0x00, RxdBuf, 3)) {
+				if (RxdBuf[0] == 0x70 && RxdBuf[1] == 0x20
+						&& RxdBuf[2] == 0x00) {
+					;//pass
+				} else {
+					rslt = -4;
+				}
+			} else {
+				rslt = -3;
+			}
+		} else {
+			rslt = -2;
+		}
+	} else {
+		rslt = -1;
+	}
+
+	I2C_DEV_end(mag->_dev);
+	return rslt;
+}
+
+bool Mag_HMC5883_update(Mag_HMC5883_t *mag) {
+	bool rslt;
+	uint8_t RxdBuf[6];
+
+	rslt = false;
+
+	if (I2C_DEV_begin(mag->_dev, 1)) {
+		if (mag->exti)
+		{
+		   mag->exti->cnt = 0;
+		   rslt = true;
+		}
+		else
+		{
+			if (I2C_DEV_read_registers(mag->_dev, 0x09, RxdBuf, 1)) {
+				if (RxdBuf[0] & 0x01) {
+					rslt = true;
+				}
+			}
+		}
+		if (rslt && I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6)) {
+			mag->magnet_xyz[0] =
+					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
+			mag->magnet_xyz[2] =
+					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
+			mag->magnet_xyz[1] =
+					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
+			mag->cnt++;
+		} else {
+			rslt = false;
+		}
+
+		I2C_DEV_end(mag->_dev);
+	} else {
+		rslt = false;
+	}
+
+	return rslt;
+}
+
+void Mag_HMC5883_stats(Mag_HMC5883_t *mag) {
+	mag->pps = mag->cnt - mag->last_cnt;
+	mag->last_cnt = mag->cnt;
+}