[Upload Code]:AI Thread Add Done
This commit is contained in:
parent
f83e1289e6
commit
efa7e1d88c
@ -56,6 +56,7 @@
|
|||||||
| 2025-05-14 | 增加Shell控制台、增加串口2用于蓝牙 | 增加调试接口 |
|
| 2025-05-14 | 增加Shell控制台、增加串口2用于蓝牙 | 增加调试接口 |
|
||||||
| 2025-05-16 | 更换芯片STM32F405 | 重新构建 |
|
| 2025-05-16 | 更换芯片STM32F405 | 重新构建 |
|
||||||
| 2025-05-19 | AI算法融合完毕，验证数据集可行 | 文件更新 |
|
| 2025-05-19 | AI算法融合完毕，验证数据集可行 | 文件更新 |
|
||||||
|
| 2025-05-20 | 新建AI线程 算法功能验证完成 | 线程增加 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@ -67,7 +68,7 @@
|
|||||||
|
|
||||||
- [ ] 传感器采集逻辑完成
|
- [ ] 传感器采集逻辑完成
|
||||||
|
|
||||||
- [ ] BLE 通信模块开发完成
|
- [ ] 无线模块开发完成
|
||||||
|
|
||||||
- [x] RTOS 集成调通
|
- [x] RTOS 集成调通
|
||||||
|
|
||||||
|
|||||||
@ -2,25 +2,46 @@
|
|||||||
#define MYEDGE_AI_APP_H
|
#define MYEDGE_AI_APP_H
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C"
|
||||||
|
{
|
||||||
#endif
|
#endif
|
||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "app_x-cube-ai.h"
|
#include "app_x-cube-ai.h"
|
||||||
|
|
||||||
|
void PictureCharArrayToFloat(const uint8_t *srcBuf, float *dstBuf, int len);
|
||||||
|
void preprocess_data(const float *input, float *output, size_t length);
|
||||||
|
void aiRun(const void *in_data, void *out_data);
|
||||||
|
int AiModel(uint8_t *input);
|
||||||
|
int aiInit(void);
|
||||||
|
|
||||||
|
extern const uint8_t test[1024];
|
||||||
|
extern float inputBuf[1024];
|
||||||
|
|
||||||
void PictureCharArrayToFloat(const uint8_t *srcBuf, float *dstBuf, int len);
|
extern float aiInData[AI_MODEL_IN_1_SIZE];
|
||||||
void preprocess_data(const float *input, float *output, size_t length);
|
extern float aiOutData[AI_MODEL_OUT_1_SIZE];
|
||||||
void aiRun(const void *in_data, void *out_data);
|
|
||||||
int aiInit(void);
|
|
||||||
|
|
||||||
extern const uint8_t test[1024];
|
typedef enum
|
||||||
extern float inputBuf[1024];
|
{
|
||||||
|
AI_STATUS_IDLE = 0, // 空闲状态
|
||||||
|
AI_STATUS_COMPLETED, // 完成推理
|
||||||
|
AI_STATUS_ERROR // 发生错误
|
||||||
|
} ai_status_t;
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
POSTURE_SUPINE = 0, // 平躺
|
||||||
|
POSTURE_LEFT_SIDE, // 左侧卧
|
||||||
|
POSTURE_RIGHT_SIDE // 右侧卧
|
||||||
|
} posture_t;
|
||||||
|
|
||||||
|
typedef struct model_T
|
||||||
|
{
|
||||||
|
char result; // 结果 观察枚举类型 0 平躺 1 左侧 2 右侧
|
||||||
|
char status; // 状态 0 没有运算 1 运算完成
|
||||||
|
float confidence; // 结果的置信率
|
||||||
|
float ai_output[3]; // 结果数组
|
||||||
|
} model_t;
|
||||||
|
|
||||||
extern float aiInData[AI_MODEL_IN_1_SIZE];
|
|
||||||
extern float aiOutData[AI_MODEL_OUT_1_SIZE];
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -91,24 +91,7 @@ int main(void)
|
|||||||
MX_USART2_UART_Init();
|
MX_USART2_UART_Init();
|
||||||
MX_X_CUBE_AI_Init();
|
MX_X_CUBE_AI_Init();
|
||||||
/* USER CODE BEGIN 2 */
|
/* USER CODE BEGIN 2 */
|
||||||
aiInit();
|
|
||||||
|
|
||||||
rt_kprintf("\n covFloat Start 1 Tick : %ld\n", rt_tick_get());
|
|
||||||
PictureCharArrayToFloat(test, inputBuf, 1024);
|
|
||||||
rt_kprintf("\n covFloat End 1 Tick : %ld\n", rt_tick_get());
|
|
||||||
|
|
||||||
rt_kprintf("\n alg Start 2 Tick : %ld\n", rt_tick_get());
|
|
||||||
preprocess_data((const float *)inputBuf, aiInData, 1024);
|
|
||||||
rt_kprintf("\n alg End 2 Tick : %ld\n", rt_tick_get());
|
|
||||||
|
|
||||||
rt_kprintf("\n ai Start 3 Tick : %ld\n", rt_tick_get());
|
|
||||||
aiRun(aiInData, aiOutData);
|
|
||||||
rt_kprintf("\n ai End 3 Tick : %ld\n", rt_tick_get());
|
|
||||||
|
|
||||||
for (int i = 0; i < AI_MODEL_OUT_1_SIZE; i++)
|
|
||||||
{
|
|
||||||
rt_kprintf("\n aiOutData[%d] = 0.%d\n", i, (int)(aiOutData[i] * 100.0f));
|
|
||||||
}
|
|
||||||
/* USER CODE END 2 */
|
/* USER CODE END 2 */
|
||||||
|
|
||||||
/* Infinite loop */
|
/* Infinite loop */
|
||||||
|
|||||||
@ -6,7 +6,12 @@
|
|||||||
#include "myEdge_ai_app.h"
|
#include "myEdge_ai_app.h"
|
||||||
#include "app_x-cube-ai.h"
|
#include "app_x-cube-ai.h"
|
||||||
|
|
||||||
|
model_t model;
|
||||||
ai_handle network;
|
ai_handle network;
|
||||||
|
|
||||||
|
ai_buffer *ai_input;
|
||||||
|
ai_buffer *ai_output;
|
||||||
|
|
||||||
AI_ALIGNED(32)
|
AI_ALIGNED(32)
|
||||||
float aiInData[AI_MODEL_IN_1_SIZE];
|
float aiInData[AI_MODEL_IN_1_SIZE];
|
||||||
|
|
||||||
@ -16,9 +21,6 @@ float aiOutData[AI_MODEL_OUT_1_SIZE];
|
|||||||
AI_ALIGNED(32)
|
AI_ALIGNED(32)
|
||||||
ai_u8 activations[AI_MODEL_DATA_ACTIVATIONS_SIZE];
|
ai_u8 activations[AI_MODEL_DATA_ACTIVATIONS_SIZE];
|
||||||
|
|
||||||
ai_buffer *ai_input;
|
|
||||||
ai_buffer *ai_output;
|
|
||||||
|
|
||||||
AI_ALIGNED(32)
|
AI_ALIGNED(32)
|
||||||
const uint8_t test[1024] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 14, 46, 99, 64, 42, 20, 7, 0, 0, 3, 5, 37, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 9, 18, 46, 178, 171, 223, 198, 210, 147, 30, 9, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 23, 56, 78, 79, 84, 62, 77, 109, 188, 138, 97, 61, 18, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 14, 35, 60, 78, 55, 30, 35, 33, 35, 6, 0, 7, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 0, 17, 25, 20, 16, 20, 5, 0, 5, 18, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 60, 32, 27, 23, 10, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 16, 46, 37, 46, 38, 12, 6, 0, 0, 0, 15, 4, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 7, 17, 51, 40, 45, 43, 49, 28, 15, 5, 6, 9, 11, 32, 11, 10, 37, 54, 15, 9, 24, 15, 10, 4, 4, 7, 10, 61, 0, 0, 0, 5, 12, 43, 52, 39, 61, 62, 36, 24, 13, 9, 5, 9, 18, 33, 30, 17, 55, 33, 37, 97, 100, 93, 37, 73, 53, 45, 24, 105, 0, 0, 0, 7, 23, 62, 85, 50, 58, 42, 38, 28, 18, 21, 11, 14, 20, 49, 59, 46, 68, 103, 118, 184, 72, 47, 24, 9, 26, 120, 0, 170, 0, 0, 4, 14, 53, 79, 88, 56, 80, 48, 43, 41, 23, 17, 13, 33, 39, 12, 6, 15, 45, 53, 31, 90, 105, 125, 59, 36, 32, 10, 9, 6, 0, 0, 3, 14, 25, 57, 83, 43, 26, 23, 16, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 39, 55, 26, 25, 24, 20, 17, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 29, 77, 58, 45, 29, 23, 38, 30, 8, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 14, 32, 45, 51, 46, 56, 49, 32, 31, 26, 29, 7, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 95, 96, 93, 99, 104, 108, 88, 63, 130, 64, 35, 13, 5, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 21, 64, 89, 90, 90, 94, 104, 99, 84, 72, 31, 19, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 24, 64, 154, 128, 150, 179, 150, 167, 120, 76, 68, 33, 12, 10, 7, 14, 0, 0, 5, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 34, 85, 119, 70, 75, 131, 160, 152, 88, 54, 40, 28, 8, 5, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 42, 79, 51, 46, 59, 73, 72, 72, 100, 75, 89, 74, 42, 12, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 7, 15, 18, 18, 29, 40, 40, 36, 43, 56, 88, 57, 22, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 40, 46, 57, 52, 53, 56, 65, 35, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 36, 51, 85, 72, 73, 88, 67, 61, 49, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 19, 56, 55, 53, 28, 36, 39, 24, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 14, 35, 64, 79, 64, 89, 44, 7, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 22, 24, 53, 95, 92, 69, 20, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 20, 73, 159, 109, 98, 34, 14, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 22, 55, 51, 69, 49, 49, 23, 11, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 33, 54, 64, 72, 66, 21, 13, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 18, 32, 35, 76, 64, 18, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 15, 25, 25, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
const uint8_t test[1024] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 14, 46, 99, 64, 42, 20, 7, 0, 0, 3, 5, 37, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 9, 18, 46, 178, 171, 223, 198, 210, 147, 30, 9, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 23, 56, 78, 79, 84, 62, 77, 109, 188, 138, 97, 61, 18, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 14, 35, 60, 78, 55, 30, 35, 33, 35, 6, 0, 7, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 0, 17, 25, 20, 16, 20, 5, 0, 5, 18, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 60, 32, 27, 23, 10, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 16, 46, 37, 46, 38, 12, 6, 0, 0, 0, 15, 4, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 7, 17, 51, 40, 45, 43, 49, 28, 15, 5, 6, 9, 11, 32, 11, 10, 37, 54, 15, 9, 24, 15, 10, 4, 4, 7, 10, 61, 0, 0, 0, 5, 12, 43, 52, 39, 61, 62, 36, 24, 13, 9, 5, 9, 18, 33, 30, 17, 55, 33, 37, 97, 100, 93, 37, 73, 53, 45, 24, 105, 0, 0, 0, 7, 23, 62, 85, 50, 58, 42, 38, 28, 18, 21, 11, 14, 20, 49, 59, 46, 68, 103, 118, 184, 72, 47, 24, 9, 26, 120, 0, 170, 0, 0, 4, 14, 53, 79, 88, 56, 80, 48, 43, 41, 23, 17, 13, 33, 39, 12, 6, 15, 45, 53, 31, 90, 105, 125, 59, 36, 32, 10, 9, 6, 0, 0, 3, 14, 25, 57, 83, 43, 26, 23, 16, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 39, 55, 26, 25, 24, 20, 17, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 29, 77, 58, 45, 29, 23, 38, 30, 8, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 14, 32, 45, 51, 46, 56, 49, 32, 31, 26, 29, 7, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 95, 96, 93, 99, 104, 108, 88, 63, 130, 64, 35, 13, 5, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 21, 64, 89, 90, 90, 94, 104, 99, 84, 72, 31, 19, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 24, 64, 154, 128, 150, 179, 150, 167, 120, 76, 68, 33, 12, 10, 7, 14, 0, 0, 5, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 34, 85, 119, 70, 75, 131, 160, 152, 88, 54, 40, 28, 8, 5, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 42, 79, 51, 46, 59, 73, 72, 72, 100, 75, 89, 74, 42, 12, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 7, 15, 18, 18, 29, 40, 40, 36, 43, 56, 88, 57, 22, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 40, 46, 57, 52, 53, 56, 65, 35, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 36, 51, 85, 72, 73, 88, 67, 61, 49, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 19, 56, 55, 53, 28, 36, 39, 24, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 14, 35, 64, 79, 64, 89, 44, 7, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 22, 24, 53, 95, 92, 69, 20, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 20, 73, 159, 109, 98, 34, 14, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 22, 55, 51, 69, 49, 49, 23, 11, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 33, 54, 64, 72, 66, 21, 13, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 18, 32, 35, 76, 64, 18, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17, 15, 25, 25, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
@ -110,6 +112,7 @@ int aiInit(void)
|
|||||||
/* Reteive pointers to the model's input/output tensors */
|
/* Reteive pointers to the model's input/output tensors */
|
||||||
ai_input = ai_model_inputs_get(network, NULL);
|
ai_input = ai_model_inputs_get(network, NULL);
|
||||||
ai_output = ai_model_outputs_get(network, NULL);
|
ai_output = ai_model_outputs_get(network, NULL);
|
||||||
|
rt_memset(&model, 0, sizeof(model));
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
/*
|
/*
|
||||||
@ -132,5 +135,81 @@ void aiRun(const void *in_data, void *out_data)
|
|||||||
err = ai_model_get_error(network);
|
err = ai_model_get_error(network);
|
||||||
rt_kprintf("AI ai_network_run error - type=%d code=%d\r\n", err.type, err.code);
|
rt_kprintf("AI ai_network_run error - type=%d code=%d\r\n", err.type, err.code);
|
||||||
}
|
}
|
||||||
///////////////////////////////////////////////////////////////////////////////////
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 测试代码
|
||||||
|
int AiModel(uint8_t *input)
|
||||||
|
{
|
||||||
|
//
|
||||||
|
int res = 0;
|
||||||
|
rt_tick_t Start_Tick = 0;
|
||||||
|
rt_tick_t End_Tick = 0;
|
||||||
|
int predicted_class = 0;
|
||||||
|
float max_confidence = 0.0f;
|
||||||
|
|
||||||
|
rt_kprintf("\nStart Test Code\n");
|
||||||
|
PictureCharArrayToFloat(input, inputBuf, 1024);
|
||||||
|
|
||||||
|
preprocess_data((const float *)inputBuf, aiInData, 1024);
|
||||||
|
|
||||||
|
Start_Tick = rt_tick_get();
|
||||||
|
aiRun(aiInData, aiOutData);
|
||||||
|
End_Tick = rt_tick_get();
|
||||||
|
|
||||||
|
rt_kprintf("AI model UseTime : %ld ms\n", End_Tick - Start_Tick);
|
||||||
|
for (int i = 0; i < AI_MODEL_OUT_1_SIZE; i++)
|
||||||
|
{
|
||||||
|
if (aiOutData[i] > max_confidence)
|
||||||
|
{
|
||||||
|
max_confidence = aiOutData[i];
|
||||||
|
predicted_class = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
res = predicted_class;
|
||||||
|
model.result = predicted_class;
|
||||||
|
model.confidence = max_confidence;
|
||||||
|
model.status = AI_STATUS_COMPLETED;
|
||||||
|
// Map class to posture name for debugging
|
||||||
|
const char *posture_names[] = {"Supine", "Left Side", "Right Side"};
|
||||||
|
rt_kprintf("Detected posture: %s (confidence: %d%%)\n",
|
||||||
|
posture_names[predicted_class], (int)(model.confidence * 100.0f));
|
||||||
|
rt_memcpy(model.ai_output, aiOutData, sizeof(aiOutData));
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
/* 定义线程栈与控制块（静态分配） */
|
||||||
|
#define AI_THREAD_STACK_SIZE 1024
|
||||||
|
static struct rt_thread ai_thread;
|
||||||
|
static rt_uint8_t ai_thread_stack[AI_THREAD_STACK_SIZE];
|
||||||
|
|
||||||
|
/* 线程入口函数 */
|
||||||
|
static void ai_thread_entry(void *parameter)
|
||||||
|
{
|
||||||
|
aiInit();
|
||||||
|
while (1)
|
||||||
|
{
|
||||||
|
if (model.status == AI_STATUS_IDLE)
|
||||||
|
AiModel((uint8_t *)test);
|
||||||
|
rt_thread_mdelay(1000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 初始化函数，使用静态线程启动 AI 运算 */
|
||||||
|
int ai_thread_init(void)
|
||||||
|
{
|
||||||
|
rt_thread_init(&ai_thread, // 线程控制块
|
||||||
|
"ai_task", // 名称
|
||||||
|
ai_thread_entry, // 入口函数
|
||||||
|
RT_NULL, // 参数
|
||||||
|
&ai_thread_stack[0], // 栈起始地址
|
||||||
|
sizeof(ai_thread_stack), // 栈大小
|
||||||
|
20, // 优先级
|
||||||
|
10); // 时间片
|
||||||
|
rt_thread_startup(&ai_thread); // 启动线程
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
INIT_APP_EXPORT(ai_thread_init);
|
||||||
|
|||||||
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
Binary file not shown.
BIN
资料/协议文档/床垫串口协议.pdf
Normal file
BIN
资料/协议文档/床垫串口协议.pdf
Normal file
Binary file not shown.
BIN
资料/协议文档/程序框图.pdf
Normal file
BIN
资料/协议文档/程序框图.pdf
Normal file
Binary file not shown.
836
资料/算法文件/mcu_controller.c
Normal file
836
资料/算法文件/mcu_controller.c
Normal file
@ -0,0 +1,836 @@
|
|||||||
|
/**
|
||||||
|
* MCU Mattress Controller - C Implementation
|
||||||
|
*
|
||||||
|
* This program implements sleep posture detection and mattress control for MCUs.
|
||||||
|
* It uses TensorFlow Lite for Microcontrollers for inference.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include "tensorflow/lite/micro/all_ops_resolver.h"
|
||||||
|
#include "tensorflow/lite/micro/micro_error_reporter.h"
|
||||||
|
#include "tensorflow/lite/micro/micro_interpreter.h"
|
||||||
|
#include "tensorflow/lite/schema/schema_generated.h"
|
||||||
|
|
||||||
|
// If model data is included as a C array
|
||||||
|
#include "model_tflite.h" // Replace with your model header
|
||||||
|
|
||||||
|
// Constants for mode definitions
|
||||||
|
#define MODE_MANUAL 0
|
||||||
|
#define MODE_ADAPTIVE 1
|
||||||
|
#define MODE_PREVENTION 2
|
||||||
|
#define MODE_ABAB 3
|
||||||
|
#define MODE_TURNING 4
|
||||||
|
#define MODE_FLOATING 5
|
||||||
|
|
||||||
|
// Command codes from remote controller
|
||||||
|
#define CMD_STOP 0x00
|
||||||
|
#define CMD_ADAPTIVE_MODE 0x01
|
||||||
|
#define CMD_PREVENTION_MODE 0x02
|
||||||
|
#define CMD_ABAB_MODE 0x03
|
||||||
|
#define CMD_TURNING_MODE 0x04
|
||||||
|
#define CMD_FLOATING_MODE 0x05
|
||||||
|
#define CMD_DETECT_POSTURE 0x06
|
||||||
|
|
||||||
|
// Posture definitions
|
||||||
|
#define POSTURE_SUPINE 0 // Supine (back)
|
||||||
|
#define POSTURE_LEFT 1 // Left side
|
||||||
|
#define POSTURE_RIGHT 2 // Right side
|
||||||
|
|
||||||
|
// Serial port defines - platform specific
|
||||||
|
#define MATTRESS_PORT "/dev/ttyS1"
|
||||||
|
#define SENSOR_PORT "/dev/ttyS2"
|
||||||
|
#define REMOTE_PORT "/dev/ttyS0"
|
||||||
|
|
||||||
|
// Data structures
|
||||||
|
typedef struct {
|
||||||
|
int posture_index;
|
||||||
|
float confidence;
|
||||||
|
bool success;
|
||||||
|
} PostureResult;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
// Serial handles
|
||||||
|
void* mattress_ser;
|
||||||
|
void* sensor_ser;
|
||||||
|
void* remote_ser;
|
||||||
|
|
||||||
|
// Thread management
|
||||||
|
pthread_t running_threads[5]; // Support up to 5 concurrent threads
|
||||||
|
int thread_count;
|
||||||
|
pthread_mutex_t thread_mutex;
|
||||||
|
bool stop_event;
|
||||||
|
|
||||||
|
// TFLite model
|
||||||
|
tflite::MicroErrorReporter error_reporter;
|
||||||
|
tflite::AllOpsResolver resolver;
|
||||||
|
const tflite::Model* model;
|
||||||
|
tflite::MicroInterpreter* interpreter;
|
||||||
|
TfLiteTensor* input_tensor;
|
||||||
|
TfLiteTensor* output_tensor;
|
||||||
|
bool model_loaded;
|
||||||
|
bool is_quantized;
|
||||||
|
|
||||||
|
// Device ID bytes for mattress commands
|
||||||
|
uint8_t id_bytes[4];
|
||||||
|
|
||||||
|
// Current posture tracking
|
||||||
|
int current_posture;
|
||||||
|
float confidence;
|
||||||
|
|
||||||
|
// Memory buffer for TensorFlow Lite arena
|
||||||
|
uint8_t tensor_arena[128 * 1024]; // 128KB tensor arena
|
||||||
|
} MCUController;
|
||||||
|
|
||||||
|
// Forward declarations for threading
|
||||||
|
void* adaptive_thread_func(void* arg);
|
||||||
|
void* prevention_thread_func(void* arg);
|
||||||
|
void* abab_thread_func(void* arg);
|
||||||
|
void* turning_thread_func(void* arg);
|
||||||
|
void* floating_thread_func(void* arg);
|
||||||
|
|
||||||
|
// Serial communication stubs (platform-specific implementations needed)
|
||||||
|
bool serial_open(void** handle, const char* port, int baudrate);
|
||||||
|
void serial_close(void* handle);
|
||||||
|
bool serial_write(void* handle, const uint8_t* data, size_t length);
|
||||||
|
int serial_read(void* handle, uint8_t* buffer, size_t max_length);
|
||||||
|
int serial_available(void* handle);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Initialize the controller
|
||||||
|
*/
|
||||||
|
bool controller_init(MCUController* controller) {
|
||||||
|
memset(controller, 0, sizeof(MCUController));
|
||||||
|
|
||||||
|
// Initialize mutex
|
||||||
|
pthread_mutex_init(&controller->thread_mutex, NULL);
|
||||||
|
|
||||||
|
// Initialize device ID bytes
|
||||||
|
controller->id_bytes[0] = 0x01;
|
||||||
|
controller->id_bytes[1] = 0x02;
|
||||||
|
controller->id_bytes[2] = 0x03;
|
||||||
|
controller->id_bytes[3] = 0x04;
|
||||||
|
|
||||||
|
// Load TFLite model
|
||||||
|
controller->model = tflite::GetModel(g_model_tflite); // Use the model data from header
|
||||||
|
if (controller->model->version() != TFLITE_SCHEMA_VERSION) {
|
||||||
|
printf("Model schema version mismatch!\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create interpreter
|
||||||
|
static tflite::MicroInterpreter static_interpreter(
|
||||||
|
controller->model,
|
||||||
|
controller->resolver,
|
||||||
|
controller->tensor_arena,
|
||||||
|
sizeof(controller->tensor_arena),
|
||||||
|
&controller->error_reporter);
|
||||||
|
|
||||||
|
controller->interpreter = &static_interpreter;
|
||||||
|
|
||||||
|
// Allocate tensors
|
||||||
|
TfLiteStatus allocate_status = controller->interpreter->AllocateTensors();
|
||||||
|
if (allocate_status != kTfLiteOk) {
|
||||||
|
printf("Failed to allocate tensors!\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get input and output tensors
|
||||||
|
controller->input_tensor = controller->interpreter->input(0);
|
||||||
|
controller->output_tensor = controller->interpreter->output(0);
|
||||||
|
|
||||||
|
// Check if model is quantized
|
||||||
|
controller->is_quantized = controller->input_tensor->type == kTfLiteInt8;
|
||||||
|
|
||||||
|
printf("Model loaded successfully\n");
|
||||||
|
printf("Model is %s\n", controller->is_quantized ? "quantized" : "floating-point");
|
||||||
|
|
||||||
|
controller->model_loaded = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Connect to mattress
|
||||||
|
*/
|
||||||
|
bool connect_mattress(MCUController* controller, const char* port, int baudrate) {
|
||||||
|
// Close existing connection if any
|
||||||
|
if (controller->mattress_ser) {
|
||||||
|
serial_close(controller->mattress_ser);
|
||||||
|
controller->mattress_ser = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Open new connection
|
||||||
|
if (serial_open(&controller->mattress_ser, port, baudrate)) {
|
||||||
|
printf("Connected to mattress on %s\n", port);
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
printf("Failed to connect to mattress on %s\n", port);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Connect to sensor
|
||||||
|
*/
|
||||||
|
bool connect_sensor(MCUController* controller, const char* port, int baudrate) {
|
||||||
|
// Close existing connection if any
|
||||||
|
if (controller->sensor_ser) {
|
||||||
|
serial_close(controller->sensor_ser);
|
||||||
|
controller->sensor_ser = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Open new connection
|
||||||
|
if (serial_open(&controller->sensor_ser, port, baudrate)) {
|
||||||
|
printf("Connected to sensor on %s\n", port);
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
printf("Failed to connect to sensor on %s\n", port);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Disconnect from all devices
|
||||||
|
*/
|
||||||
|
void disconnect_all(MCUController* controller) {
|
||||||
|
// Stop all running threads
|
||||||
|
stop_current_mode(controller);
|
||||||
|
|
||||||
|
// Close serial connections
|
||||||
|
if (controller->mattress_ser) {
|
||||||
|
serial_close(controller->mattress_ser);
|
||||||
|
controller->mattress_ser = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (controller->sensor_ser) {
|
||||||
|
serial_close(controller->sensor_ser);
|
||||||
|
controller->sensor_ser = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (controller->remote_ser) {
|
||||||
|
serial_close(controller->remote_ser);
|
||||||
|
controller->remote_ser = NULL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Read pressure data from sensor
|
||||||
|
*/
|
||||||
|
bool read_pressure_data(MCUController* controller, float* data, size_t data_size) {
|
||||||
|
if (!controller->sensor_ser) {
|
||||||
|
printf("Sensor not connected\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Send command to request data
|
||||||
|
uint8_t cmd[] = {0xAA, 0x01, 0xBB};
|
||||||
|
serial_write(controller->sensor_ser, cmd, sizeof(cmd));
|
||||||
|
|
||||||
|
// Wait for response
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 0;
|
||||||
|
ts.tv_nsec = 100000000; // 100ms
|
||||||
|
nanosleep(&ts, NULL);
|
||||||
|
|
||||||
|
// Calculate expected data size (1024 float values = 4096 bytes)
|
||||||
|
size_t expected_bytes = data_size * sizeof(float);
|
||||||
|
|
||||||
|
// Check if enough data is available
|
||||||
|
if (serial_available(controller->sensor_ser) >= expected_bytes) {
|
||||||
|
// Read raw bytes
|
||||||
|
uint8_t raw_data[expected_bytes];
|
||||||
|
size_t bytes_read = serial_read(controller->sensor_ser, raw_data, expected_bytes);
|
||||||
|
|
||||||
|
if (bytes_read == expected_bytes) {
|
||||||
|
// Convert raw bytes to float array
|
||||||
|
memcpy(data, raw_data, expected_bytes);
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
printf("Incomplete data: got %zu bytes, expected %zu\n", bytes_read, expected_bytes);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
printf("Insufficient data: %d bytes available\n", serial_available(controller->sensor_ser));
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Preprocess the raw sensor data
|
||||||
|
*/
|
||||||
|
void preprocess_data(const float* raw_data, float* processed_data, size_t data_size) {
|
||||||
|
const float clip_min = 0.0f;
|
||||||
|
const float clip_max = 300.0f;
|
||||||
|
const float epsilon = 1e-6f;
|
||||||
|
|
||||||
|
// Calculate mean and std_dev
|
||||||
|
float sum = 0.0f;
|
||||||
|
float sum_sq = 0.0f;
|
||||||
|
|
||||||
|
// First pass: calculate mean
|
||||||
|
for (size_t i = 0; i < data_size; i++) {
|
||||||
|
// Apply clipping
|
||||||
|
float value = raw_data[i];
|
||||||
|
if (value < clip_min) value = clip_min;
|
||||||
|
if (value > clip_max) value = clip_max;
|
||||||
|
|
||||||
|
// Sqrt(x+1)
|
||||||
|
value = sqrtf(value + 1.0f);
|
||||||
|
|
||||||
|
// Accumulate for mean calculation
|
||||||
|
sum += value;
|
||||||
|
sum_sq += value * value;
|
||||||
|
}
|
||||||
|
|
||||||
|
float mean = sum / data_size;
|
||||||
|
float variance = (sum_sq / data_size) - (mean * mean);
|
||||||
|
float std_dev = sqrtf(variance) + epsilon;
|
||||||
|
|
||||||
|
// Second pass: apply normalization, tanh, and scaling
|
||||||
|
for (size_t i = 0; i < data_size; i++) {
|
||||||
|
// Apply clipping
|
||||||
|
float value = raw_data[i];
|
||||||
|
if (value < clip_min) value = clip_min;
|
||||||
|
if (value > clip_max) value = clip_max;
|
||||||
|
|
||||||
|
// Sqrt(x+1)
|
||||||
|
value = sqrtf(value + 1.0f);
|
||||||
|
|
||||||
|
// Normalize
|
||||||
|
value = (value - mean) / std_dev;
|
||||||
|
|
||||||
|
// Apply tanh
|
||||||
|
value = tanhf(value);
|
||||||
|
|
||||||
|
// Scale to [0,1]
|
||||||
|
value = (value + 1.0f) / 2.0f;
|
||||||
|
|
||||||
|
processed_data[i] = value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Detect sleep posture from sensor data
|
||||||
|
*/
|
||||||
|
PostureResult detect_posture(MCUController* controller) {
|
||||||
|
PostureResult result = {0};
|
||||||
|
result.success = false;
|
||||||
|
|
||||||
|
if (!controller->model_loaded) {
|
||||||
|
printf("Model not loaded\n");
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Read pressure data
|
||||||
|
float raw_data[1024];
|
||||||
|
if (!read_pressure_data(controller, raw_data, 1024)) {
|
||||||
|
printf("Failed to read pressure data\n");
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Preprocess data
|
||||||
|
float processed_data[1024];
|
||||||
|
preprocess_data(raw_data, processed_data, 1024);
|
||||||
|
|
||||||
|
// Copy data to input tensor
|
||||||
|
if (controller->is_quantized) {
|
||||||
|
// For quantized model, quantize the input
|
||||||
|
int8_t* input_data = controller->input_tensor->data.int8;
|
||||||
|
float input_scale = controller->input_tensor->params.scale;
|
||||||
|
int input_zero_point = controller->input_tensor->params.zero_point;
|
||||||
|
|
||||||
|
for (int i = 0; i < 1024; i++) {
|
||||||
|
input_data[i] = (int8_t)(processed_data[i] / input_scale + input_zero_point);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// For float model, copy directly
|
||||||
|
float* input_data = controller->input_tensor->data.f;
|
||||||
|
memcpy(input_data, processed_data, 1024 * sizeof(float));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Run inference
|
||||||
|
TfLiteStatus invoke_status = controller->interpreter->Invoke();
|
||||||
|
if (invoke_status != kTfLiteOk) {
|
||||||
|
printf("Invoke failed\n");
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get output
|
||||||
|
float output_values[3]; // Assuming 3 classes
|
||||||
|
|
||||||
|
if (controller->is_quantized) {
|
||||||
|
// Dequantize output for quantized model
|
||||||
|
int8_t* output_data = controller->output_tensor->data.int8;
|
||||||
|
float output_scale = controller->output_tensor->params.scale;
|
||||||
|
int output_zero_point = controller->output_tensor->params.zero_point;
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; i++) {
|
||||||
|
output_values[i] = (output_data[i] - output_zero_point) * output_scale;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// Copy output for float model
|
||||||
|
float* output_data = controller->output_tensor->data.f;
|
||||||
|
memcpy(output_values, output_data, 3 * sizeof(float));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Find predicted class and confidence
|
||||||
|
int predicted_class = 0;
|
||||||
|
float max_confidence = output_values[0];
|
||||||
|
|
||||||
|
for (int i = 1; i < 3; i++) {
|
||||||
|
if (output_values[i] > max_confidence) {
|
||||||
|
max_confidence = output_values[i];
|
||||||
|
predicted_class = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set result
|
||||||
|
result.posture_index = predicted_class;
|
||||||
|
result.confidence = max_confidence * 100.0f; // Convert to percentage
|
||||||
|
result.success = true;
|
||||||
|
|
||||||
|
// Store current posture and confidence
|
||||||
|
controller->current_posture = predicted_class;
|
||||||
|
controller->confidence = result.confidence;
|
||||||
|
|
||||||
|
// Map class to posture name for debugging
|
||||||
|
const char* posture_names[] = {"Supine", "Left Side", "Right Side"};
|
||||||
|
printf("Detected posture: %s (confidence: %.1f%%)\n",
|
||||||
|
posture_names[predicted_class], result.confidence);
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Send command to the mattress controller
|
||||||
|
*/
|
||||||
|
bool send_mattress_command(MCUController* controller, uint8_t mode_byte, uint8_t* args, size_t args_length) {
|
||||||
|
if (!controller->mattress_ser) {
|
||||||
|
printf("Mattress not connected\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create command packet
|
||||||
|
uint8_t packet[20]; // Allocate enough space for command
|
||||||
|
size_t packet_index = 0;
|
||||||
|
|
||||||
|
// Start byte
|
||||||
|
packet[packet_index++] = 0xAA;
|
||||||
|
|
||||||
|
// Mode byte
|
||||||
|
packet[packet_index++] = mode_byte;
|
||||||
|
|
||||||
|
// Parameter bytes
|
||||||
|
for (size_t i = 0; i < args_length && packet_index < 14; i++) {
|
||||||
|
packet[packet_index++] = args[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pad to fixed length if needed
|
||||||
|
while (packet_index < 14) {
|
||||||
|
packet[packet_index++] = 0x00;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add ID bytes
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
packet[packet_index++] = controller->id_bytes[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// End byte
|
||||||
|
packet[packet_index++] = 0xBB;
|
||||||
|
|
||||||
|
// Send packet
|
||||||
|
if (serial_write(controller->mattress_ser, packet, packet_index)) {
|
||||||
|
printf("Sent mattress command: mode=%d\n", mode_byte);
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
printf("Failed to send mattress command\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Stop the current operating mode
|
||||||
|
*/
|
||||||
|
bool stop_current_mode(MCUController* controller) {
|
||||||
|
// Set stop event
|
||||||
|
controller->stop_event = true;
|
||||||
|
|
||||||
|
// Wait for threads to finish (simple implementation)
|
||||||
|
// In a real-time system, you would use proper thread joining with timeout
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 1;
|
||||||
|
ts.tv_nsec = 0;
|
||||||
|
nanosleep(&ts, NULL);
|
||||||
|
|
||||||
|
// Reset thread count and stop event
|
||||||
|
pthread_mutex_lock(&controller->thread_mutex);
|
||||||
|
controller->thread_count = 0;
|
||||||
|
controller->stop_event = false;
|
||||||
|
pthread_mutex_unlock(&controller->thread_mutex);
|
||||||
|
|
||||||
|
// Send stop command to mattress
|
||||||
|
if (controller->mattress_ser) {
|
||||||
|
uint8_t args[1] = {0};
|
||||||
|
send_mattress_command(controller, MODE_MANUAL, args, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Run adaptive mode based on posture detection
|
||||||
|
*/
|
||||||
|
bool run_adaptive_mode(MCUController* controller, int detection_interval, int runtime) {
|
||||||
|
if (!controller->mattress_ser || !controller->sensor_ser) {
|
||||||
|
printf("Both mattress and sensor must be connected\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Stop any current mode
|
||||||
|
stop_current_mode(controller);
|
||||||
|
|
||||||
|
// Create thread arguments (allocated on heap to avoid stack issues)
|
||||||
|
typedef struct {
|
||||||
|
MCUController* controller;
|
||||||
|
int detection_interval;
|
||||||
|
int runtime;
|
||||||
|
} AdaptiveArgs;
|
||||||
|
|
||||||
|
AdaptiveArgs* args = (AdaptiveArgs*)malloc(sizeof(AdaptiveArgs));
|
||||||
|
args->controller = controller;
|
||||||
|
args->detection_interval = detection_interval;
|
||||||
|
args->runtime = runtime;
|
||||||
|
|
||||||
|
// Start adaptive thread
|
||||||
|
pthread_mutex_lock(&controller->thread_mutex);
|
||||||
|
|
||||||
|
if (controller->thread_count >= 5) {
|
||||||
|
printf("Too many threads running\n");
|
||||||
|
pthread_mutex_unlock(&controller->thread_mutex);
|
||||||
|
free(args);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
pthread_t thread;
|
||||||
|
if (pthread_create(&thread, NULL, adaptive_thread_func, args) != 0) {
|
||||||
|
printf("Failed to create adaptive thread\n");
|
||||||
|
pthread_mutex_unlock(&controller->thread_mutex);
|
||||||
|
free(args);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
controller->running_threads[controller->thread_count++] = thread;
|
||||||
|
pthread_mutex_unlock(&controller->thread_mutex);
|
||||||
|
|
||||||
|
printf("Started adaptive mode\n");
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Implementation of adaptive thread function
|
||||||
|
void* adaptive_thread_func(void* arg) {
|
||||||
|
typedef struct {
|
||||||
|
MCUController* controller;
|
||||||
|
int detection_interval;
|
||||||
|
int runtime;
|
||||||
|
} AdaptiveArgs;
|
||||||
|
|
||||||
|
AdaptiveArgs* args = (AdaptiveArgs*)arg;
|
||||||
|
MCUController* controller = args->controller;
|
||||||
|
int detection_interval = args->detection_interval;
|
||||||
|
int runtime = args->runtime;
|
||||||
|
|
||||||
|
// Free args struct - we've copied the data
|
||||||
|
free(args);
|
||||||
|
|
||||||
|
printf("Adaptive thread starting (interval=%d, runtime=%d)\n", detection_interval, runtime);
|
||||||
|
|
||||||
|
int adjustment_count = 0;
|
||||||
|
time_t start_time = time(NULL);
|
||||||
|
|
||||||
|
while (!controller->stop_event) {
|
||||||
|
// Check if runtime exceeded
|
||||||
|
time_t current_time = time(NULL);
|
||||||
|
int elapsed_time = (int)(current_time - start_time);
|
||||||
|
|
||||||
|
if (elapsed_time >= runtime) {
|
||||||
|
printf("Adaptive mode completed after %d seconds\n", elapsed_time);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate progress percentage
|
||||||
|
int progress = (elapsed_time * 100) / runtime;
|
||||||
|
printf("Adaptive mode progress: %d%%\n", progress);
|
||||||
|
|
||||||
|
// Detect current posture
|
||||||
|
PostureResult result = detect_posture(controller);
|
||||||
|
|
||||||
|
if (result.success) {
|
||||||
|
int posture_index = result.posture_index;
|
||||||
|
float confidence = result.confidence;
|
||||||
|
|
||||||
|
// Only adjust if confidence is high enough
|
||||||
|
if (confidence >= 70.0f) {
|
||||||
|
uint8_t args[4];
|
||||||
|
|
||||||
|
// Adjust mattress based on posture
|
||||||
|
if (posture_index == POSTURE_SUPINE) {
|
||||||
|
// Supine: neutral position
|
||||||
|
args[0] = 50;
|
||||||
|
args[1] = 50;
|
||||||
|
args[2] = 50;
|
||||||
|
args[3] = 50;
|
||||||
|
} else if (posture_index == POSTURE_LEFT) {
|
||||||
|
// Left side: adjust for comfort
|
||||||
|
args[0] = 70;
|
||||||
|
args[1] = 40;
|
||||||
|
args[2] = 70;
|
||||||
|
args[3] = 40;
|
||||||
|
} else if (posture_index == POSTURE_RIGHT) {
|
||||||
|
// Right side: adjust for comfort
|
||||||
|
args[0] = 40;
|
||||||
|
args[1] = 70;
|
||||||
|
args[2] = 40;
|
||||||
|
args[3] = 70;
|
||||||
|
}
|
||||||
|
|
||||||
|
send_mattress_command(controller, MODE_MANUAL, args, 4);
|
||||||
|
adjustment_count++;
|
||||||
|
|
||||||
|
const char* posture_names[] = {"Supine", "Left Side", "Right Side"};
|
||||||
|
printf("Adjusted mattress for %s (adjustment #%d)\n",
|
||||||
|
posture_names[posture_index], adjustment_count);
|
||||||
|
} else {
|
||||||
|
printf("Low confidence (%.1f%%), no adjustment made\n", confidence);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
printf("Posture detection failed, no adjustment made\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait for next detection interval or until stopped
|
||||||
|
for (int i = 0; i < detection_interval && !controller->stop_event; i++) {
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 1;
|
||||||
|
ts.tv_nsec = 0;
|
||||||
|
nanosleep(&ts, NULL);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset to manual mode when finished
|
||||||
|
if (!controller->stop_event) {
|
||||||
|
uint8_t args[1] = {0};
|
||||||
|
send_mattress_command(controller, MODE_MANUAL, args, 0);
|
||||||
|
printf("Adaptive mode completed\n");
|
||||||
|
} else {
|
||||||
|
printf("Adaptive mode stopped\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Process a command from the remote controller
|
||||||
|
*/
|
||||||
|
bool process_remote_command(MCUController* controller, uint8_t command_byte, uint8_t param1, uint8_t param2) {
|
||||||
|
printf("Processing remote command: cmd=%d, param1=%d, param2=%d\n", command_byte, param1, param2);
|
||||||
|
|
||||||
|
switch (command_byte) {
|
||||||
|
case CMD_STOP:
|
||||||
|
return stop_current_mode(controller);
|
||||||
|
|
||||||
|
case CMD_ADAPTIVE_MODE: {
|
||||||
|
// param1: interval in minutes, param2: runtime in hours
|
||||||
|
int interval = param1 > 0 ? param1 * 60 : 300;
|
||||||
|
int runtime = param2 > 0 ? param2 * 3600 : 3600;
|
||||||
|
return run_adaptive_mode(controller, interval, runtime);
|
||||||
|
}
|
||||||
|
|
||||||
|
case CMD_PREVENTION_MODE: {
|
||||||
|
// Implementation would be similar to adaptive mode
|
||||||
|
printf("Prevention mode not fully implemented\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
case CMD_ABAB_MODE: {
|
||||||
|
// Implementation would be similar to adaptive mode
|
||||||
|
printf("ABAB mode not fully implemented\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
case CMD_TURNING_MODE: {
|
||||||
|
// Implementation would be similar to adaptive mode
|
||||||
|
printf("Turning mode not fully implemented\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
case CMD_FLOATING_MODE: {
|
||||||
|
// Implementation would be similar to adaptive mode
|
||||||
|
printf("Floating mode not fully implemented\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
case CMD_DETECT_POSTURE: {
|
||||||
|
// Just detect and report posture
|
||||||
|
PostureResult result = detect_posture(controller);
|
||||||
|
return result.success;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
printf("Unknown command: %d\n", command_byte);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Listen for remote controller commands
|
||||||
|
*/
|
||||||
|
void* remote_listener_thread(void* arg) {
|
||||||
|
MCUController* controller = (MCUController*)arg;
|
||||||
|
|
||||||
|
// Buffer for received bytes
|
||||||
|
uint8_t buffer[16];
|
||||||
|
int buffer_index = 0;
|
||||||
|
|
||||||
|
while (1) {
|
||||||
|
// Check if data is available
|
||||||
|
if (serial_available(controller->remote_ser) > 0) {
|
||||||
|
// Read one byte
|
||||||
|
uint8_t byte;
|
||||||
|
if (serial_read(controller->remote_ser, &byte, 1) == 1) {
|
||||||
|
buffer[buffer_index++] = byte;
|
||||||
|
|
||||||
|
// Check for complete command (3 bytes)
|
||||||
|
if (buffer_index >= 3) {
|
||||||
|
// Process command
|
||||||
|
uint8_t cmd = buffer[0];
|
||||||
|
uint8_t param1 = buffer[1];
|
||||||
|
uint8_t param2 = buffer[2];
|
||||||
|
|
||||||
|
printf("Received command: %d %d %d\n", cmd, param1, param2);
|
||||||
|
|
||||||
|
// Process the command
|
||||||
|
process_remote_command(controller, cmd, param1, param2);
|
||||||
|
|
||||||
|
// Clear buffer
|
||||||
|
buffer_index = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Small delay to prevent CPU overuse
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 0;
|
||||||
|
ts.tv_nsec = 10000000; // 10ms
|
||||||
|
nanosleep(&ts, NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start remote controller listener
|
||||||
|
*/
|
||||||
|
bool start_remote_listener(MCUController* controller, const char* port, int baudrate) {
|
||||||
|
// Connect to remote controller
|
||||||
|
if (controller->remote_ser) {
|
||||||
|
serial_close(controller->remote_ser);
|
||||||
|
controller->remote_ser = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!serial_open(&controller->remote_ser, port, baudrate)) {
|
||||||
|
printf("Failed to connect to remote controller on %s\n", port);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("Connected to remote controller on %s\n", port);
|
||||||
|
|
||||||
|
// Start listener thread
|
||||||
|
pthread_t thread;
|
||||||
|
if (pthread_create(&thread, NULL, remote_listener_thread, controller) != 0) {
|
||||||
|
printf("Failed to create remote listener thread\n");
|
||||||
|
serial_close(controller->remote_ser);
|
||||||
|
controller->remote_ser = NULL;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Make thread detached so it cleans up automatically
|
||||||
|
pthread_detach(thread);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Main entry point
|
||||||
|
*/
|
||||||
|
int main() {
|
||||||
|
// Create controller
|
||||||
|
MCUController controller;
|
||||||
|
|
||||||
|
printf("Initializing MCU Mattress Controller...\n");
|
||||||
|
|
||||||
|
// Initialize controller
|
||||||
|
if (!controller_init(&controller)) {
|
||||||
|
printf("Failed to initialize controller\n");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Connect to devices
|
||||||
|
connect_mattress(&controller, MATTRESS_PORT, 115200);
|
||||||
|
connect_sensor(&controller, SENSOR_PORT, 115200);
|
||||||
|
|
||||||
|
// Start remote controller listener
|
||||||
|
start_remote_listener(&controller, REMOTE_PORT, 9600);
|
||||||
|
|
||||||
|
// Main loop
|
||||||
|
printf("Controller running...\n");
|
||||||
|
|
||||||
|
// In a real implementation, you might need some form of
|
||||||
|
// application main loop here. For simplicity, we just sleep.
|
||||||
|
while (1) {
|
||||||
|
struct timespec ts;
|
||||||
|
ts.tv_sec = 1;
|
||||||
|
ts.tv_nsec = 0;
|
||||||
|
nanosleep(&ts, NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clean up (unreachable in this simple example)
|
||||||
|
disconnect_all(&controller);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Platform-specific serial communication implementations would go here
|
||||||
|
// These are just stubs and would need to be implemented for your specific platform
|
||||||
|
|
||||||
|
bool serial_open(void** handle, const char* port, int baudrate) {
|
||||||
|
// Implementation depends on your platform (UART, USART, etc.)
|
||||||
|
printf("Opening serial port %s at %d baud\n", port, baudrate);
|
||||||
|
*handle = malloc(1); // Dummy handle
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void serial_close(void* handle) {
|
||||||
|
// Implementation depends on your platform
|
||||||
|
if (handle) free(handle);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool serial_write(void* handle, const uint8_t* data, size_t length) {
|
||||||
|
// Implementation depends on your platform
|
||||||
|
printf("Writing %zu bytes to serial port\n", length);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
int serial_read(void* handle, uint8_t* buffer, size_t max_length) {
|
||||||
|
// Implementation depends on your platform
|
||||||
|
return 0; // Return number of bytes read
|
||||||
|
}
|
||||||
|
|
||||||
|
int serial_available(void* handle) {
|
||||||
|
// Implementation depends on your platform
|
||||||
|
return 0; // Return number of bytes available
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user