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