#include "message.h"
#include "connection.h"
#include "robomaster.h"

void
gimbal_ctrl_speed (
		union Request* req,
        uint16_t seq,
        bool ack,
		int16_t p,
		int16_t y,
		int16_t r ) {
    req->gimbspeed.yaw = y;
    req->gimbspeed.roll = r;
    req->gimbspeed.pitch = p;
    req->gimbspeed.ctrl = 0xDC;
    req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
}

void
gimbal_rotate (
		union Request* req,
        uint16_t seq,
        bool ack,
		int16_t p,
		int16_t y ) {
    //req->gimbalrot.coordinate = 1;
    //req->gimbalrot.yaw_valid = 1;
    //req->gimbalrot.pitch_valid = 1;
    //req->gimbalrot.roll_valid = 0;
    req->gimbalrot.coord_bitfield = 0;
    req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 1 << 3;

    //req->gimbalrot.frequency = 2;
    //req->gimbalrot.control = 0;
    req->gimbalrot.control_bitfield = 0;
    req->gimbalrot.control_bitfield = 0 | 1 << 2;

    req->gimbalrot.id = 33;
    req->gimbalrot.yaw = 0;
    req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
    req->gimbalrot.roll = 0;
    req->gimbalrot.pitch = 0;
    req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
    req->gimbalrot.error = 0;
    req->gimbalrot.yaw_speed = 15;
    req->gimbalrot.roll_speed = 0;
    req->gimbalrot.pitch_speed = 15;
    req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
}