typedef struct CMD_CONTROL_t {
    uint8_t control_mode;
    uint16_t speed_roll;
    uint16_t angle_roll;
    uint16_t speed_pitch;
    uint16_t angle_pitch;
    uint16_t speed_yaw;
    uint16_t angle_yaw;
}CMD_CONTROL_t;

typedef struct SBGC_MSG_t{
    uint8_t head;
    uint8_t command_id;
    uint8_t data_size;
    uint8_t header_cs;
    CMD_CONTROL_t data;
    uint8_t body_cs;
}SBGC_MSG_t;


uint8_t body_cs(CMD_CONTROL_t cmd){
    return cmd.control_mode +
           cmd.angle_pitch +
           cmd.angle_roll +
           cmd.angle_yaw +
           cmd.speed_roll +
           cmd.speed_pitch +
           cmd.speed_yaw;
}


int8_t app_init(void) {

    CMD_CONTROL_t cmd_msg = {0, 0, 0, 0, 0, 0, 0};
    cmd_msg.control_mode = 1;
    cmd_msg.speed_roll = 0x55;

    SBGC_MSG_t msg;
    msg.head = '>';
    msg.command_id = 67;
    msg.data_size = sizeof(CMD_CONTROL_t);
    msg.header_cs = msg.command_id + msg.data_size;
    msg.data = cmd_msg;
    msg.body_cs = body_cs(cmd_msg);



    while (1){
        UART_write(UART_1,&msg,sizeof(msg));
    }

    return 0;
}