public void OnGotMavlinkParam(MAVLink.mavlink_param_value_t param, bool GetAllMavlinkParams)
{
if (GotMavlinkParam != null)
{
GotMavlinkParam(this, new MavParameterEventArgs(param, GetAllMavlinkParams));
}
}
public void CheckParameterValueObject()
{
MAVLink.mavlink_param_value_t data = new MAVLink.mavlink_param_value_t();
data.param_count = 1;
data.param_id = Encoding.ASCII.GetBytes("foo");
data.param_index = 3;
data.param_type = 4;
data.param_value = 5;
MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.PARAM_VALUE, data);
ParamValue obj = new ParamValue(message);
Assert.AreEqual(obj.param_count, data.param_count);
Assert.AreEqual(Encoding.ASCII.GetBytes(obj.param_id)[0], data.param_id[0]);
Assert.AreEqual(obj.param_index, data.param_index);
Assert.AreEqual((int)obj.param_type, data.param_type);
Assert.AreEqual(obj.param_value, data.param_value);
ParamValueDTO dto = DTOFactory.createParamValueDTO(obj);
Assert.AreEqual(dto.param_count, obj.param_count);
Assert.AreEqual(dto.param_id, obj.param_id);
Assert.AreEqual(dto.param_index, obj.param_index);
Assert.AreEqual(dto.param_type, obj.param_type.ToString());
Assert.AreEqual(dto.param_value, obj.param_value);
}
public ParamValue(MavLinkMessage message) : base(null)
{
if (message.messid.Equals(this.MessageID))
{
MAVLink.mavlink_param_value_t data = (MAVLink.mavlink_param_value_t)message.data_struct;
this.param_count = data.param_count;
this.param_id = System.Text.ASCIIEncoding.ASCII.GetString(data.param_id);
this.param_id = this.param_id.Replace("\0", string.Empty);
this.param_index = data.param_index;
this.param_type = (MAVLink.MAV_PARAM_TYPE)data.param_type;
this.param_value = data.param_value;
}
}
/// <summary>
/// Event handler for retreiving gain in each
/// direction and for retreiving current waypoints
/// </summary>
/// <param name="data">A MAVLink message of the data.</param>
/// <returns></returns>
private static void MavLinkPacketReceived_Handler(object o, MAVLink.MAVLinkMessage msg)
{
if (msg.sysid == system_id && msg.compid == comp_id) //Get Pi messages
{
if (msg.msgid == (int)MAVLink.MAVLINK_MSG_ID.COMMAND_ACK)
{
MAVLink.mavlink_command_ack_t cmd_ack_msg = (MAVLink.mavlink_command_ack_t)msg.data;
scan_completion_status = cmd_ack_msg.result;
}
if (msg.msgid == (int)MAVLink.MAVLINK_MSG_ID.PARAM_VALUE)
{
MAVLink.mavlink_param_value_t param_value_msg = (MAVLink.mavlink_param_value_t)msg.data;
string param_id = System.Text.Encoding.Default.GetString(param_value_msg.param_id).Trim().ToUpper();
if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'S' && param_id[5] == 'N' &&
param_id[6] == 'R')
{
float SNR = param_value_msg.param_value;
int direction = (int)(Math.Round(MavLinkCom.MAV.cs.yaw / 5.0) * 5);
RDFData.Add(new KeyValuePair <int, float>(direction, SNR));
RDFDataReceived(new object(), new EventArgs());
//Set to unsupported so that a retrigger of a new packet
//does not happen.
scan_completion_status = (byte)MAVLink.MAV_RESULT.UNSUPPORTED;
vhf_snr_state_changed = true;
}
else if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'F' && param_id[5] == 'R' &&
param_id[6] == 'E' && param_id[7] == 'Q')
{
vhf_freq_state_changed = true;
}
else if (param_id[0] == 'I' && param_id[1] == 'F')
{
if_gain_state_changed = true;
}
else if (param_id[0] == 'M' && param_id[1] == 'I' &&
param_id[2] == 'X')
{
mixer_gain_state_changed = true;
}
else if (param_id[0] == 'L' && param_id[1] == 'N' &&
param_id[2] == 'A')
{
lna_gain_state_changed = true;
}
else if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'R' && param_id[5] == 'S' &&
param_id[6] == 'T' && param_id[7] == 'T')
{
SendMavLinkFrequency(vhf_freq);
SendMavLinkIFGain(if_gain);
SendMavLinkMixerGain(mixer_gain);
SendMavLinkLNAGain(lna_gain);
}
}
}
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
{
continue;
}
object packet = packetbytes.data;
if (packet == null)
{
continue;
}
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t)packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
{
AHRS_ORIENTATION = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_ORIENT")
{
COMPASS_ORIENT = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_EXTERNAL")
{
COMPASS_EXTERNAL = (int)(m.param_value);
}
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t)packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err / count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return(rans.name);
}
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())));
}
}
internal Message AddMessage(MavLinkMessage e)
{
if (this.data == null)
{
this.data = new List <Message>();
}
DateTime time = epoch.AddMilliseconds((double)e.Time / (double)1000);
Message msg = new Model.MavlinkLog.Message()
{
Msg = e,
Timestamp = time,
Ticks = e.Time
};
if (e.TypedPayload == null)
{
Type msgType = MAVLink.MAVLINK_MESSAGE_INFO[(int)e.MsgId];
if (msgType != null)
{
byte[] msgBuf = new byte[256];
GCHandle handle = GCHandle.Alloc(msgBuf, GCHandleType.Pinned);
IntPtr ptr = handle.AddrOfPinnedObject();
// convert to proper mavlink structure.
msg.TypedValue = Marshal.PtrToStructure(ptr, msgType);
handle.Free();
}
}
else
{
msg.TypedValue = e.TypedPayload;
}
lock (data)
{
this.data.Add(msg);
if (this.data.Count > MaxSize)
{
this.data.RemoveAt(0);
}
if (msg.TypedValue is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)msg.TypedValue;
string name = ConvertToString(param.param_id);
parameters[name] = param;
Debug.WriteLine("{0}={1}", name, param.param_value);
}
}
if (msg.TypedValue != null)
{
CreateSchema(msg.TypedValue);
}
lock (queryEnumerators)
{
foreach (var q in queryEnumerators)
{
q.Add(msg);
}
}
return(msg);
}
public static void ScanAccel()
{
string[] files = Directory.GetFiles(MainV2.LogDir, "*.tlog", SearchOption.AllDirectories);
List <string> badfiles = new List <string>();
foreach (var file in files)
{
bool board = false;
Console.WriteLine(file);
using (MAVLinkInterface mavi = new MAVLinkInterface())
using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file)))
{
mavi.logreadmode = true;
try
{
while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length)
{
byte[] packet = mavi.readPacket();
if (packet.Length == 0)
{
break;
}
var objectds = mavi.DebugPacket(packet, false);
if (objectds is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds;
if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID"))
{
if (param.param_value == 0 || param.param_value == 5)
{
board = true;
}
else
{
break;
}
}
}
if (objectds is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds;
if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 &&
Math.Abs(rawimu.zacc) > 2000)
{
//CustomMessageBox.Show("Bad Log " + file);
badfiles.Add(file);
break;
}
}
}
}
catch
{
}
}
}
if (badfiles.Count > 0)
{
FileStream fs = File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip",
FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
foreach (var file in badfiles)
{
// entry 2
string entryName = ZipEntry.CleanName(Path.GetFileName(file));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(file))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
}
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + MainV2.LogDir +
Path.DirectorySeparatorChar +
"SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>");
}
else
{
CustomMessageBox.Show("No Bad Logs Found");
}
}
void MavMessageEvent(object sender, MAVLink.MavMessageEventArgs e)
{
DoModeChange(CommMode.MAVLINK);
//Respond to Mavpixel heartbeat or ping reply
if (e.Message.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t) ||
e.Message.data.GetType() == typeof(MAVLink.mavlink_ping_t))
{
//Discovered our current Mavpixel, save reply id
sysid = e.Message.sysid;
compid = e.Message.compid;
//Discovery complete, trigger next action
if (waitForBoot || waitForPrompt)
{
stopTimer();
if (commType == CommType.BOOTING)
{
//Just booting? Ready.
OnProgressChanged(commType, 0, "Configuration: Mavpixel ready.");
OnBootCompleted();
OnCompleted(commType, true, 0);
}
else
{
//Collecting parameters? Start fetch.
OnComStarted(commType);
OnProgressChanged(commType, 0, "Configuration: Fetching " + dataName + "..");
mavlinkNextParamFetch();
}
}
}
//Respond to a Mavlink parameter
if (e.Message.data.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
if (collectingReply)
{
stopTimer();
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)e.Message.data;
//Send parameter to host's mavlink message parser
OnGotMavlinkParam(param, getAllMavlinkParams);
MessageCounter++;
//Update parameter received flags
mavlinkParameterReceived[param.param_index - mavlinkStart] = true;
//Send progress report
int progress = (int)((float)(param.param_index - mavlinkStart) / (float)mavlinkCount * 32);
OnProgressChanged(commType, progress, "");
//Fetch next parameter
if (getAllMavlinkParams && param.param_index < param.param_count - 1)
{
//Retrieving all Mavlink parameters list.. no reply needed
startTimer(ref collectingReply);
}
else
{
//Any parameters not yet received?
if (mavlinkParameterReceived.Contains(false))
{
getAllMavlinkParams = false; //Ensure we dont request all parameters
mavlinkNextParamFetch(); //Fetch next unreceived parameter
startTimer(ref collectingReply);
}
else
{
OnProgressChanged(commType, 32, "Configuration: Read all data OK.");
OnCompleted(commType, true, mavlinkCount);
}
}
}
else if (sendingCommands)
{
stopTimer();
//Look for a modified parameter to send
for (sentCommandIndex++; sentCommandIndex < commandsToSend; sentCommandIndex++)
{
if (mavlinkWriteCommand(sentCommandIndex))
{
break;
}
}
//All parameters sent?
if (sentCommandIndex == commandsToSend)
{
//All parameters sent successfully
stopTimer();
OnProgressChanged(commType, 32, "Configuration: Sent all data OK.");
OnCompleted(commType, true, sentCommandIndex - 1);
}
}
}
}
public MavParameterEventArgs(MAVLink.mavlink_param_value_t param, bool getAllMavlinkParams)
{
Param = param;
GetAllMavlinkParams = getAllMavlinkParams;
}
private static void HandlePacket(object packet)
{
if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet;
droneState = (MAVLink.MAV_STATE)parsed.system_status;
//MAVLink.MAV_MODE
mode = (APMModes.Quadrotor)parsed.custom_mode;
}
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;
// dronePosition.lat = (double)gps.lat / 10000000;
// dronePosition.lng = (double)gps.lon / 10000000;
// dronePosition.alt = (double)gps.alt / 1000;
dronePosition.altFromGround = (double)gps.relative_alt / 1000;
dronePosition.heading = (double)gps.hdg / 100;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
GPSHdop = (double)gps.eph / 100;
dronePosition.lat = (double)gps.lat / 10000000;
dronePosition.lng = (double)gps.lon / 10000000;
dronePosition.alt = (double)gps.alt / 1000;
satCount = gps.satellites_visible;
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
missionWaypointsCount = paramValue.count;
missionItems = new SMissionItem[missionWaypointsCount];
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet;
SMissionItem missionItem = new SMissionItem();
missionItem.command = (MAVLink.MAV_CMD)paramValue.command;
// if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF)
// // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH))
// {
// missionItem.lat = dronePosition.lat;
// missionItem.lng = dronePosition.lng;
// }
// else
{
missionItem.lat = paramValue.x;
missionItem.lng = paramValue.y;
}
missionItem.p1 = paramValue.param1;
missionItem.p2 = paramValue.param2;
missionItem.p3 = paramValue.param3;
missionItem.p4 = paramValue.param4;
missionItem.alt = paramValue.z;
// Is this the home?
missionItem.IsHome = (paramValue.seq == 0);
missionItems[paramValue.seq] = missionItem;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet;
SetMissionItem(missionReq.seq);
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id);
name = name.Trim().Replace("\0", "");
double val = paramValue.param_value;
if (OnParamUpdate != null)
{
OnParamUpdate(name, val, paramValue.param_index);
}
}
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;
// Check that all estimations are good
EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance))));
EKFFlags = ekf.flags;
IsDroneReady();
}
}
