Skip to content

Commit e0401f8

Browse files
LJW0401LJW0401
committed
Merge branch 'master' of https://gitee.com/Ljw0401/StandardRobotpp into 板间通信checkout
2 parents 89b68a1 + 073fd20 commit e0401f8

25 files changed

+1259
-684
lines changed

‎MDK-ARM/standard_robot.uvprojx

+10
Original file line numberDiff line numberDiff line change
@@ -939,6 +939,16 @@
939939
<FileType>1</FileType>
940940
<FilePath>..\application\assist\usb_task.c</FilePath>
941941
</File>
942+
<File>
943+
<FileName>develop_task.c</FileName>
944+
<FileType>1</FileType>
945+
<FilePath>..\application\assist\develop_task.c</FilePath>
946+
</File>
947+
<File>
948+
<FileName>data_exchange.c</FileName>
949+
<FileType>1</FileType>
950+
<FilePath>..\application\assist\data_exchange.c</FilePath>
951+
</File>
942952
</Files>
943953
</Group>
944954
<Group>

‎application/IMU/IMU_task.c

+85-83
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
#include "math.h"
3636
#include "pid.h"
3737
#include "usb_task.h"
38+
#include "data_exchange.h"
3839

3940
// clang-format off
4041
#define IMU_temp_PWM(pwm) imu_pwm_set(pwm) //pwm给定
@@ -100,7 +101,7 @@ static void imu_temp_control(fp32 temp);
100101
*/
101102
static void imu_cmd_spi_dma(void);
102103

103-
static void AutoCaliImuData(void);
104+
// static void AutoCaliImuData(void);
104105
static void UpdateImuData(void);
105106

106107
// clang-format off
@@ -177,36 +178,36 @@ static Imu_t IMU_DATA = {
177178
.z_accel = 0.0f,
178179
};
179180

180-
typedef struct ImuCaliData
181-
{
182-
struct reference
183-
{
184-
float ax, ay, az;
185-
float r, p, y1, y2;
186-
} ref;
187-
188-
struct time
189-
{
190-
uint32_t start;
191-
uint32_t end;
192-
} time;
193-
194-
struct offect
195-
{
196-
float roll;
197-
float pitch;
198-
float yaw;
199-
float yaw_drift_rate;
200-
} offect;
201-
202-
uint8_t read_cnt;
203-
} ImuCaliData_t;
204-
205-
static ImuCaliData_t IMU_CALI_DATA = {
206-
.ref = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
207-
.offect = {0.0f, 0.0f, 0.0f, 0.0f},
208-
.read_cnt = 0,
209-
};
181+
// typedef struct ImuCaliData
182+
// {
183+
// struct reference
184+
// {
185+
// float ax, ay, az;
186+
// float r, p, y1, y2;
187+
// } ref;
188+
189+
// struct time
190+
// {
191+
// uint32_t start;
192+
// uint32_t end;
193+
// } time;
194+
195+
// struct offect
196+
// {
197+
// float roll;
198+
// float pitch;
199+
// float yaw;
200+
// float yaw_drift_rate;
201+
// } offect;
202+
203+
// uint8_t read_cnt;
204+
// } ImuCaliData_t;
205+
206+
// static ImuCaliData_t IMU_CALI_DATA = {
207+
// .ref = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
208+
// .offect = {0.0f, 0.0f, 0.0f, 0.0f},
209+
// .read_cnt = 0,
210+
// };
210211

