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

6
fun/HCBle.c
View File

@@ -33,7 +33,7 @@ uint8_t rx_data; //
uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
RingBuffer ble_rx_ring = {0}; //<2F><>ʼ<EFBFBD><CABC>
MotorCommand cmd;
LocationData current_location = {0};
BleMessage current_location = {0};
float imu_angle = 0.0f;
TX_EVENT_FLAGS_GROUP ble_event_flags;
@@ -200,8 +200,10 @@ void ble_tx_task_entry(ULONG thread_input) {
if(tx_queue_receive(&ble_tx_queue,&msg,TX_WAIT_FOREVER) == TX_SUCCESS)
{
HCBle_SendData("#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.1f}\n",
msg.lat, msg.lon,0);
msg.lat, msg.lon,msg.angle);
}
tx_thread_sleep(100);
}
}

9
fun/HCBle.h
View File

@@ -30,12 +30,17 @@ typedef struct
}RingBuffer;
typedef struct
{
float lat;
float lon;
float angle;
}BleMessage;
extern uint8_t rx_data;
extern RingBuffer ble_rx_ring; //<2F><>ʼ<EFBFBD><CABC>
extern uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
extern LocationData current_location;
extern BleMessage current_location;
extern uint8_t flag;

7
fun/gps.c
View File

@@ -349,13 +349,14 @@ void gps_thread_entry(ULONG thread_input)
tx_event_flags_set(&system_events, EVENT_LOCATION_UPDATED, TX_OR);
msg.lat = current_location.lat;
msg.lon = current_location.lon;
// msg.lat = current_location.lat;
// msg.lon = current_location.lon;
// snprintf(msg, sizeof(msg), "#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n",
// current_location.lat,
// current_location.lon,
// current_location.angle);
tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
BleMessage msg = current_location;
tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
}
}

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);
}
}
}

12
fun/value.h
View File

@@ -1,12 +1,12 @@
#ifndef __VALUE_H__
#define __VALUE_H__
typedef struct
{
float lat;
float lon;
float angle;
}BleMessage;
//typedef struct
//{
// float lat;
// float lon;
// float angle;
//}BleMessage;
#define EVENT_GPS_DATA_READY (1U << 0)
#define EVENT_IMU_DATA_READY (1U << 1)