129 lines
3.1 KiB
C
129 lines
3.1 KiB
C
/*******************************************************************************
|
|
* @file cmd_sensor.c
|
|
* @brief BLE command handlers
|
|
******************************************************************************/
|
|
#include <zephyr/sys/util.h>
|
|
#include <limits.h>
|
|
|
|
#include "cmd_common.h"
|
|
#include "main.h"
|
|
#include "debug_print.h"
|
|
#include "battery_adc.h"
|
|
#include "imu_i2c.h"
|
|
#include "led_control.h"
|
|
#include "cmd_sensor.h"
|
|
|
|
/*
|
|
* msn: battery voltage read
|
|
*/
|
|
int cmd_msn(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
ARG_UNUSED(data);
|
|
ARG_UNUSED(data_len);
|
|
|
|
int mv = battery_read_mv();
|
|
if (mv < 0)
|
|
{
|
|
mv = 0;
|
|
}
|
|
|
|
cmd_send_response_u16("rsn:", (uint16_t)mv);
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* msp: IMU data direct read
|
|
*/
|
|
int cmd_msp(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
ARG_UNUSED(data);
|
|
ARG_UNUSED(data_len);
|
|
|
|
if (processing)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
int16_t accel[3], gyro[3];
|
|
|
|
int ret = imu_read(accel, gyro);
|
|
if (ret != 0)
|
|
{
|
|
cmd_send_response_u16("rsp:", 0xFFFF);
|
|
DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret);
|
|
return 1;
|
|
}
|
|
|
|
cmd_send_response_imu(accel, gyro);
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* mst: IMU temperature direct read
|
|
*/
|
|
int cmd_mst(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
ARG_UNUSED(data);
|
|
ARG_UNUSED(data_len);
|
|
|
|
power_button_suspend(true);
|
|
|
|
int16_t t_cdeg = INT16_MIN;
|
|
int ret = imu_read_temperature_cdeg(&t_cdeg);
|
|
|
|
power_button_suspend(false);
|
|
|
|
if ((ret != 0) || (t_cdeg == INT16_MIN))
|
|
{
|
|
cmd_send_response_u16("rst:", 0xFFFF);
|
|
DBG_PRINTF("[CMD] mst: imu temp read fail ret=%d\r\n", ret);
|
|
return 1;
|
|
}
|
|
|
|
// 음수 온도도 2's complement로 그대로 전송 (앱이 int16로 해석)
|
|
cmd_send_response_u16("rst:", (uint16_t)t_cdeg);
|
|
DBG_PRINTF("[CMD] mst -> %d.%02d C\r\n", t_cdeg / 100, (t_cdeg < 0 ? -t_cdeg : t_cdeg) % 100);
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
* mls: LED status set
|
|
*/
|
|
int cmd_mls(const uint8_t *data, uint8_t data_len)
|
|
{
|
|
// 파라미터 부족: 에러 코드 0xFFFF 에코
|
|
if (data_len < 2)
|
|
{
|
|
cmd_send_response_u16("rls:", 0xFFFF);
|
|
DBG_PRINTF("[CMD] mls: no data\r\n");
|
|
return 1;
|
|
}
|
|
|
|
// state 필드: 2바이트 big-endian
|
|
uint16_t state = ((uint16_t)data[0] << 8) | (uint16_t)data[1];
|
|
|
|
// 범위 초과: 에러 코드 0xFFFE 에코
|
|
if (state > LED_STATE_ERROR)
|
|
{
|
|
cmd_send_response_u16("rls:", 0xFFFE);
|
|
DBG_PRINTF("[CMD] mls: invalid state %d\r\n", state);
|
|
return 1;
|
|
}
|
|
|
|
led_set_state((led_state_t)state);
|
|
|
|
// 정렬 LED 상태와 보호 판정 정책 연결
|
|
// - ALIGN_SEARCHING/ALIGN_COMPLETE 동안 보호 판정 차단
|
|
// - 다른 LED 상태에서는 기존 조건에 따라 보호 판정 재개
|
|
battery_protection_set_alignment_mode((state == LED_STATE_ALIGN_SEARCHING) || (state == LED_STATE_ALIGN_COMPLETE));
|
|
|
|
// OFF 상태에서는 정렬/측정용 FIFO를 함께 정리
|
|
if (state == LED_STATE_OFF)
|
|
{
|
|
(void)imu_fifo_stop();
|
|
}
|
|
|
|
cmd_send_response_u16("rls:", state);
|
|
return 1;
|
|
}
|