211212
/**
212213
* @brief imu task, init bmi088, ist8310, calculate the euler angle
@@ -220,6 +221,9 @@ static ImuCaliData_t IMU_CALI_DATA = {
220221
*/
221222
void IMU_task(void const * pvParameters)
222223
{
224+
// 发布IMU数据
225+
Publish(&IMU_DATA, "imu_data");
226+
223227
// clang-format off
224228
//wait a time
225229
osDelay(INS_TASK_INIT_TIME);
@@ -322,7 +326,7 @@ void IMU_task(void const * pvParameters)
322326
// ist8310_read_mag(ist8310_real_data.mag);
323327
}
324328
// clang-format on
325-
AutoCaliImuData();
329+
// AutoCaliImuData();
326330
UpdateImuData();
327331
}
328332
}
@@ -331,59 +335,59 @@ void IMU_task(void const * pvParameters)
331335
* @brief 自动校准IMU数据，每次上电时，读取一定数量的数据，计算出静态角度
332336
* @param
333337
*/
334-
static void AutoCaliImuData(void)
335-
{
336-
if (HAL_GetTick() < 100) {
337-
return;
338-
}
339-
340-
if (IMU_CALI_DATA.read_cnt > IMU_CALI_MAX_COUNT) {
341-
return;
342-
}
343-
344-
if (IMU_CALI_DATA.read_cnt < IMU_CALI_MAX_COUNT) {
345-
IMU_CALI_DATA.ref.ax += INS_accel[INS_ACCEL_X_ADDRESS_OFFSET];
346-
IMU_CALI_DATA.ref.ay += INS_accel[INS_ACCEL_Y_ADDRESS_OFFSET];
347-
IMU_CALI_DATA.ref.az += INS_accel[INS_ACCEL_Z_ADDRESS_OFFSET];
348-
349-
IMU_CALI_DATA.ref.r += INS_angle[INS_ROLL_ADDRESS_OFFSET];
350-
IMU_CALI_DATA.ref.p += INS_angle[INS_PITCH_ADDRESS_OFFSET];
351-
352-
if (IMU_CALI_DATA.read_cnt == 0) {
353-
IMU_CALI_DATA.ref.y1 = INS_angle[INS_YAW_ADDRESS_OFFSET];
354-
IMU_CALI_DATA.time.start = HAL_GetTick();
355-
}
356-
357-
IMU_CALI_DATA.read_cnt++;
358-
} else if (IMU_CALI_DATA.read_cnt == IMU_CALI_MAX_COUNT) {
359-
IMU_CALI_DATA.time.end = HAL_GetTick();
360-
IMU_CALI_DATA.ref.y2 = INS_angle[INS_YAW_ADDRESS_OFFSET];
361-
362-
IMU_CALI_DATA.ref.ax = IMU_CALI_DATA.ref.ax / IMU_CALI_MAX_COUNT;
363-
IMU_CALI_DATA.ref.ay = IMU_CALI_DATA.ref.ay / IMU_CALI_MAX_COUNT;
364-
IMU_CALI_DATA.ref.az = IMU_CALI_DATA.ref.az / IMU_CALI_MAX_COUNT;
365-
366-
IMU_CALI_DATA.ref.r = IMU_CALI_DATA.ref.r / IMU_CALI_MAX_COUNT;
367-
IMU_CALI_DATA.ref.p = IMU_CALI_DATA.ref.p / IMU_CALI_MAX_COUNT;
368-
369-
float static_roll = atan2f(IMU_CALI_DATA.ref.ay, IMU_CALI_DATA.ref.az);
370-
float static_pitch = atan2f(
371-
-IMU_CALI_DATA.ref.ax, sqrtf(
372-
IMU_CALI_DATA.ref.ay * IMU_CALI_DATA.ref.ay +
373-
IMU_CALI_DATA.ref.az * IMU_CALI_DATA.ref.az));
374-
375-
IMU_CALI_DATA.offect.roll = static_roll - IMU_CALI_DATA.ref.r;
376-
IMU_CALI_DATA.offect.pitch = static_pitch - IMU_CALI_DATA.ref.p;
377-
378-
IMU_CALI_DATA.read_cnt++;
379-
}
380-
}
338+
// static void AutoCaliImuData(void)
339+
// {
340+
// if (HAL_GetTick() < 100) {
341+
// return;
342+
// }
343+
344+
// if (IMU_CALI_DATA.read_cnt > IMU_CALI_MAX_COUNT) {
345+
// return;
346+
// }
347+
348+
// if (IMU_CALI_DATA.read_cnt < IMU_CALI_MAX_COUNT) {
349+
// IMU_CALI_DATA.ref.ax += INS_accel[INS_ACCEL_X_ADDRESS_OFFSET];
350+
// IMU_CALI_DATA.ref.ay += INS_accel[INS_ACCEL_Y_ADDRESS_OFFSET];
351+
// IMU_CALI_DATA.ref.az += INS_accel[INS_ACCEL_Z_ADDRESS_OFFSET];
352+
353+
// IMU_CALI_DATA.ref.r += INS_angle[INS_ROLL_ADDRESS_OFFSET];
354+
// IMU_CALI_DATA.ref.p += INS_angle[INS_PITCH_ADDRESS_OFFSET];
355+
356+
// if (IMU_CALI_DATA.read_cnt == 0) {
357+
// IMU_CALI_DATA.ref.y1 = INS_angle[INS_YAW_ADDRESS_OFFSET];
358+
// IMU_CALI_DATA.time.start = HAL_GetTick();
359+
// }
360+
361+
// IMU_CALI_DATA.read_cnt++;
362+
// } else if (IMU_CALI_DATA.read_cnt == IMU_CALI_MAX_COUNT) {
363+
// IMU_CALI_DATA.time.end = HAL_GetTick();
364+
// IMU_CALI_DATA.ref.y2 = INS_angle[INS_YAW_ADDRESS_OFFSET];
365+
366+
// IMU_CALI_DATA.ref.ax = IMU_CALI_DATA.ref.ax / IMU_CALI_MAX_COUNT;
367+
// IMU_CALI_DATA.ref.ay = IMU_CALI_DATA.ref.ay / IMU_CALI_MAX_COUNT;
368+
// IMU_CALI_DATA.ref.az = IMU_CALI_DATA.ref.az / IMU_CALI_MAX_COUNT;
369+
370+
// IMU_CALI_DATA.ref.r = IMU_CALI_DATA.ref.r / IMU_CALI_MAX_COUNT;
371+
// IMU_CALI_DATA.ref.p = IMU_CALI_DATA.ref.p / IMU_CALI_MAX_COUNT;
372+
373+
// float static_roll = atan2f(IMU_CALI_DATA.ref.ay, IMU_CALI_DATA.ref.az);
374+
// float static_pitch = atan2f(
375+
// -IMU_CALI_DATA.ref.ax, sqrtf(
376+
// IMU_CALI_DATA.ref.ay * IMU_CALI_DATA.ref.ay +
377+
// IMU_CALI_DATA.ref.az * IMU_CALI_DATA.ref.az));
378+
379+
// IMU_CALI_DATA.offect.roll = static_roll - IMU_CALI_DATA.ref.r;
380+
// IMU_CALI_DATA.offect.pitch = static_pitch - IMU_CALI_DATA.ref.p;
381+
382+
// IMU_CALI_DATA.read_cnt++;
383+
// }
384+
// }
381385

