p_lusterinc_camera_focallength_encoder_reset/usersrc/udpclient.c

#include "stm32f4xx_hal.h"
#include "lwip.h"
#include "udp.h"
#include "string.h"
#include "udpclient.h"
#include "socket.h"
#include "sockets.h"
#include "def.h"
#include "protocol.h"
#include "encoder.h"
#include "zport.h"

#define cmd_checksum(data)                                             \
    if (computesum8((char *)data, recv_datalen - 1) != data->checksum) \
    {                                                                  \
        printf("checksum error\r\n");                                  \
        return;                                                        \
    }

static bool udp_client_active_flag;
static bool genlock_and_esync_active_flag;

/* 定义端口号 */
#define UDP_REMOTE_PORT 8881 /* 远端端口 */
#define UDP_LOCAL_PORT 8880  /* 本地端口 */
#define SERVER_FAMILY AF_INET
#define SOCKET_ERROR -1
#define BUFFER_SIZE 256 // 缓冲区大小

#define ACTIVE_ID 0XDD
#define DEVICE_ID 0X01

#define AVTIVE_DISABLE 0
#define ACTIVE_ENABLE 1

#define CLEAR_ENCODER_ALL 0
#define CLEAR_ENCODER_1 1
#define CLEAR_ENCODER_2 2

static char s_sendBuf[BUFFER_SIZE]; // 发送数据的缓冲区
static char s_receBuf[BUFFER_SIZE]; // 接收数据的缓冲区

SOCKET sock_Client = 0; // 客户端用于通信的Socket
static struct sockaddr_in addr_server;
static struct sockaddr_in addr_client;
static struct sockaddr_in sock;
socklen_t sock_len = sizeof(sock);

active_report_data_t active_report_data_structer;
static int active_report_cycle;
static void active_report_data_structer_init(void)
{
    /* index在每次上报一次主动上报数据后+1，时间戳在获取ESync命令时更新，编码器数值在 active_report_data_structer_update中更新，校验和在上传数据前(udp_client_active_response)进行更新*/
    active_report_data_structer.index = 0;             /*  */
    active_report_data_structer.cmd_id = ACTIVE_ID;    /* 指令id */
    active_report_data_structer.time_stamp_s = 0;      /* 时间戳 */
    active_report_data_structer.encoder_1_count = 0;   /* 编码器1计数 */
    active_report_data_structer.encoder_2_count = 0;   /* 编码器2计数 */
    active_report_data_structer.device_id = DEVICE_ID; /* 设备ID号 */
}

static void active_report_data_structer_update(void)
{
    /* 时间戳暂时先不管，后续完善 */
    encoder_read_with_encoder(CAMERA_ENCODER, &active_report_data_structer.encoder_1_count);
    encoder_read_with_encoder(DRIVEN_ENCODER_GEAR, &active_report_data_structer.encoder_2_count);
}

static void udp_client_create_basic_response(basic_report_data_t *rxcmd, int recv_datalen)
{
    /* 因为发送和接收是一致的，所以不需要二次校验，直接把接收的数据发送即可 */
    if (sendto(sock_Client, rxcmd, sizeof(basic_report_data_t), 0, (struct sockaddr *)&addr_server, sizeof(struct sockaddr_in)) == SOCKET_ERROR)
    {
        printf("send basic_response error\r\n");
    }
}

static void udp_client_create_get_encoder_response(get_encoder_report_t *get_encoder_cmd, int recv_datalen)
{
    get_encoder_respont_t get_encoder_respont_structer;

    get_encoder_respont_structer.index = get_encoder_cmd->index;
    get_encoder_respont_structer.cmd_id = get_encoder_cmd->cmd_id;
    encoder_read_with_encoder(CAMERA_ENCODER, &get_encoder_respont_structer.encoder1);
    encoder_read_with_encoder(DRIVEN_ENCODER_GEAR, &get_encoder_respont_structer.encoder2);
    get_encoder_respont_structer.checksum = computesum8((char *)&get_encoder_respont_structer, sizeof(get_encoder_respont_t) - 1);

    if (sendto(sock_Client, &get_encoder_respont_structer, sizeof(get_encoder_respont_t), 0, (struct sockaddr *)&addr_server, sizeof(struct sockaddr_in)) == SOCKET_ERROR)
    {
        printf("send basic_response error\r\n");
    }
}

static void udp_client_active_response(void)
{
    active_report_data_structer.checksum = computesum8((char *)&active_report_data_structer, sizeof(active_report_data_t) - 1);
    if (sendto(sock_Client, &active_report_data_structer, sizeof(active_report_data_t), 0, (struct sockaddr *)&addr_server, sizeof(struct sockaddr_in)) == SOCKET_ERROR)
    {
        printf("send basic_response error\r\n");
    }
}

static void udp_client_all_encoder_light_set(encoder_light_state_t state)
{
    if (state == STANDBY)
    {
        encoder_set_state(CAMERA_ENCODER, STANDBY);
        encoder_set_state(DRIVEN_ENCODER_GEAR, STANDBY);
    }
    else if (state == WORKING)
    {
        encoder_set_state(CAMERA_ENCODER, WORKING);
        encoder_set_state(DRIVEN_ENCODER_GEAR, WORKING);
    }
}

void udp_client_recv_data_dump(int recv_datalen)
{
    /* debug使用 */
    for (size_t i = 0; i < recv_datalen; i++)
    {
        printf("%d ", s_receBuf[i]);
    }
    printf("\r\n");
}

void config_server(struct sockaddr_in *addr_server)
{
    addr_server->sin_family = SERVER_FAMILY;
    addr_server->sin_port = htons(UDP_REMOTE_PORT);
    addr_server->sin_addr.s_addr = htonl(INADDR_BROADCAST);
}

