가속도 센서로 속도 계산, 차량 견인 감지 하기

// 속도를 저장할 구조체 (예시)
typedef struct {
    float x, y, z;
} Velocity;

// 가속도(g)를 m/s^2 단위로 변환하는 상수 (1g = 9.8m/s^2)
#define GRAVITY_MSS 9.80665f

/**
 * @brief 중력을 제거한 선형 가속도를 계산하고, 이를 적분하여 속도를 추정한다.
 * @param imu_data 현재 센서 데이터 (가속도는 g 단위)
 * @param current_angles 현재 기울기 정보 (Pitch, Roll은 degree 단위)
 * @param velocity 이전 속도 값이자, 새로운 속도를 저장할 포인터 (m/s 단위)
 * @param dt 시간 변화량 (초)
 */
void update_velocity(const ImuData* imu_data, const Angles* current_angles, Velocity* velocity, float dt) {
    // 1. 현재 기울기를 이용해 중력 벡터 성분을 계산한다 (센서 좌표계 기준).
    // 실제 구현에서는 더 정확한 회전 변환을 사용해야 한다.
    float roll_rad = current_angles->roll * (M_PI / 180.0f);
    float pitch_rad = current_angles->pitch * (M_PI / 180.0f);
    float gravity_x_component = sin(pitch_rad);
    float gravity_y_component = -cos(pitch_rad) * sin(roll_rad);
    
    // 2. 센서 측정값에서 중력 성분을 뺀다 (선형 가속도).
    float linear_accel_x_g = imu_data->accel_x - gravity_x_component;
    float linear_accel_y_g = imu_data->accel_y - gravity_y_component;
    
    // 3. 가속도(g)를 m/s^2 단위로 변환한다.
    float linear_accel_x_mss = linear_accel_x_g * GRAVITY_MSS;
    float linear_accel_y_mss = linear_accel_y_g * GRAVITY_MSS;

    // 4. 속도 계산 (v = v0 + a*t)
    velocity->x += linear_accel_x_mss * dt;
    velocity->y += linear_accel_y_mss * dt;
}

#define TILT_THRESHOLD_DEG 5.0      // 5도 이상 기울어지면 감지
#define ACCEL_THRESHOLD_G 0.1       // 0.1g 이상의 순수 선형 가속도가 감지되면 (수평 이동)

/**
 * @brief 차량 견인 상황을 감지하는 함수
 * @param imu_data 현재 센서 데이터
 * @param is_ignition_on 차량 시동 상태 (true: ON, false: OFF)
 * @return 견인 의심 상황이면 true 반환
 */
bool check_towing_scenario(const ImuData* imu_data, bool is_ignition_on) {
    // 시동이 켜져 있으면 정상 주행이므로 감지 안 함
    if (is_ignition_on) {
        return false;
    }

    // 1. 기울기 변화 감지 (견인차에 들릴 때)
    Angles current_tilt = {0};
    // calculate_tilt 함수는 "가속도계(Accelerometer)와 방향 계산" 글에서 정의된 함수
    calculate_tilt(imu_data, &current_tilt); 

    if (fabs(current_tilt.pitch) > TILT_THRESHOLD_DEG || fabs(current_tilt.roll) > TILT_THRESHOLD_DEG) {
        // 실제 제품에서는 '일정 시간 이상' 기울기가 유지되는지 확인하는 로직 추가
        return true; 
    }

    // 2. 선형 가속도 감지 (앞에서 끌려갈 때)
    // (간단한 예시로, Z축 가속도가 중력(1g)에서 크게 벗어나지 않으면 수평 이동으로 간주)
    float horizontal_accel_x = imu_data->accel_x; // 대략적인 수평 가속도
    float horizontal_accel_y = imu_data->accel_y; // 대략적인 수평 가속도
    float total_horizontal_accel_g = sqrt(horizontal_accel_x * horizontal_accel_x + horizontal_accel_y * horizontal_accel_y);

    if (total_horizontal_accel_g > ACCEL_THRESHOLD_G) {
        // 실제 제품에서는 '일정 시간 이상' 가속도가 유지되는지 확인하는 로직 추가
        return true;
    }

    return false;
}