382386
static void UpdateImuData(void)
383387
{
384388
// clang-format off
385-
IMU_DATA.pitch = INS_angle[INS_PITCH_ADDRESS_OFFSET] + IMU_CALI_DATA.offect.pitch;
386-
IMU_DATA.roll = INS_angle[INS_ROLL_ADDRESS_OFFSET] + IMU_CALI_DATA.offect.roll;
389+
IMU_DATA.pitch = INS_angle[INS_PITCH_ADDRESS_OFFSET];
390+
IMU_DATA.roll = INS_angle[INS_ROLL_ADDRESS_OFFSET];
387391
IMU_DATA.yaw = INS_angle[INS_YAW_ADDRESS_OFFSET];
388392

389393
IMU_DATA.roll_vel = INS_gyro[INS_GYRO_X_ADDRESS_OFFSET];
@@ -399,8 +403,6 @@ static void UpdateImuData(void)
399403
// clang-format on
400404
}
401405

402-
const Imu_t * GetImuDataPoint(void) { return &IMU_DATA; }
403-
404406
// clang-format off
405407

406408
/**

‎application/IMU/IMU_task.h

+7-8
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#ifndef INS_Task_H
2525
#define INS_Task_H
2626
#include "struct_typedef.h"
27+
#include "data_exchange.h"
2728

2829
// clang-format off
2930
#define SPI_DMA_GYRO_LENGHT 8
@@ -73,12 +74,12 @@
7374
#define INS_MAG_Z_ADDRESS_OFFSET 2
7475
// clang-format on
7576

76-
typedef struct __Imu
77-
{
78-
float yaw, pitch, roll; // rad
79-
float yaw_vel, pitch_vel, roll_vel; // rad/s
80-
float x_accel, y_accel, z_accel; // m/s^2
81-
} Imu_t;
77+
// typedef struct __Imu
78+
// {
79+
// float yaw, pitch, roll; // rad
80+
// float yaw_vel, pitch_vel, roll_vel; // rad/s
81+
// float x_accel, y_accel, z_accel; // m/s^2
82+
// } Imu_t;
8283

8384
extern void IMU_task(void const * pvParameters);
8485

@@ -96,6 +97,4 @@ extern const fp32 * get_accel_data_point(void);
9697

9798
extern const fp32 * get_mag_data_point(void);
9899

99-
extern const Imu_t * GetImuDataPoint(void);
100-
101100
#endif

‎application/assist/data_exchange.c

+37-33
Original file line numberDiff line numberDiff line change
@@ -17,53 +17,57 @@
1717

1818
#include "string.h"
1919

20-
#define DATA_LEN 4
20+
#define DATA_LIST_LEN 10
21+
#define NAME_LEN 16
22+
2123
typedef struct
2224
{
23-
uint8_t data[DATA_LEN];
24-
DataType_e data_type;
25+
void * data_address;
26+
uint32_t data_size;
27+
char data_name[NAME_LEN];
2528
} Data_t;
2629

27-
static Data_t DATA_BUFFER[Data_Exchange_INDEX_NUM] = {0};
30+
static Data_t DATA_LIST[DATA_LIST_LEN] = {0};
31+
static uint8_t USED_LEN = 0; // 已经使用的数据量
2832

2933
/**
3034
* @brief 发布数据
31-
* @param[in] index 数据索引
32-
* @param[in] data 发布的数据（统一存储为4个字节）
33-
* @retval none
35+
* @param[in] address 数据地址
36+
* @param[in] name 数据名称(最大长度为15字符)
37+
* @retval 数据发布状态
3438
*/
35-
void Publish(DataExchangeIndex_e index, uint8_t * data, DataType_e data_type)
39+
uint8_t Publish(void * address, char * name)
3640
{
37-
memcpy(&DATA_BUFFER[index], data, DATA_LEN);
38-
DATA_BUFFER[index].data_type = data_type;
41+
if (USED_LEN >= DATA_LIST_LEN) { // 判断数据列表已满
42+
return PUBLISH_ALREADY_FULL;
43+
}
44+
45+
for (uint8_t i = 0; i < USED_LEN; i++) { // 判断数据是否已经存在
46+
if (DATA_LIST[i].data_address == address) {
47+
return PUBLISH_ALREADY_EXIST;
48+
}
49+
}
50+
51+
// 保存数据
52+
// DATA_LIST[USED_LEN].data_address = address;
53+
memcpy(&DATA_LIST[USED_LEN].data_address, &address, 4);
54+
memcpy(DATA_LIST[USED_LEN].data_name, name, NAME_LEN);
55+
USED_LEN++;
56+
return PUBLISH_OK;
3957
}
4058

