public GpsRawInt(MavLinkMessage message) : base(null)
{
if (message.messid == this.MessageID)
{
MAVLink.mavlink_gps_raw_int_t raw_data = (MAVLink.mavlink_gps_raw_int_t)message.data_struct;
this.alt = raw_data.alt;
this.cog = raw_data.cog;
this.eph = raw_data.eph;
this.epv = raw_data.epv;
this.fix_type = raw_data.fix_type;
this.fixTypeEnum = (FixType)this.fix_type;
this.lat = raw_data.lat;
this.lon = raw_data.lon;
this.satellites_visible = raw_data.satellites_visible;
this.time_usec = raw_data.time_usec;
this.vel = raw_data.vel;
}
}
public void CheckGpsRawIntObject()
{
MAVLink.mavlink_gps_raw_int_t gpsStruct = new MAVLink.mavlink_gps_raw_int_t();
gpsStruct.alt = 1;
gpsStruct.cog = 2;
gpsStruct.eph = 3;
gpsStruct.epv = 4;
gpsStruct.fix_type = 3;
gpsStruct.lat = 6;
gpsStruct.lon = 7;
gpsStruct.satellites_visible = 8;
gpsStruct.time_usec = 9;
gpsStruct.vel = 10;
MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT, gpsStruct);
GpsRawInt obj = new GpsRawInt(message);
Assert.AreEqual(gpsStruct.alt, obj.alt);
Assert.AreEqual(gpsStruct.cog, obj.cog);
Assert.AreEqual(gpsStruct.eph, obj.eph);
Assert.AreEqual(gpsStruct.epv, obj.epv);
Assert.AreEqual(gpsStruct.fix_type, obj.fix_type);
Assert.AreEqual(gpsStruct.lat, obj.lat);
Assert.AreEqual(gpsStruct.lon, obj.lon);
Assert.AreEqual(gpsStruct.satellites_visible, obj.satellites_visible);
Assert.AreEqual(gpsStruct.time_usec, obj.time_usec);
Assert.AreEqual(gpsStruct.vel, obj.vel);
Assert.AreEqual(GpsRawInt.FixType.FIX_3D, obj.fixTypeEnum);
GpsRawIntDTO dto = DTOFactory.createGpsRawIntDTO(obj);
Assert.AreEqual(dto.alt, obj.alt);
Assert.AreEqual(dto.cog, obj.cog);
Assert.AreEqual(dto.eph, obj.eph);
Assert.AreEqual(dto.epv, obj.epv);
Assert.AreEqual(dto.fix_type, obj.fix_type);
Assert.AreEqual(dto.lat, obj.lat);
Assert.AreEqual(dto.lon, obj.lon);
Assert.AreEqual(dto.satellites_visible, obj.satellites_visible);
Assert.AreEqual(dto.time_usec, obj.time_usec);
Assert.AreEqual(dto.vel, obj.vel);
Assert.AreEqual(GpsRawInt.FixType.FIX_3D, obj.fixTypeEnum);
}
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())));
}
}
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();
}
}
private void mavTimer_Tick(object sender, EventArgs e)
{
if (mavbusy)
{
return;
}
mavbusy = true;
mavlink.sysid = 7;
mavlink.compid = 1;
MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t();
hb.autopilot = (byte)(MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA);
//hb.system_status = MAVLink.MAV
mavlink.sendPacket(hb);
MAVLink.mavlink_gps_raw_int_t g = new MAVLink.mavlink_gps_raw_int_t();
lock (this)
{
if (angle >= 0)
{
// lat = (int)(5000 * Math.Cos(angle / 2 / Math.PI));
// lon = (int)(5000 * Math.Sin(angle / 2 / Math.PI));
double lat2, lon2;
getDstPoint(0.0f, 0.0, 500, (angle * Math.PI / 180), out lat2, out lon2);
lat = (int)(lat2 * 1.0e7);
lon = (int)(lon2 * 1.0e7);
double a2 = getBearing(0.0, 0.0, lat2, lon2);
Console.WriteLine("angle {0} {1}", angle, a2);
angle += 2;
if (angle > 360)
{
angle -= 360;
}
}
else if (angle >= -5)
{
lat = lon = 0;
angle--;
}
else
{
angle = 0;
}
// int.TryParse(tbxLat.Text, out lat);
// int.TryParse(tbxLon.Text, out lon);
// int.TryParse(tbxAlt.Text, out alt);
g.lat = lat;
g.lon = lon;
g.alt = alt;
}
g.fix_type = 3;
mavlink.sendPacket(g);
MAVLink.mavlink_vfr_hud_t vfr = new MAVLink.mavlink_vfr_hud_t();
vfr.airspeed = 10;
vfr.groundspeed = 10;
vfr.alt = alt / 1000.0f;
vfr.heading = (short)angle;
vfr.throttle = 10;
mavlink.sendPacket(vfr);
mavbusy = false;
//if (serialPort2.BytesToRead > 0)
//{
// //Console.WriteLine("mavlink read");
// Console.Write(serialPort2.ReadExisting());
//}
}
