/// <summary>
/// 接收并显示飞行器的当前状态。
/// </summary>
protected void _receiveRobotStatus()
{
// Get the latest Miniquad status
if (Miniquad.Miniquad.RefreshStatus(SerialPortController.ReceivedDataBuffer))
{
// Get the status
MiniquadStatus status = Miniquad.Miniquad.Status;
// Get the throttle outputs (PC mode)
int[] throttles = Miniquad.Miniquad.GetAdjustedThrottleOutputs_PC();
// Check the existence of the status
if (status != null)
{
// Clear the buffer
SerialPortController.ClearReceivedDataBuffer();
// Quaternion
lblQuaternion.Dispatcher.Invoke(new Action(() =>
{
lblQuaternion.Content =
status.Quaternion.W.ToString("0.00") + ", " +
status.Quaternion.X.ToString("0.00") + ", " +
status.Quaternion.Y.ToString("0.00") + ", " +
status.Quaternion.Z.ToString("0.00");
}));
// Euler Angle
lblEulerAngle.Dispatcher.Invoke(new Action(() =>
{
lblEulerAngle.Content =
status.EulerAngle.Psi.ToString("0.00") + ", " +
status.EulerAngle.Theta.ToString("0.00") + ", " +
status.EulerAngle.Phi.ToString("0.00");
}));
// Rotation
lblRotation.Dispatcher.Invoke(new Action(() =>
{
lblRotation.Content =
status.Rotation.X.ToString("0.00") + ", " +
status.Rotation.Y.ToString("0.00") + ", " +
status.Rotation.Z.ToString("0.00");
}));
// Acceleration
lblAcceleration.Dispatcher.Invoke(new Action(() =>
{
lblAcceleration.Content =
status.Acceleration.X.ToString("0.00") + ", " +
status.Acceleration.Y.ToString("0.00") + ", " +
status.Acceleration.Z.ToString("0.00");
}));
// Gravity
lblGravity.Dispatcher.Invoke(new Action(() =>
{
lblGravity.Content =
status.Gravity.X.ToString("0.00") + ", " +
status.Gravity.Y.ToString("0.00") + ", " +
status.Gravity.Z.ToString("0.00");
}));
// Yaw Pitch Roll
lblYawPitchRoll.Dispatcher.Invoke(new Action(() =>
{
lblYawPitchRoll.Content =
status.YawPitchRoll.Yaw.ToString("0.00") + ", " +
status.YawPitchRoll.Pitch.ToString("0.00") + ", " +
status.YawPitchRoll.Roll.ToString("0.00");
}));
// Temperature
//lblTemperature.Content = "null";
// Throttles
lblThrottles.Dispatcher.Invoke(new Action(() =>
{
lblThrottles.Content =
status.Propeller1.Throttle.ToString() + ", " +
status.Propeller2.Throttle.ToString() + ", " +
status.Propeller3.Throttle.ToString() + ", " +
status.Propeller4.Throttle.ToString();
//lblThrottles.Content =
// throttles[0].ToString() + ", " +
// throttles[1].ToString() + ", " +
// throttles[2].ToString() + ", " +
// throttles[3].ToString();
}));
// Auto controlled by algorithm
if (_isAotoControlled)
{
Miniquad.Miniquad.SetThrottleOutputs_PC(throttles[0], throttles[1], throttles[2], throttles[3]);
}
}
}
}