4159
/**
4260
* @brief 订阅数据
43-
* @param[in] index 数据索引
44-
* @param[in] out 输出数据的地址
45-
* @retval 订阅数据的起始地址，需使用memcpy将值拷贝出来
61+
* @param[in] name 数据名称
62+
* @retval 订阅数据的地址
4663
*/
47-
void Subscribe(DataExchangeIndex_e index, uint8_t * out)
64+
const void * Subscribe(char * name)
4865
{
49-
uint8_t data_len = 0;
50-
switch (DATA_BUFFER[index].data_type) {
51-
case DE_INT8:
52-
case DE_UINT8: {
53-
data_len = 1;
54-
} break;
55-
case DE_INT16:
56-
case DE_UINT16: {
57-
data_len = 2;
58-
} break;
59-
case DE_INT32:
60-
case DE_UINT32:
61-
case DE_FLOAT: {
62-
data_len = 4;
63-
} break;
64-
default:
65-
break;
66+
for (uint8_t i = 0; i < USED_LEN; i++) {
67+
if (strcmp(DATA_LIST[i].data_name, name) == 0) {
68+
return DATA_LIST[i].data_address;
69+
}
6670
}
6771

68-
memcpy(out, &DATA_BUFFER[index + DATA_LEN - data_len], data_len);
69-
}
72+
return NULL;
73+
}

‎application/assist/data_exchange.h

+19-3
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,23 @@ typedef enum __Data_Type {
3535
Data_Type_NUM
3636
} DataType_e;
3737

38-
extern void Publish(DataExchangeIndex_e index, uint8_t* data, DataType_e data_type);
39-
extern void Subscribe(DataExchangeIndex_e index, uint8_t * out);
38+
typedef enum DataPublishStatus {
39+
PUBLISH_FAIL = 0,
40+
PUBLISH_OK,
41+
PUBLISH_ALREADY_EXIST,
42+
PUBLISH_ALREADY_FULL
43+
} DataPublishStatus_e;
4044

41-
#endif // __DATA_EXCHANGE_H
45+
typedef enum DataSubscribeStatus { SUBSCRIBE_FAIL = 0, SUBSCRIBE_OK } DataSubscribeStatus_e;
46+
47+
typedef struct __Imu
48+
{
49+
float yaw, pitch, roll; // rad
50+
float yaw_vel, pitch_vel, roll_vel; // rad/s
51+
float x_accel, y_accel, z_accel; // m/s^2
52+
} Imu_t;
53+
54+
extern uint8_t Publish(void * address, char * name);
55+
extern const void * Subscribe(char * name);
56+
57+
#endif // __DATA_EXCHANGE_H

0 commit comments

Comments
 (0)