private void sendGimbalCommand(double x, double y, double z)
{
if (gimbal != null)
{
if (gimbal.IsConnected)
{
var q = Quaterion.FromEulerAngles(x, y, z);
float[] quat = { (float)q.x, (float)q.y, (float)q.z, (float)q.w };
var byteArray = new byte[quat.Length * 4 + 1];
Buffer.BlockCopy(quat, 0, byteArray, 0, quat.Length * 4);
byteArray[quat.Length * 4] = 0; //Cmd
gimbal.Publish("gimbal/control", byteArray, MqttMsgBase.QOS_LEVEL_EXACTLY_ONCE, false);
}
}
}
private void btnGimbalControl_Click(object sender, RoutedEventArgs e)
{
//byte[] x = BitConverter.GetBytes(1.0f);
//byte[] y = BitConverter.GetBytes(2.0f);
//byte[] z = BitConverter.GetBytes(3.0f);
//byte[] w = BitConverter.GetBytes(4.0f);
//byte[] mode = BitConverter.GetBytes((char)0);
//byte[] message = new byte[x.Length + y.Length + z.Length + w.Length + mode.Length];
//System.Buffer.BlockCopy(x, 0, message, 0, x.Length);
//System.Buffer.BlockCopy(y, 0, message, x.Length, y.Length);
//System.Buffer.BlockCopy(z, 0, message, x.Length + y.Length, z.Length);
//System.Buffer.BlockCopy(w, 0, message, x.Length + y.Length + z.Length, w.Length);
//System.Buffer.BlockCopy(mode, 0, message, x.Length + y.Length + z.Length + w.Length, mode.Length);
if (gimbal.IsConnected)
{
var quter = Quaterion.FromEulerAngles(3.14, 0.0, 0.0);
float[] quat = { (float)quter.x, (float)quter.y, (float)quter.z, (float)quter.w };
var byteArray = new byte[quat.Length * 4 + 1];
Buffer.BlockCopy(quat, 0, byteArray, 0, quat.Length * 4);
byteArray[quat.Length * 4] = 0; //Cmd
gimbal.Publish("gimbal/control", byteArray, MqttMsgBase.QOS_LEVEL_EXACTLY_ONCE, false);
}
}
