145 lines
4.3 KiB
C
145 lines
4.3 KiB
C
/*----------------------------------------------------------------------------
|
|
* Copyright (c) TJD Technologies Co., Ltd. 2020. All rights reserved.
|
|
*
|
|
* Description: service_hrsensor.c
|
|
*
|
|
* Author: liangjianfei
|
|
*
|
|
* Create: 2024-4-26
|
|
*--------------------------------------------------------------------------*/
|
|
|
|
#include "securec.h"
|
|
#include "sys_config.h"
|
|
#include "common_def.h"
|
|
#include "soc_osal.h"
|
|
#include "cmsis_os2.h"
|
|
#include "sys_typedef.h"
|
|
#include "task_service_timer.h"
|
|
#include "msensor_api.h"
|
|
#include "service_msensor.h"
|
|
#include "service_gsensor.h"
|
|
#include <stdio.h>
|
|
|
|
#define ENABLE_PRINT_INFO 1
|
|
#define ENABLE_DEBUG 1
|
|
|
|
#if ENABLE_PRINT_INFO
|
|
#define static_print_info(...) sys_ms_log_i(__VA_ARGS__) //一般信息打印宏控制
|
|
#define static_print_warn(...) sys_ms_log_w(__VA_ARGS__) //警告信息打印一般常开
|
|
#define static_print_error(...) sys_ms_log_e(__VA_ARGS__) //错误信息打印一般常开
|
|
#if ENABLE_DEBUG
|
|
#define static_print_debug(...) sys_ms_log_d(__VA_ARGS__)
|
|
#else
|
|
#define static_print_debug(...)
|
|
#endif
|
|
#else
|
|
#define static_print_info(...)
|
|
#define static_print_warn(...)
|
|
#define static_print_error(...)
|
|
#endif
|
|
|
|
static msensor_info *g_ms_driver = NULL;
|
|
static uint8_t g_calibration_status = 0;
|
|
static float gsen_data[3] = {0};
|
|
|
|
orientation_info_t ori_data = {0}; // 方位角
|
|
|
|
static signed int tjd_service_msensor_handle(void *param)
|
|
{
|
|
if (tjd_service_gs_get_fifo_count(GSEN_SERVICE_MSEN) != 0) {
|
|
gsensor_xyz_info gsen_synt_data;
|
|
tjd_service_gs_get_xyz_fifo(GSEN_SERVICE_MSEN, &gsen_synt_data, 1);
|
|
gsen_data[0] = -(gsen_synt_data.gsensor_x.raw_data*0.244)/1000;
|
|
gsen_data[1] = (gsen_synt_data.gsensor_y.raw_data*0.244)/1000;
|
|
gsen_data[2] = (gsen_synt_data.gsensor_z.raw_data*0.244)/1000;
|
|
|
|
// static_print_debug("gsen_data: %4.2f %4.2f %4.2f", gsen_data[0], gsen_data[1], gsen_data[2]);
|
|
}
|
|
|
|
if(g_calibration_status < 3){
|
|
g_calibration_status = g_ms_driver->timer_callback(gsen_data, &ori_data);
|
|
return 20; //定时执行间隔单位为ms
|
|
}
|
|
else{
|
|
g_calibration_status = g_ms_driver->timer_callback(gsen_data, &ori_data);
|
|
return 80; //定时执行间隔单位为ms
|
|
}
|
|
}
|
|
|
|
static int32_t tjd_service_msensor_start_send(void)
|
|
{
|
|
if (!g_ms_driver) {
|
|
static_print_error("service_msensor has not been open!");
|
|
return -1;
|
|
}
|
|
|
|
g_ms_driver->resume();
|
|
if(g_calibration_status < 3) g_ms_driver->alg_init();
|
|
queue_default_info_t msg_data = { tjd_service_msensor_handle, NULL, 20, NULL};
|
|
int32_t ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
|
|
if (ret != OSAL_SUCCESS) {
|
|
static_print_error("osal_msg_queue_write_copy fail! ret:%d", ret);
|
|
return ret;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void tjd_service_msensor_end_send(void)
|
|
{
|
|
if (!g_ms_driver) {
|
|
static_print_error("service_msensor has not been open!");
|
|
return;
|
|
}
|
|
|
|
g_ms_driver->suspend();
|
|
queue_default_info_t msg_data = { tjd_service_msensor_handle, NULL, 0, NULL};
|
|
osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
|
|
}
|
|
|
|
uint8_t tjd_service_msensor_open(void)
|
|
{
|
|
g_ms_driver = tjd_driver_ms_api_get_ops();
|
|
|
|
if (tjd_service_msensor_start_send() != 0) {
|
|
return -1;
|
|
}
|
|
|
|
static_print_info("tjd_service_msensor_open succ!");
|
|
return 0;
|
|
}
|
|
|
|
void tjd_service_msensor_close(void)
|
|
{
|
|
tjd_service_msensor_end_send();
|
|
g_ms_driver = NULL;
|
|
}
|
|
|
|
float tjd_service_msensor_get_azimuth_data(void)
|
|
{
|
|
if (g_ms_driver == NULL) {
|
|
static_print_error("tjd_service_msensor is not open");
|
|
return 0;
|
|
}
|
|
return ori_data.Azimuth;
|
|
}
|
|
|
|
float tjd_service_msensor_get_pitch_data(void)
|
|
{
|
|
if (g_ms_driver == NULL) {
|
|
static_print_error("tjd_service_msensor is not open");
|
|
return 0;
|
|
}
|
|
return ori_data.Pitch;
|
|
}
|
|
|
|
float tjd_service_msensor_get_roll_data(void)
|
|
{
|
|
if (g_ms_driver == NULL) {
|
|
static_print_error("tjd_service_msensor is not open");
|
|
return 0;
|
|
}
|
|
return ori_data.Roll;
|
|
}
|
|
|
|
int tjd_service_msensor_get_accuracy(void) {return g_ms_driver->get_accuracy();} |