typedef struct{
    /* NO_CONTROL = 0
     * MODE_SPEED = 1
     * MODE_ANGLE = 2
     * MODE_SPEED_ANGLE = 3
     * MODE_RC = 4
     * MODE_ANGLE_REL_FRAME = 5
     * -----------------
     * Speed of rotation.
     * If acceleration limiter is enabled in the settings, given speed may be limited.
     * Units: 0,1220740379 degree/sec
     * -----------------
     * Target angle. Ignored in the MODE_SPEED mode.
     * If mode=MODE_RC, it specifies RC data in range-500..500
     * Units: 0,02197265625 degree.
     * */
    uint8_t control_mode;
    int16_t speed_roll;
    int16_t angle_roll;
    int16_t speed_pitch;
    int16_t angle_pitch;
    int16_t speed_yaw;
    int16_t angle_yaw;

}CMD_CONTROL_t;

typedef struct{
    int8_t start;
    int8_t command_id;
    int8_t data_size;
    int8_t header_checksum;
    CMD_CONTROL_t data;
    int8_t body_checksum;

}SimpleBGC_MSG_t;

int8_t header_checksum(SimpleBGC_MSG_t msg){
    return (msg.command_id + msg.data_size)%256;
}
int8_t body_checksum(SimpleBGC_MSG_t msg){
    int16_t sum = msg.data.control_mode +
            msg.data.angle_pitch +
            msg.data.angle_roll +
            msg.data.angle_yaw +
            msg.data.speed_pitch +
            msg.data.speed_roll +
            msg.data.speed_yaw;
    return (sum % 256);
}


    CMD_CONTROL_t data = {0,0,0,0,0,0,0};
    data.control_mode = 4; // no control
    data.angle_pitch = 20;
    data.angle_roll = 20;

    SimpleBGC_MSG_t msg;
    msg.start = 0x3E;
    msg.command_id = CMD_CONTROL;
    msg.data_size = sizeof(data);
    msg.header_checksum =header_checksum(msg);
    msg.data = data;
    msg.body_checksum = body_checksum(msg);


    while(1)
    {
        UART_write(UART_4,&msg, sizeof(msg));
       sleep(1000);
    }


    return 0;
}