public void MavlinkDataReceivedHandlerMain(MavlinkPacket packet)
{
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
game_rotation_angles.update_from_mavlink((MavLink.Msg_mav_game_rotation_vector_euler)packet.Message);
UpdateMainScreen();
}
else if (packet.Message.GetType() == typeof(MavLink.Msg_mav_batt_state))
{
MavLink.Msg_mav_batt_state battery_state = (MavLink.Msg_mav_batt_state)packet.Message;
battery_voltage = battery_state.voltage;
}
}
public void ProcessRxMavlinkPacket(object sender, MavlinkPacket e)
{
//System.Diagnostics.Debug.WriteLine("number", e.SequenceNumber.ToString());
if (e.Message.GetType() == typeof(MavLink.Msg_mav_rotation_vector_euler))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
rotation_angles.update_from_mavlink((MavLink.Msg_mav_rotation_vector_euler)e.Message);
anglesDisplayRotation.update_angles(rotation_angles);
if (chkUseGyro.Checked == false)
{
mapControl1.draw_direction(rotation_angles.yaw);
}
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_quaternion))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
MavLink.Msg_mav_quaternion m_quaternion = (MavLink.Msg_mav_quaternion)e.Message;
raw_quaternion = new Quaternion(m_quaternion.x, m_quaternion.y, m_quaternion.z, m_quaternion.w);
lblRawQuaternion.Text = $"Raw Quat: X={raw_quaternion.X:0.0000} Y={raw_quaternion.Y:0.0000} Z={raw_quaternion.Z:0.0000} W={raw_quaternion.W:0.0000}";
rotation_angles_t1.update_from_quaternion(raw_quaternion);
//anglesDisplay1.update_angles(rotation_angles_t1);
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
game_rotation_angles.update_from_mavlink((MavLink.Msg_mav_game_rotation_vector_euler)e.Message);
anglesDisplay1.update_angles(game_rotation_angles);
if (chkUseGyro.Checked == true)
{
mapControl1.draw_direction(game_rotation_angles.yaw);
}
NMEAPublisherObj.CurrentAngle = game_rotation_angles.yaw;
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_button_event))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
parse_keys_event((MavLink.Msg_mav_button_event)e.Message);
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_batt_state))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
MavLink.Msg_mav_batt_state msg = (MavLink.Msg_mav_batt_state)e.Message;
lblBattery.Text = $"Battery: {msg.voltage:0.00} V";
});
}
statistic_analyse(e.Message.GetType().ToString());
}