BMS STM32 V4.0.0.0

2026-08-25 17:30:40 +08:00
commit 3ae7bcb054
313 changed files with 115734 additions and 0 deletions
@@ -0,0 +1,120 @@
+/**
+  ******************************************************************************
+  * @file    LBS_Transmit.c 
+  * @author  
+  * @version 
+  * @date    
+  * @brief   
+  ******************************************************************************
+  * @attention
+  *
+  *
+  ******************************************************************************
+  */
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f10x.h"
+#include "global.h"
+#include <math.h>
+#include <stdint.h>
+
+
+// PI定义
+#define M_PI          3.1415926535897932384626433832795
+
+// 常量定义
+#define EARTH_RADIUS  6378245.0               // 地球长半径
+#define EE            0.00669342162296594323  // 偏心率平方
+
+
+/**
+ * @brief 检查坐标是否在中国大陆以外
+ * @return 1:境外, 0:境内
+ */
+static int is_out_of_china(double lon, double lat)
+{
+	if (lon < 72.004 || lon > 137.8347)
+	{
+		return 1;
+	}
+	if (lat < 0.8293 || lat > 55.8271)
+	{
+		return 1;
+	}
+	
+	return 0;
+}
+
+/**
+ * @brief 纬度转换辅助函数
+ */
+static double transform_lat(double x, double y)
+{
+	double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
+	ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
+	ret += (20.0 * sin(y * M_PI) + 40.0 * sin(y / 3.0 * M_PI)) * 2.0 / 3.0;
+	ret += (160.0 * sin(y / 12.0 * M_PI) + 320 * sin(y * M_PI / 30.0)) * 2.0 / 3.0;
+	return ret;
+}
+
+/**
+ * @brief 经度转换辅助函数
+ */
+static double transform_lon(double x, double y)
+{
+	double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
+	ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
+	ret += (20.0 * sin(x * M_PI) + 40.0 * sin(x / 3.0 * M_PI)) * 2.0 / 3.0;
+	ret += (150.0 * sin(x / 12.0 * M_PI) + 300.0 * sin(x / 30.0 * M_PI)) * 2.0 / 3.0;
+	return ret;
+}
+
+/**
+ * @brief GCJ-02转WGS84坐标系
+ * @param gcj_lon GCJ-02经度
+ * @param gcj_lat GCJ-02纬度
+ * @param wgs_lon WGS84经度输出指针
+ * @param wgs_lat WGS84纬度输出指针
+ * @note  迭代7次，精度约0.1-0.5米
+ */
+void gcj02_to_wgs84(double gcj_lon, double gcj_lat, double *wgs_lon, double *wgs_lat)
+{
+	//如果坐标不在中国大陆，直接返回原坐标
+	if(is_out_of_china(gcj_lon, gcj_lat))
+	{
+		*wgs_lon = gcj_lon;
+		*wgs_lat = gcj_lat;
+		return;
+	}
+	
+	//使用迭代法进行转换（7次迭代足够精确）
+	double d_lon = 0.0, d_lat = 0.0;
+	double tmp_lon = gcj_lon, tmp_lat = gcj_lat;
+	
+	for(int i = 0; i < 7; i++)
+	{
+		//计算当前WGS84坐标转GCJ02的偏移
+		double delta_lat = transform_lat(tmp_lon - 105.0, tmp_lat - 35.0);
+		double delta_lon = transform_lon(tmp_lon - 105.0, tmp_lat - 35.0);
+		
+		double rad_lat = tmp_lat * M_PI / 180.0;
+		double magic = sin(rad_lat);
+		magic = 1 - EE * magic * magic;
+		double sqrt_magic = sqrt(magic);
+		
+		delta_lat = (delta_lat * 180.0) / ((EARTH_RADIUS * (1 - EE)) / (magic * sqrt_magic) * M_PI);
+		delta_lon = (delta_lon * 180.0) / (EARTH_RADIUS / sqrt_magic * cos(rad_lat) * M_PI);
+		
+		// 计算与目标GCJ02坐标的差值
+		d_lat = gcj_lat - (tmp_lat + delta_lat);
+		d_lon = gcj_lon - (tmp_lon + delta_lon);
+		
+		// 更新WGS84坐标估计
+		tmp_lat += d_lat;
+		tmp_lon += d_lon;
+	}
+	
+	*wgs_lon = tmp_lon;
+	*wgs_lat = tmp_lat;
+}
+