暂存：数据发送协议完善中
This commit is contained in:
@ -35,6 +35,9 @@
|
||||
#else
|
||||
#define xlog(format, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储XXX个sensor_data_t元素
|
||||
|
||||
//
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -42,6 +45,7 @@
|
||||
//START -- 函数定义
|
||||
void send_data_to_ble_client(const u8* data, u16 length);
|
||||
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
|
||||
extern void close_process(u16* pid,char* name);
|
||||
//END -- 函数定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -49,492 +53,109 @@ extern void create_process(u16* pid, const char* name, void *priv, void (*func)(
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//START -- 变量定义
|
||||
|
||||
// --- 任务ID ---
|
||||
static u16 xtell_i2c_test_id;
|
||||
static u16 collect_data_id;
|
||||
static u16 ble_send_data_id;
|
||||
static u16 sensor_read_data_id;
|
||||
static u16 calculate_data_id;
|
||||
|
||||
// --- 环形缓冲区 ---
|
||||
#define SENSOR_DATA_BUFFER_SIZE 512
|
||||
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
|
||||
static circle_buffer_t sensor_cb;
|
||||
typedef struct {
|
||||
// -- 六轴 --
|
||||
signed short acc_data[3];
|
||||
signed short gyr_data[3];
|
||||
// -- 磁力计 --
|
||||
uint8_t mmc5603nj_buffer[9];
|
||||
// -- 速度 --
|
||||
int speed_cms;
|
||||
// -- 气压计 --
|
||||
//...
|
||||
} BLE_send_data_t;
|
||||
|
||||
static int count = 0;
|
||||
|
||||
//--- test ---
|
||||
// 全局变量
|
||||
u16 gsensor_id=0;
|
||||
u16 test_id=0;
|
||||
// --- 环形缓冲区 ---
|
||||
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
|
||||
|
||||
|
||||
//END -- 变量定义
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
BLE_send_data_t BLE_send_data;
|
||||
void test(){
|
||||
signed short acc_data_buf[3] = {0};
|
||||
signed short gyr_data_buf[3] = {0};
|
||||
signed short acc_gyro_input[6] = {0};
|
||||
float Angle_output[3] = {0};
|
||||
// xlog("============start\n");
|
||||
|
||||
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
||||
|
||||
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
|
||||
u8 data[50];
|
||||
data[0] = 0xBB;
|
||||
data[1] = 0xBE;
|
||||
data[2] = 0x01;
|
||||
data[3] = sizeof(BLE_send_data_t); //后续包的数据长度
|
||||
// send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
|
||||
memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t));
|
||||
|
||||
static int count = 0;
|
||||
if(count >=10){
|
||||
count = 0;
|
||||
|
||||
char* division = "==========\n";
|
||||
send_data_to_ble_client(division,strlen(division));
|
||||
|
||||
char log_buffer[100]; // 100个字符应该足够了
|
||||
|
||||
// 使用 snprintf 进行格式化
|
||||
int num_chars_written = snprintf(
|
||||
log_buffer, // 目标缓冲区
|
||||
sizeof(log_buffer), // 目标缓冲区的最大容量
|
||||
"s %d, %dcm/s, %dcm\n", // 格式化字符串
|
||||
BLE_send_data.skiing_state, // 第一个 %d 的参数
|
||||
BLE_send_data.speed_cms, // 第二个 %d 的参数
|
||||
BLE_send_data.distance_cm // 第三个 %d 的参数
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
extern BLE_KS_send_data_t KS_data;
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"Acc:%d, %d, %d\n",
|
||||
KS_data.acc_KS[0],KS_data.acc_KS[1],KS_data.acc_KS[2]
|
||||
); // cm/s^2
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"Gyr_dps:%d, %d, %d\n",
|
||||
KS_data.gyr_KS_dps[0],
|
||||
KS_data.gyr_KS_dps[1],
|
||||
KS_data.gyr_KS_dps[2]
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"angle: %d, %d, %d\n",
|
||||
KS_data.angle_KS[0],
|
||||
KS_data.angle_KS[1],
|
||||
KS_data.angle_KS[2]
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
extern debug_t debug1;
|
||||
extern debug_t debug2;
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"debug:%.2f,%.2f,%.2f(%.2f),%.2f\n",
|
||||
debug1.acc_variance,
|
||||
debug1.gyr_variance,
|
||||
debug1.acc_magnitude,
|
||||
debug2.acc_magnitude, //滤波后的加速度
|
||||
debug1.gyr_magnitude
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
xlog("Call interval\n");
|
||||
}
|
||||
count++;
|
||||
|
||||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||
memset(&data, 0, 50);
|
||||
// xlog("end============\n");
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储100个sensor_data_t元素
|
||||
static circle_buffer_t sensor_read; // 环形缓冲区管理结构体
|
||||
typedef struct {
|
||||
signed short acc_data[3];
|
||||
signed short gyr_data[3];
|
||||
float angle[3]; //pitch roll yaw
|
||||
float quaternion_output[4]; //四元数数据
|
||||
} sensor_data_t;
|
||||
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
|
||||
|
||||
static circle_buffer_t sensor_send; // 环形缓冲区管理结构体
|
||||
static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble要发送的数据
|
||||
|
||||
static u8 mutex1 = 0;
|
||||
static u8 mutex2 = 0;
|
||||
static int count_test1 = 0;
|
||||
static int count_test2 = 0;
|
||||
/**
|
||||
* @brief //读取传感器的数据放进缓冲区
|
||||
* @brief 六轴静态校准
|
||||
*
|
||||
*/
|
||||
void sensor_read_data(){
|
||||
|
||||
void SC7U22_static_calibration(void){
|
||||
signed short acc_data_buf[3];
|
||||
signed short gyr_data_buf[3];
|
||||
float angle[3];
|
||||
float quaternion_output[3];
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static int calibration_done = 0;
|
||||
char status = 0;
|
||||
set_SC7U22_Error_Flag(0);
|
||||
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);
|
||||
|
||||
if(status == 1){ //校准完成
|
||||
extern u16 SC7U22_calibration_id;
|
||||
extern u8 SC7U22_init;
|
||||
SC7U22_init = 1;
|
||||
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
|
||||
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x01};
|
||||
send_data_to_ble_client(&send2_1,strlen(send2_1));
|
||||
}
|
||||
// if(count > 100){
|
||||
// count = 0;
|
||||
// char log_buffer[100];
|
||||
// // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
|
||||
// // send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
// xlog("status:%d\n", status);
|
||||
// xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
|
||||
// }
|
||||
// count++;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 开始采集传感器数据和计算速度
|
||||
*
|
||||
*/
|
||||
void start_collect_fuc(void){
|
||||
// xlog("=======sensor_read_data START\n");
|
||||
|
||||
static signed short combined_raw_data[6];
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
char status = 0;
|
||||
if(circle_buffer_is_full(&sensor_read)){
|
||||
// xlog("sensor_read_data: read buffer full\n");
|
||||
return;
|
||||
}
|
||||
BLE_send_data_t BLE_send_data;
|
||||
uint8_t mmc5603nj_buffer[9];
|
||||
signed short acc_data_buf[3];
|
||||
signed short gyr_data_buf[3];
|
||||
float angle[3];
|
||||
float quaternion_output[3];
|
||||
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
||||
mmc5603nj_read_origin_data(mmc5603nj_buffer);
|
||||
|
||||
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
|
||||
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
|
||||
|
||||
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
int speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
|
||||
|
||||
static sensor_data_t tmp;
|
||||
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
|
||||
// xlog("=======sensor_read_data middle 1\n");
|
||||
memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
|
||||
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
// status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
if(count > 100){
|
||||
count = 0;
|
||||
char log_buffer[100]; // 100个字符应该足够了
|
||||
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
|
||||
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
xlog("status:%d\n", status);
|
||||
}
|
||||
count++;
|
||||
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
// printf("Calculate the time interval =============== start\n");
|
||||
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
if(mutex1 == 0){
|
||||
mutex1 = 1;
|
||||
// count_test1++;
|
||||
// xlog("count_test_1: %d\n",count_test1);
|
||||
circle_buffer_write(&sensor_read, &tmp);
|
||||
mutex1 = 0;
|
||||
}
|
||||
extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ;
|
||||
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
|
||||
}
|
||||
// xlog("=======sensor_read_data END\n");
|
||||
|
||||
}
|
||||
|
||||
void calculate_data(){
|
||||
// xlog("=======start\n");
|
||||
sensor_data_t tmp;
|
||||
|
||||
if(circle_buffer_is_empty(&sensor_read)){
|
||||
// xlog("sensor_read_buffer: read buffer empty\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if(mutex1 == 0){
|
||||
mutex1 = 1;
|
||||
circle_buffer_read(&sensor_read, &tmp);
|
||||
mutex1 = 0;
|
||||
}else{
|
||||
return;
|
||||
}
|
||||
|
||||
// -- 数据包装进结构体 --
|
||||
|
||||
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle,tmp.quaternion_output);
|
||||
memcpy(BLE_send_data.acc_data, acc_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(BLE_send_data.gyr_data, gyr_data_buf, 3 * sizeof(signed short));
|
||||
memcpy(BLE_send_data.mmc5603nj_buffer, mmc5603nj_buffer, 9);
|
||||
BLE_send_data.speed_cms = speed;
|
||||
|
||||
|
||||
|
||||
if(circle_buffer_is_full(&sensor_send))
|
||||
return;
|
||||
|
||||
if(mutex2 == 0){
|
||||
mutex2 = 1;
|
||||
circle_buffer_write(&sensor_send, &data_by_calculate);
|
||||
mutex2 = 0;
|
||||
}
|
||||
|
||||
|
||||
// extern void BLE_send_data();
|
||||
// BLE_send_data();
|
||||
// xlog("=======end\n");
|
||||
}
|
||||
|
||||
extern char xt_Check_Flag;
|
||||
void BLE_send_data(){
|
||||
// xlog("=======start\n");
|
||||
if(circle_buffer_is_empty(&sensor_send)){
|
||||
// xlog("sensor_send_buffer: send buffer empty\n");
|
||||
return;
|
||||
}
|
||||
#ifdef XTELL_TEST
|
||||
// #if 0
|
||||
BLE_send_data_t tmp;
|
||||
if(mutex2 == 0){
|
||||
mutex2 = 1;
|
||||
circle_buffer_read(&sensor_send, &tmp);
|
||||
mutex2 = 0;
|
||||
}else{
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if(count >=100){
|
||||
// extern debug_t debug2;
|
||||
// xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm);
|
||||
// xlog("Acc:%d, %d, %d\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]);
|
||||
// xlog("Gyr:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
|
||||
// xlog("debug2.acc_magnitude:%.2f\n", debug2.acc_magnitude);
|
||||
|
||||
int num_chars_written;
|
||||
count = 0;
|
||||
char* division = "==========\n";
|
||||
send_data_to_ble_client(division,strlen(division));
|
||||
char log_buffer[100]; // 100个字符应该足够了
|
||||
|
||||
|
||||
// extern char iic_read_len;
|
||||
// extern char iic_write_result;
|
||||
// num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d, %d, %d\n", xt_Check_Flag, iic_read_len, iic_write_result);
|
||||
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
#if 1
|
||||
// 使用 snprintf 进行格式化
|
||||
num_chars_written = snprintf(
|
||||
log_buffer, // 目标缓冲区
|
||||
sizeof(log_buffer), // 目标缓冲区的最大容量
|
||||
"s %d, %dcm/s, %dcm\n", // 格式化字符串
|
||||
tmp.skiing_state, // 第一个 %d 的参数
|
||||
tmp.speed_cms, // 第二个 %d 的参数
|
||||
tmp.distance_cm // 第三个 %d 的参数
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer, strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"Acc:%d, %d, %d\n",
|
||||
tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"Gyr:%d, %d, %d\n",
|
||||
tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"Angle:%.1f, %.1f, %1.f\n",
|
||||
tmp.angle_data[0],tmp.angle_data[1],tmp.angle_data[2]
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
#endif
|
||||
short acc_mo_cms = sqrtf(tmp.acc_data[0]*tmp.acc_data[0] + tmp.acc_data[1]*tmp.acc_data[1] + tmp.acc_data[2]*tmp.acc_data[2])-900;
|
||||
memset(&log_buffer, 0, 100);
|
||||
num_chars_written = snprintf(
|
||||
log_buffer,
|
||||
sizeof(log_buffer),
|
||||
"acc_cm/s^2:%d\n",
|
||||
acc_mo_cms
|
||||
);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
// xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm);
|
||||
// xlog("Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]);
|
||||
// xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
|
||||
}
|
||||
count++;
|
||||
// xlog("=======end\n");
|
||||
#else
|
||||
#endif
|
||||
}
|
||||
|
||||
//iic测试调用的
|
||||
#if 0
|
||||
|
||||
static u16 xt_iic_test_id;
|
||||
char log_buffer_1[100];
|
||||
extern char sen_log_buffer_1[100];
|
||||
extern char sen_log_buffer_2[100];
|
||||
extern char sen_log_buffer_3[100];
|
||||
extern char sen_log_buffer_4[100];
|
||||
extern char sen_log_buffer_5[100];
|
||||
extern char w_log_buffer_1[100];
|
||||
extern char w_log_buffer_2[100];
|
||||
extern char w_log_buffer_3[100];
|
||||
extern char w_log_buffer_4[100];
|
||||
extern char w_log_buffer_5[100];
|
||||
void xt_iic_test(){
|
||||
|
||||
char log_buffer[100];
|
||||
send_data_to_ble_client(&log_buffer_1,strlen(log_buffer_1));
|
||||
extern char iic_read_len;
|
||||
extern char iic_write_result;
|
||||
int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d,%d,%d\n", xt_Check_Flag, iic_read_len, iic_write_result);
|
||||
extern void send_data_to_ble_client(const u8* data, u16 length);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
|
||||
if(sen_log_buffer_1 != NULL)
|
||||
send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1));
|
||||
if(sen_log_buffer_2 != NULL)
|
||||
send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2));
|
||||
if(sen_log_buffer_3 != NULL)
|
||||
send_data_to_ble_client(&sen_log_buffer_3,strlen(sen_log_buffer_3));
|
||||
if(sen_log_buffer_4 != NULL)
|
||||
send_data_to_ble_client(&sen_log_buffer_4,strlen(sen_log_buffer_4));
|
||||
if(sen_log_buffer_5 != NULL)
|
||||
send_data_to_ble_client(&sen_log_buffer_5,strlen(sen_log_buffer_5));
|
||||
|
||||
if(w_log_buffer_1 != NULL)
|
||||
send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1));
|
||||
if(w_log_buffer_2 != NULL)
|
||||
send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2));
|
||||
if(w_log_buffer_3 != NULL)
|
||||
send_data_to_ble_client(&w_log_buffer_3,strlen(w_log_buffer_3));
|
||||
if(w_log_buffer_4 != NULL)
|
||||
send_data_to_ble_client(&w_log_buffer_4,strlen(w_log_buffer_4));
|
||||
if(w_log_buffer_5 != NULL)
|
||||
send_data_to_ble_client(&w_log_buffer_5,strlen(w_log_buffer_5));
|
||||
|
||||
// SL_SC7U22_Config();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
u16 xt_iic_test_id;
|
||||
char hw_iic_init_result;
|
||||
void xt_hw_iic_test(){
|
||||
char log_buffer[100];
|
||||
extern char iic_read_len;
|
||||
extern char iic_write_result;
|
||||
|
||||
int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"init result:%d, SL_SC7U22_Check=0x%d,%d,%d\n",hw_iic_init_result, xt_Check_Flag, iic_read_len, iic_write_result);
|
||||
extern void send_data_to_ble_client(const u8* data, u16 length);
|
||||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
}
|
||||
#endif
|
||||
|
||||
void sensor_measure(void){
|
||||
// xlog("=======sensor_read_data START\n");
|
||||
static signed short combined_raw_data[6];
|
||||
static int initialized = 0;
|
||||
static int calibration_done = 0;
|
||||
char status = 0;
|
||||
|
||||
|
||||
static sensor_data_t tmp;
|
||||
mmc5603nj_mag_data_t mag_data;
|
||||
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
|
||||
// os_time_dly(1);
|
||||
mmc5603nj_read_mag_data(&mag_data);
|
||||
// xlog("=======sensor_read_data middle 1\n");
|
||||
memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short));
|
||||
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
|
||||
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
// status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
if(count > 100){
|
||||
count = 0;
|
||||
char log_buffer[100];
|
||||
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
|
||||
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||||
xlog("status:%d\n", status);
|
||||
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
|
||||
}
|
||||
count++;
|
||||
|
||||
if (status == 1) {
|
||||
calibration_done = 1;
|
||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||
}
|
||||
} else {
|
||||
// printf("Calculate the time interval =============== start\n");
|
||||
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
// status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
|
||||
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
|
||||
|
||||
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output);
|
||||
extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ;
|
||||
ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);
|
||||
}
|
||||
|
||||
// mmc5603nj_mag_data_t mag_data;
|
||||
// mmc5603nj_read_mag_data(&mag_data);
|
||||
// float temperature = mmc5603nj_get_temperature();
|
||||
// count_test1++;
|
||||
// if(count_test1 >500){
|
||||
// count_test1 =0;
|
||||
// xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z);
|
||||
// }
|
||||
|
||||
// xlog("=======sensor_read_data END\n");
|
||||
}
|
||||
|
||||
|
||||
void xtell_task_create(void){
|
||||
|
||||
// int ret = hw_iic_init(0);
|
||||
// xlog("hw_iic_init result:%d\n",ret);
|
||||
// //初始化传感器
|
||||
// SL_SC7U22_Config();
|
||||
|
||||
#if TCFG_GSENOR_USER_IIC_TYPE
|
||||
|
||||
int ret = hw_iic_init(0);
|
||||
xlog("init iic result:%d\n", ret); //返回0成功
|
||||
#else
|
||||
@ -543,52 +164,13 @@ void xtell_task_create(void){
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
// os_time_dly(10);
|
||||
// delay_2ms(10);
|
||||
|
||||
|
||||
|
||||
// if(bmp280_init() != 0){
|
||||
// xlog("bmp280 init error\n");
|
||||
// }
|
||||
// float temp, press;
|
||||
// bmp280_read_data(&temp, &press);
|
||||
// xlog("get temp: %d, get press: %d\n",temp, press);
|
||||
|
||||
|
||||
|
||||
// MPU9250_Mag_Init();
|
||||
//iic总线设备扫描
|
||||
// extern void i2c_scanner_probe(void);
|
||||
// i2c_scanner_probe();
|
||||
|
||||
extern void i2c_scanner_probe(void);
|
||||
i2c_scanner_probe();
|
||||
|
||||
xlog("xtell_task_create\n");
|
||||
// 初始化环形缓冲区
|
||||
// circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
|
||||
|
||||
|
||||
ano_protocol_init(115200);
|
||||
|
||||
|
||||
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));
|
||||
|
||||
circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
|
||||
//
|
||||
// create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4);
|
||||
|
||||
// create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
|
||||
|
||||
#if 0
|
||||
hw_iic_init_result = ret;
|
||||
create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000);
|
||||
#endif
|
||||
circle_buffer_init(&BLE_send_buff, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user