public ScaledImu2(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_scaled_imu2_t data = (MAVLink.mavlink_scaled_imu2_t)message.data_struct; this.time_boot_ms = data.time_boot_ms; this.xacc = data.xacc; this.xgyro = data.xgyro; this.xmag = data.xmag; this.yacc = data.yacc; this.ygyro = data.ygyro; this.ymag = data.ymag; this.zacc = data.zacc; this.zgyro = data.zgyro; this.zmag = data.zmag; } }
public void CheckScaledImu2DTO() { MAVLink.mavlink_scaled_imu2_t data = new MAVLink.mavlink_scaled_imu2_t(); data.time_boot_ms = 1; data.xacc = 2; data.xgyro = 3; data.xmag = 4; data.yacc = 5; data.ygyro = 6; data.ymag = 7; data.zacc = 8; data.zgyro = 9; data.zmag = 10; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, data); ScaledImu2 obj = new ScaledImu2(message); Assert.AreEqual(data.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(data.xacc, obj.xacc); Assert.AreEqual(data.xgyro, obj.xgyro); Assert.AreEqual(data.xmag, obj.xmag); Assert.AreEqual(data.yacc, obj.yacc); Assert.AreEqual(data.ygyro, obj.ygyro); Assert.AreEqual(data.ymag, obj.ymag); Assert.AreEqual(data.zacc, obj.zacc); Assert.AreEqual(data.zgyro, obj.zgyro); Assert.AreEqual(data.zmag, obj.zmag); ScaledImu2DTO dto = DTOFactory.createScaledImu2DTO(obj); Assert.AreEqual(dto.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(dto.xacc, obj.xacc); Assert.AreEqual(dto.xgyro, obj.xgyro); Assert.AreEqual(dto.xmag, obj.xmag); Assert.AreEqual(dto.yacc, obj.yacc); Assert.AreEqual(dto.ygyro, obj.ygyro); Assert.AreEqual(dto.ymag, obj.ymag); Assert.AreEqual(dto.zacc, obj.zacc); Assert.AreEqual(dto.zgyro, obj.zgyro); Assert.AreEqual(dto.zmag, obj.zmag); }
static bool ReceviedPacket(byte[] rawpacket) { if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.SCALED_IMU2) { MAVLink.mavlink_scaled_imu2_t packet = rawpacket.ByteArrayToStructure <MAVLink.mavlink_scaled_imu2_t>(); // filter dataset string item = (int)(packet.xmag / div) + "," + (int)(packet.ymag / div) + "," + (int)(packet.zmag / div); if (filtercompass2.ContainsKey(item)) { filtercompass2[item] = (int)filtercompass2[item] + 1; if ((int)filtercompass2[item] > 3) { return(false); } } else { filtercompass2[item] = 1; } // values - offsets are 0 float rawmx = packet.xmag; float rawmy = packet.ymag; float rawmz = packet.zmag; // add data lock (datacompass2) { datacompass2.Add(new Tuple <float, float, float>(rawmx, rawmy, rawmz)); } return(true); } else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU) { MAVLink.mavlink_raw_imu_t packet = rawpacket.ByteArrayToStructure <MAVLink.mavlink_raw_imu_t>(); // filter dataset string item = (int)(packet.xmag / div) + "," + (int)(packet.ymag / div) + "," + (int)(packet.zmag / div); if (filtercompass1.ContainsKey(item)) { filtercompass1[item] = (int)filtercompass1[item] + 1; if ((int)filtercompass1[item] > 3) { return(false); } } else { filtercompass1[item] = 1; } // values float rawmx = packet.xmag - (float)MainV2.comPort.MAV.cs.mag_ofs_x; float rawmy = packet.ymag - (float)MainV2.comPort.MAV.cs.mag_ofs_y; float rawmz = packet.zmag - (float)MainV2.comPort.MAV.cs.mag_ofs_z; // add data lock (datacompass1) { datacompass1.Add(new Tuple <float, float, float>(rawmx, rawmy, rawmz)); } return(true); } return(true); }
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()))); } }