void config_client(struct sockaddr_in *addr_client)
{
    addr_client->sin_family = AF_INET;
    addr_client->sin_addr.s_addr = inet_addr("0.0.0.0");
    addr_client->sin_port = htons(UDP_LOCAL_PORT);
}

void udp_client_send_string(char *pData)
{
    memcpy(s_sendBuf, pData, strlen(pData));
    if (sendto(sock_Client, s_sendBuf, strlen(pData), 0, (struct sockaddr *)&addr_server, sizeof(struct sockaddr_in)) == SOCKET_ERROR)
    {
        printf("send string error\n");
    }
}

void udp_client_init(void)
{
    active_report_data_structer_init();
    active_report_cycle = 1000; /* 上报周期开始设置默认为1S */

    // bool bOpt = true;
    struct timeval tv = {0, 1000};

    config_server(&addr_server);
    config_client(&addr_client);

    // 创建客户端用于通信的Socket
    sock_Client = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (sock_Client == SOCKET_ERROR)
    {
        printf("create socket error...\n");
    }
    else
    {
        printf("create socket success!\n");
    }

    // /* 开启广播 */
    // if (lwip_setsockopt(sock_Client, SOL_SOCKET, SO_BROADCAST, (char *)&bOpt, sizeof(bOpt)) == SOCKET_ERROR)
    // {
    //     printf("enable broadcast error...\n");
    // }
    // else
    // {
    //     printf("enable broadcast success!\n");
    // }

    /* 设置超时 */
    if (lwip_setsockopt(sock_Client, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(tv)) == SOCKET_ERROR)
    {
        printf("enable receive timeout error...\n");
    }
    else
    {
        printf("enable receive timeout success!\n");
    }

    /* 绑定 */
    if (bind(sock_Client, (struct sockaddr *)&addr_client, sizeof(addr_client)) == SOCKET_ERROR)
    {
        printf("Bind failed");
        return;
    }
}

void udp_client_recv(void)
{
    int recv_datalen = recvfrom(sock_Client, s_receBuf, sizeof(s_receBuf), 0, (struct sockaddr *)&sock, &sock_len);
    if (recv_datalen > 0)
    {
        printf("udp recv data len:%d\r\n", recv_datalen);
        udp_client_parse(recv_datalen);
    }
}

void udp_client_parse(int recv_datalen)
{
    if (recv_datalen >= adwin_config_protocol_size)
    {
        adwin_config_protocol_t *rxcmd = (adwin_config_protocol_t *)s_receBuf;
        active_report_data_structer.time_stamp_s = rxcmd->time_stamp_s;
        genlock_and_esync_active_flag = true;
    }
    else if (recv_datalen <= basic_report_data_size)
    {
        basic_report_data_t *rxcmd = (basic_report_data_t *)s_receBuf;
        get_encoder_report_t *get_encoder_cmd = (get_encoder_report_t *)s_receBuf;
        switch (rxcmd->cmd_id)
        {
        case CMD_SET_AUTOMATIC_REPORTING_FREQUENCY: /* 设置自动上报频率 */
            cmd_checksum(rxcmd);
            if (rxcmd->data >= 10)
            {
                active_report_cycle = rxcmd->data;
            }
            udp_client_create_basic_response(rxcmd, recv_datalen);
            break;
        case CMD_GET_ENCODER_DATA: /* 获取编码器数据 */
            cmd_checksum(get_encoder_cmd);
            udp_client_create_get_encoder_response(get_encoder_cmd, recv_datalen);

            if (!udp_client_active_flag)
            {
                udp_client_all_encoder_light_set(STANDBY);
            }

            break;
        case CMD_SET_ACTIVE: /* 设置主动上报 */
            cmd_checksum(rxcmd);
            if ((rxcmd->data == AVTIVE_DISABLE) || (rxcmd->data == ACTIVE_ENABLE))
            {
                udp_client_active_flag = rxcmd->data;
                if (udp_client_active_flag)
                {
                    udp_client_all_encoder_light_set(WORKING);
                }
                else
                {
                    udp_client_all_encoder_light_set(STANDBY);
                }
            }
            udp_client_create_basic_response(rxcmd, recv_datalen);
            break;
        case CMD_CLEAR_ENCODER: /* 清除编码器数据 */
            cmd_checksum(rxcmd);

            switch (rxcmd->data)
            {
            case CLEAR_ENCODER_ALL:
                encoder_all_clear_counter();
                break;
            case CLEAR_ENCODER_1:
                encoder_clear_counter(CAMERA_ENCODER);
                break;
            case CLEAR_ENCODER_2:
                encoder_clear_counter(DRIVEN_ENCODER_GEAR);
                break;

            default:
                printf("not find clear encoder cmd\r\n");
                break;
            }

            udp_client_create_basic_response(rxcmd, recv_datalen);

            break;

        default:
            break;
        }
    }
}

void udp_client_active(void)
{
    if (udp_client_active_flag)
    {
        static uint32_t lastprocess = 0;
        if (sys_haspassedms(lastprocess) > active_report_cycle)
        {
            lastprocess = HAL_GetTick();
            active_report_data_structer_update();
            udp_client_active_response();
            active_report_data_structer.index += 1;
        }
    }
}

void udp_client_genlock_and_esync_active(void)
{
    if (genlock_and_esync_active_flag)
    {
        active_report_data_structer_update();
        udp_client_active_response();

        if (!udp_client_active_flag)
        {
            udp_client_all_encoder_light_set(STANDBY);
        }

        active_report_data_structer.index += 1;
        active_report_data_structer.time_stamp_s = 0;
        genlock_and_esync_active_flag = false;
    }
}

bool *udp_client_genlock_and_esync_active_flag_ret(void)
{
    return &genlock_and_esync_active_flag;
}