public static void setAngle(int roll, int pitch, int yaw, ref ControlCommandStructure cCmd) { byte[] byteRead = new byte[100]; RealtimeDataStructure rData = rtD; cCmd.setMode(MODE_ANGLE); cCmd.setAnglePitch(pitch); cCmd.setAngleRoll(roll); cCmd.setAngleYaw(yaw); //default 30 cCmd.setSpeedPitch(30); cCmd.setSpeedRoll(30); cCmd.setSpeedYaw(30); if (sendCommand(CMD_CONTROL, cCmd.getControlStructure())) { Console.WriteLine("Done!"); //호출을 한번 더 해줘야 함. 이유: 나중에 각도 읽어올때 쓰레기값 보냄 if (sendCommand(CMD_REALTIME_DATA)) { System.Threading.Thread.Sleep(30); port.Read(byteRead, 0, 100); rData = parseRealTimeData(byteRead); float[] angle = rData.getAngle(); } } else { Console.WriteLine("Can't send command a message"); return; } }
public static float[] getAngle() { byte[] byteRead = new byte[100]; RealtimeDataStructure rData = rtD; if (sendCommand(CMD_REALTIME_DATA)) { System.Threading.Thread.Sleep(30); port.Read(byteRead, 0, 100); rData = parseRealTimeData(byteRead); float[] angle = rData.getAngle(); Console.WriteLine("Roll: " + rData.getRoll()); Console.WriteLine("Pitch: " + rData.getPitch()); Console.WriteLine("Yaw: " + rData.getYaw()); return(angle); } else { return(new float[0]); } }