public void CheckVfrHudObject()
{
MAVLink.mavlink_vfr_hud_t data = new MAVLink.mavlink_vfr_hud_t();
data.airspeed = 1;
data.alt = 2;
data.climb = 3;
data.groundspeed = 4;
data.heading = 5;
data.throttle = 6;
MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.VFR_HUD, data);
VfrHud obj = new VfrHud(message);
Assert.AreEqual(data.airspeed, obj.airspeed);
Assert.AreEqual(data.alt, obj.alt);
Assert.AreEqual(data.climb, obj.climb);
Assert.AreEqual(data.groundspeed, obj.groundspeed);
Assert.AreEqual(data.heading, obj.heading);
Assert.AreEqual(data.throttle, obj.throttle);
VfrHudDTO dto = DTOFactory.createVfrHudDTO(obj);
Assert.AreEqual(dto.airspeed, obj.airspeed);
Assert.AreEqual(dto.alt, obj.alt);
Assert.AreEqual(dto.climb, obj.climb);
Assert.AreEqual(dto.groundspeed, obj.groundspeed);
Assert.AreEqual(dto.heading, obj.heading);
Assert.AreEqual(dto.throttle, obj.throttle);
}
public VfrHud(MavLinkMessage message) : base(null)
{
if (message.messid.Equals(this.MessageID))
{
MAVLink.mavlink_vfr_hud_t data = (MAVLink.mavlink_vfr_hud_t)message.data_struct;
this.airspeed = data.airspeed;
this.alt = data.alt;
this.climb = data.climb;
this.groundspeed = data.groundspeed;
this.heading = data.heading;
this.throttle = data.throttle;
}
}
private int updatedata(int timeout = 5000)
{
if (ReadHeartBeat())
{
DateTime deadline = DateTime.Now.AddMilliseconds(timeout);
while (DateTime.Now < deadline)
{
try
{
var data = _mavlinkparse.ReadPacket(_datastream);
if (data == null)
{
continue;
}
switch (data.GetType().ToString())
{
case "MAVLink+mavlink_attitude_t":
MAVLink.mavlink_attitude_t pack0 = (MAVLink.mavlink_attitude_t)data;
Vehicle.Roll = pack0.roll;
Vehicle.RollSpeed = pack0.rollspeed;
Vehicle.Pitch = pack0.pitch;
Vehicle.PitchSpeed = pack0.pitchspeed;
Vehicle.Yaw = pack0.yaw;
Vehicle.YawSpeed = pack0.yawspeed;
Vehicle.BootTime = pack0.time_boot_ms;
break;
case "MAVLink+_global_position_int_t":
MAVLink.mavlink_global_position_int_t pack1 = (MAVLink.mavlink_global_position_int_t)data;
Vehicle.Attitude = pack1.alt;
Vehicle.Latitude = pack1.lat;
Vehicle.Longtitude = pack1.lon;
break;
case "MAVLink+mavlink_sys_status_t":
MAVLink.mavlink_sys_status_t pack2 = (MAVLink.mavlink_sys_status_t)data;
Vehicle.BatteryVoltage = pack2.voltage_battery;
Vehicle.BatteryCurrent = pack2.current_battery;
Vehicle.BatteryRemaining = pack2.battery_remaining;
break;
case "MAVLink+mavlink_vfr_hud_t":
MAVLink.mavlink_vfr_hud_t pack3 = (MAVLink.mavlink_vfr_hud_t)data;
Vehicle.AirSpeed = pack3.airspeed;
Vehicle.GroungSpeed = pack3.groundspeed;
break;
case "MAVLink+mavlink_heartbeat_t":
ReadHeartBeat();
Vehicle.HeartBeating = true;
deadline = DateTime.Now.AddMilliseconds(timeout);
break;
default:
break;
}
}
catch
{
Vehicle.Connected = false;
}
}
}
return(0);
}
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());
//}
}
