MADAO/ManGoWalk_STM32
1
0
Fork 0
generated from Template/H563ZI-HAL-CMake-Template
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

imu and gps allright

Browse Source
This commit is contained in:
MADAO
2025-07-02 00:06:16 +08:00
parent a6ebf4b8a5
commit 0361cd17af
15 changed files with 1293 additions and 1202 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
fun/imu948.c
View File

@@ -1,5 +1,6 @@
#include "imu948.h"
#define DECLINATION_DEG -3.0
extern _GPSData gps_data;
extern TX_QUEUE ble_tx_queue;
extern uint8_t rx_byte;
@@ -65,7 +66,9 @@ extern TX_QUEUE im948_uart_rx_queue;
void imu_angle_ble_task_entry(ULONG thread_input)
{
ULONG rx_data;
BleMessage msg;
static uint8_t filtInit = 0;
static float heading_filt = 0;
const float alpha = 0.20f; // 20% һ<>׵<EFBFBD>ͨ
// <20><>ʼ<EFBFBD><CABC>ģ<EFBFBD><C4A3>
// IM948_Init();
// HCBle_SendData("halo");
@@ -82,12 +85,28 @@ void imu_angle_ble_task_entry(ULONG thread_input)
{
isNewData = 0;
float angle = AngleZ * 0.0054931640625f;
// msg.angle = AngleZ * 0.0054931640625f;
// tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
// HCBle_SendData("Z:%.2f\r\n",AngleZ);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>angle<6C><65>ä<EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD>ж<EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>angle<6C><65>ä<EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD>ж<EFBFBD>
/* <20><><EFBFBD><EFBFBD> <20><> IMU <20>Ĵ<EFBFBD><C4B4><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>ں<EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> */
float angleRaw = AngleZ; /* <20><><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD>׼ */
/* ת<>汱 <20><>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>ڡ<EFBFBD><DAA1><EFBFBD><EFBFBD>ݴ<EFBFBD>ƫ<EFBFBD><C6AB>Ϊ-3.0<EFBFBD><EFBFBD>*/
float heading = angleRaw + DECLINATION_DEG;
if (heading > 180) heading -= 360;
if (heading < -180) heading += 360;
/* <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB> IIR */
const float alpha = 0.2f;
heading_filt += alpha * (heading - heading_filt);
current_location.angle = heading_filt;
// current_location.angle = AngleZ * 0.0054931640625f;
if(current_location.lat != 0 && current_location.lon != 0)
{
BleMessage msg = current_location; // <20>ṹ<EFBFBD><E1B9B9>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>
tx_queue_send(&ble_tx_queue,&msg,TX_NO_WAIT);
}
tx_thread_sleep(10);
}
}
}