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(); } }