public static void ReadParameter(string name, bool retry = true)
{
byte[] paramID;
MAVLink.mavlink_param_request_read_t req = new MAVLink.mavlink_param_request_read_t();
req.target_system = 0;
req.target_component = 0;
if (CDatacenter.knownParametersIndex.ContainsKey(name))
{
paramID = ASCIIEncoding.ASCII.GetBytes("\0");
Array.Resize(ref paramID, 16);
req.param_id = paramID;
req.param_index = (short)CDatacenter.knownParametersIndex[name];
}
else
{
paramID = ASCIIEncoding.ASCII.GetBytes(name + "\0");
// I'm not sure why this is needed, but it is
Array.Resize(ref paramID, 16);
req.param_id = paramID;
req.param_index = -1;
}
byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.PARAM_REQUEST_READ, req);
if (retry)
{
SendWaitRetry(bytes, typeof(MAVLink.mavlink_param_value_t));
}
else
{
SendPacket(bytes);
}
}
/// <summary>
/// Request the value of a data scan from the Pi.
/// </summary>
/// <returns></returns>
public static void ReadMavLinkSNR()
{
//Setting IF Gain ID
byte[] paramid = new byte[16];
paramid[0] = (byte)'V';
paramid[1] = (byte)'H';
paramid[2] = (byte)'F';
paramid[3] = (byte)'_';
paramid[4] = (byte)'S';
paramid[5] = (byte)'N';
paramid[6] = (byte)'R';
paramid[7] = (byte)'\0';
MAVLink.mavlink_param_request_read_t getSNR = new MAVLink.mavlink_param_request_read_t();
getSNR.target_system = (byte)system_id; //Drone
getSNR.target_component = (byte)comp_id; //Pi
getSNR.param_index = (byte)4;
getSNR.param_id = paramid;
MavLinkCom.sendPacket(getSNR, MavLinkCom.sysidcurrent, MavLinkCom.compidcurrent);
}
private void LogPacket(object packet, bool ingoing, int sysId, int compId)
{
if (threadDone)
{
return;
}
if (listViewMessages.InvokeRequired)
{
try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); }
catch { };
return;
}
List <string> fields = new List <string>();
fields.Add(sysId.ToString());
fields.Add(compId.ToString());
if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked))
{
return;
}
string messageName = packet.ToString().Replace("MAVLink+mavlink_", "");
if (IsMessageFilteredOut(messageName))
{
return;
}
if (listViewMessages.IsDisposed)
{
return;
}
if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
fields.Add("GPS Raw Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
fields.Add(gps.vel.ToString());
fields.Add(gps.satellites_visible.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
fields.Add("GPS Global Position Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet;
fields.Add("Attitude");
fields.Add((att.pitch * 180.0 / Math.PI).ToString());
fields.Add((att.roll * 180.0 / Math.PI).ToString());
fields.Add((att.yaw * 180.0 / Math.PI).ToString());
fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString());
fields.Add((att.rollspeed * 180.0 / Math.PI).ToString());
fields.Add((att.yawspeed * 180.0 / Math.PI).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t))
{
MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet;
fields.Add("Scaled IMU");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t))
{
MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet;
fields.Add("Scaled IMU3");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t))
{
MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet;
fields.Add("Scaled IMU2");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t))
{
MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet;
fields.Add("System Status");
fields.Add(status.voltage_battery.ToString());
}
// else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t))
// {
// MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet;
// listViewMessages.Items.Add(new ListViewItem(new string[] {
// "Autopilot Version",
// ver.version.ToString(),
// ver.custom_version.ToString(),
// ver.capabilities.ToString()}));
// }
else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet;
fields.Add("Heartbeat");
fields.Add(hb.autopilot.ToString());
fields.Add(hb.system_status.ToString());
fields.Add(hb.mavlink_version.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t))
{
MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet;
fields.Add("Status Text");
fields.Add(ASCIIEncoding.ASCII.GetString(status.text));
fields.Add(status.severity.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
fields.Add("Param Value");
fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id));
fields.Add(paramValue.param_value.ToString());
fields.Add(paramValue.param_count.ToString());
fields.Add(paramValue.param_index.ToString());
fields.Add(paramValue.param_type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t))
{
MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet;
fields.Add("Param Request Read");
fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id));
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t))
{
MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet;
fields.Add("Param Set");
fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id));
fields.Add(paramSet.param_value.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
fields.Add("Mission Count");
fields.Add(paramValue.count.ToString());
fields.Add(paramValue.target_component.ToString());
fields.Add(paramValue.target_system.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet;
fields.Add("Mission Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet;
fields.Add("Mission Request Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t))
{
MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet;
fields.Add("Ack");
fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString());
fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t))
{
MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet;
fields.Add("Mission Ack");
fields.Add(paramValue.type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t))
{
MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet;
fields.Add("Radio Status");
fields.Add(radio.rssi.ToString());
fields.Add(radio.remrssi.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
fields.Add("EKF Status");
fields.Add(ekf.flags.ToString());
fields.Add(ekf.velocity_variance.ToString());
fields.Add(ekf.pos_horiz_variance.ToString());
fields.Add(ekf.pos_vert_variance.ToString());
fields.Add(ekf.compass_variance.ToString());
fields.Add(ekf.terrain_alt_variance.ToString());
}
else
{
fields.Add(messageName);
//Log(packet.ToString());
}
if (ingoing)
{
listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray())));
}
else
{
listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray())));
}
}
