QDAC-firmware/app/gl_garage.c

#include "zf_common_headfile.h"
#include "math.h"
#include "gl_headfile.h"

enum garage_type_e garage_type = GARAGE_NONE;

float (*garage_rpts)[2];
int garage_rpts_num;

float calculate_vector_angle(float x1, float y1, float x2, float y2)
{
    float dx = x2 - x1;
    float dy = y2 - y1;

    float vector_length = Q_sqrt(dx * dx + dy * dy);
    float angle_radians = acos(dx / vector_length);
    float angle_degrees = angle_radians * 180 / M_PI;

    return angle_degrees;
}

// void CheckGarage()
// {
//     int change_num     = 0;
//     int check_garage_h = 60;
//     for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) {
//         if ((GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) >= FIX_BINTHRESHOLD) ||
//             (GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) >= FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) < FIX_BINTHRESHOLD)) {
//             change_num++;
//         }
//     }

//     if (change_num > 14) {
//         garage_type = GARAGE_FOUND;
//         // printf("跳变点的数量为：%d\r\n", change_num);
//     }

//     change_num = 0;
// }

void CheckGarage()
{
    int change_num     = 0;
    int check_garage_h = 60;
    for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) {
        if ((GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) >= FIX_BINTHRESHOLD) ||
            (GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) >= FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) < FIX_BINTHRESHOLD)) {
            change_num++;
        }
    }

    if (change_num > 14) {
        garage_type = GARAGE_FOUND;
        // printf("跳变点的数量为：%d\r\n", change_num);
    }

    change_num = 0;
}

void RunGarage()
{

    if (garage_type == GARAGE_FOUND) {
        printf("识别到车库\r\n");
        garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题
    }
}