public static GMapRoute getMAVRoute(MAVState MAV)
{
List <PointLatLng> trackPoints = new List <PointLatLng>();
PointLatLng _currentloc = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
GMapRoute _route = new GMapRoute(trackPoints, "track" + MAV.sysid.ToString(), MAV.sysid);
_route.Points.Add(_currentloc);
return(_route);
}
public static GMapMarker getMAVMarker(MAVState MAV)
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING)
{
return(new GMapMarkerPlane(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing, MAV.cs.radius)
{
ToolTipText = MAV.cs.alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return(new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return(new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduTracker)
{
return(new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
if (MAV.param.ContainsKey("AVD_W_DIST_XY") && MAV.param.ContainsKey("AVD_F_DIST_XY"))
{
var w = MAV.param["AVD_W_DIST_XY"].Value;
var f = MAV.param["AVD_F_DIST_XY"].Value;
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
danger = (int)f,
warn = (int)w
});
}
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return(new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else
{
// unknown type
return(new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot));
}
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow,
MAVLinkInterface mavinterface, MAVState MAV)
{
lock (this)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (parent != null && parent.packetsnotlost != 0)
{
if ((DateTime.Now - MAV.lastvalidpacket).TotalSeconds > 10)
{
linkqualitygcs = 0;
}
else
{
linkqualitygcs =
(ushort) ((parent.packetsnotlost/(parent.packetsnotlost + parent.packetslost))*100.0);
}
if (linkqualitygcs > 100)
linkqualitygcs = 100;
}
if (datetime.Second != lastsecondcounter.Second)
{
lastsecondcounter = datetime;
if (lastpos.Lat != 0 && lastpos.Lng != 0 && armed)
{
if (!mavinterface.BaseStream.IsOpen && !mavinterface.logreadmode)
distTraveled = 0;
distTraveled += (float) lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, ""))*
multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
timeInAir++;
if (!gotwind)
dowindcalc();
}
// re-request streams
if (!(lastdata.AddSeconds(8) > DateTime.Now) && mavinterface.BaseStream.IsOpen)
{
try
{
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, MAV.cs.ratestatus,
MAV.sysid); // mode
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MAV.cs.rateposition,
MAV.sysid); // request gps
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MAV.cs.rateattitude,
MAV.sysid); // request attitude
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MAV.cs.rateattitude,
MAV.sysid); // request vfr
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MAV.cs.ratesensors, MAV.sysid);
// request extra stuff - tridge
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MAV.cs.ratesensors,
MAV.sysid); // request raw sensor
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc, MAV.sysid);
// request rc info
}
catch
{
log.Error("Failed to request rates");
}
lastdata = DateTime.Now.AddSeconds(30); // prevent flooding
}
byte[] bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("RC_CHANNELS_SCALED Packet");
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION];
if (bytearray != null)
{
var version = bytearray.ByteArrayToStructure<MAVLink.mavlink_autopilot_version_t>(6);
//#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_DEV
// flight_sw_version 0x03040000 uint
byte main = (byte) (version.flight_sw_version >> 24);
byte sub = (byte) ((version.flight_sw_version >> 16) & 0xff);
byte rev = (byte) ((version.flight_sw_version >> 8) & 0xff);
MAVLink.FIRMWARE_VERSION_TYPE type =
(MAVLink.FIRMWARE_VERSION_TYPE) (version.flight_sw_version & 0xff);
this.version = new Version(main, sub, (int) type, rev);
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure<MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte) MAVLink.FENCE_BREACH.NONE)
{
// fence breached
messageHigh = "Fence Breach";
messageHighTime = DateTime.Now;
}
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int) (hil.roll_ailerons*10000);
hilch2 = (int) (hil.pitch_elevator*10000);
hilch3 = (int) (hil.throttle*10000);
hilch4 = (int) (hil.yaw_rudder*10000);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HIL_CONTROLS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.OPTICAL_FLOW];
if (bytearray != null)
{
var optflow = bytearray.ByteArrayToStructure<MAVLink.mavlink_optical_flow_t>(6);
opt_m_x = optflow.flow_comp_m_x;
opt_m_y = optflow.flow_comp_m_y;
opt_x = optflow.flow_x;
opt_y = optflow.flow_y;
opt_qua = optflow.quality;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MOUNT_STATUS];
if (bytearray != null)
{
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_mount_status_t>(6);
campointa = status.pointing_a/100.0f;
campointb = status.pointing_b/100.0f;
campointc = status.pointing_c/100.0f;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.VIBRATION];
if (bytearray != null)
{
var vibe = bytearray.ByteArrayToStructure<MAVLink.mavlink_vibration_t>(6);
vibeclip0 = vibe.clipping_0;
vibeclip1 = vibe.clipping_1;
vibeclip2 = vibe.clipping_2;
vibex = vibe.vibration_x;
vibey = vibe.vibration_y;
vibez = vibe.vibration_z;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AIRSPEED_AUTOCAL];
if (bytearray != null)
{
var asac = bytearray.ByteArrayToStructure<MAVLink.mavlink_airspeed_autocal_t>(6);
asratio = asac.ratio;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];
if (bytearray != null)
{
var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);
DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
try
{
date1 = date1.AddMilliseconds(systime.time_unix_usec/1000);
gpstime = date1;
}
catch
{
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc/1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HWSTATUS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.EKF_STATUS_REPORT];
if (bytearray != null)
{
var ekfstatusm = bytearray.ByteArrayToStructure<MAVLink.mavlink_ekf_status_report_t>(6);
// > 1, between 0-1 typical > 1 = reject measurement - red
// 0.5 > amber
ekfvelv = ekfstatusm.velocity_variance;
ekfcompv = ekfstatusm.compass_variance;
ekfposhor = ekfstatusm.pos_horiz_variance;
ekfposvert = ekfstatusm.pos_vert_variance;
ekfteralt = ekfstatusm.terrain_alt_variance;
ekfflags = ekfstatusm.flags;
ekfstatus =
(float)
Math.Max(ekfvelv,
Math.Max(ekfcompv, Math.Max(ekfposhor, Math.Max(ekfposvert, ekfteralt))));
if (ekfvelv >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.velocity_variance;
messageHighTime = DateTime.Now;
}
if (ekfcompv >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.compass_variance;
messageHighTime = DateTime.Now;
}
if (ekfposhor >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.pos_horiz_variance;
messageHighTime = DateTime.Now;
}
if (ekfposvert >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.pos_vert_variance;
messageHighTime = DateTime.Now;
}
if (ekfteralt >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.terrain_alt_variance;
messageHighTime = DateTime.Now;
}
for (int a = 1; a < (int) MAVLink.EKF_STATUS_FLAGS.ENUM_END; a = a << 1)
{
int currentbit = (ekfstatusm.flags & a);
if (currentbit == 0)
{
var currentflag =
(MAVLink.EKF_STATUS_FLAGS)
Enum.Parse(typeof (MAVLink.EKF_STATUS_FLAGS), a.ToString());
switch (currentflag)
{
case MAVLink.EKF_STATUS_FLAGS.EKF_ATTITUDE: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_HORIZ: // with pos
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_VERT: // with pos
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_ABS: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_ABS: // step 1
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_AGL: // range finder
//case MAVLink.EKF_STATUS_FLAGS.EKF_CONST_POS_MODE: // never true when absolute - non gps
//case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_ABS: // ekf has origin - post arm
//messageHigh = Strings.ERROR + " " + currentflag.ToString().Replace("_", " ");
//messageHighTime = DateTime.Now;
break;
default:
break;
}
}
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.POWER_STATUS];
if (bytearray != null)
{
var power = bytearray.ByteArrayToStructure<MAVLink.mavlink_power_status_t>(6);
boardvoltage = power.Vcc;
servovoltage = power.Vservo;
voltageflag = (MAVLink.MAV_POWER_STATUS) power.flags;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360)%360;
wind_vel = wind.speed*multiplierspeed;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
if (hb.type == (byte) MAVLink.MAV_TYPE.GCS)
{
// skip gcs hb's
// only happens on log playback - and shouldnt get them here
}
else
{
armed = (hb.base_mode & (byte) MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) ==
(byte) MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
// for future use
landed = hb.system_status == (byte) MAVLink.MAV_STATE.STANDBY;
failsafe = hb.system_status == (byte) MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte) MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.comPort.MAV.cs == this &&
MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
load = (float) sysstatus.load/10.0f;
battery_voltage = (float) sysstatus.voltage_battery/1000.0f;
battery_remaining = sysstatus.battery_remaining;
current = (float) sysstatus.current_battery/100.0f;
packetdropremote = sysstatus.drop_rate_comm;
Mavlink_Sensors sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
Mavlink_Sensors sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
Mavlink_Sensors sensors_present = new Mavlink_Sensors(sysstatus.onboard_control_sensors_present);
terrainactive = sensors_health.terrain && sensors_enabled.terrain && sensors_present.terrain;
if (sensors_health.gps != sensors_enabled.gps && sensors_present.gps)
{
messageHigh = Strings.BadGPSHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.gyro != sensors_enabled.gyro && sensors_present.gyro)
{
messageHigh = Strings.BadGyroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.accelerometer != sensors_enabled.accelerometer &&
sensors_present.accelerometer)
{
messageHigh = Strings.BadAccelHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.compass != sensors_enabled.compass && sensors_present.compass)
{
messageHigh = Strings.BadCompassHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.barometer != sensors_enabled.barometer && sensors_present.barometer)
{
messageHigh = Strings.BadBaroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.LASER_POSITION != sensors_enabled.LASER_POSITION &&
sensors_present.LASER_POSITION)
{
messageHigh = Strings.BadLiDARHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.optical_flow != sensors_enabled.optical_flow &&
sensors_present.optical_flow)
{
messageHigh = Strings.BadOptFlowHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.terrain != sensors_enabled.terrain && sensors_present.terrain)
{
messageHigh = Strings.BadorNoTerrainData;
messageHighTime = DateTime.Now;
}
else if (sensors_health.geofence != sensors_enabled.geofence &&
sensors_present.geofence)
{
messageHigh = Strings.GeofenceBreach;
messageHighTime = DateTime.Now;
}
else if (sensors_health.ahrs != sensors_enabled.ahrs && sensors_present.ahrs)
{
messageHigh = Strings.BadAHRS;
messageHighTime = DateTime.Now;
}
else if (sensors_health.rc_receiver != sensors_enabled.rc_receiver &&
sensors_present.rc_receiver)
{
messageHigh = Strings.NORCReceiver;
messageHighTime = DateTime.Now;
}
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.BATTERY2];
if (bytearray != null)
{
var bat = bytearray.ByteArrayToStructure<MAVLink.mavlink_battery2_t>(6);
_battery_voltage2 = bat.voltage;
current2 = bat.current_battery;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT];
if (bytearray != null)
{
var terrainrep = bytearray.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>(6);
ter_curalt = terrainrep.current_height;
ter_alt = terrainrep.terrain_height;
ter_load = terrainrep.loaded;
ter_pend = terrainrep.pending;
ter_space = terrainrep.spacing;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll*rad2deg;
pitch = att.pitch*rad2deg;
yaw = att.yaw*rad2deg;
//Console.WriteLine(MAV.sysid + " " +roll + " " + pitch + " " + yaw);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.ATTITUDE] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
alt = loc.relative_alt/1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat/10000000.0;
lng = loc.lon/10000000.0;
altasl = loc.alt/1000.0f;
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat*1.0e-7;
lng = gps.lon*1.0e-7;
altasl = gps.alt/1000.0f;
// alt = gps.alt; // using vfr as includes baro calc
}
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float) Math.Round((double) gps.eph/100.0, 2);
satcount = gps.satellites_visible;
groundspeed = gps.vel*1.0e-2f;
groundcourse = gps.cog*1.0e-2f;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.GPS_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS2_RAW];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps2_raw_t>(6);
lat2 = gps.lat*1.0e-7;
lng2 = gps.lon*1.0e-7;
altasl2 = gps.alt/1000.0f;
gpsstatus2 = gps.fix_type;
gpshdop2 = (float) Math.Round((double) gps.eph/100.0, 2);
satcount2 = gps.satellites_visible;
groundspeed2 = gps.vel*1.0e-2f;
groundcourse2 = gps.cog*1.0e-2f;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int) wpno;
wpno = wpcur.seq;
if (mode.ToLower() == "auto" && wpno != 0)
{
lastautowp = (int) wpno;
}
if (oldwp != wpno && MainV2.speechEnable && MainV2.comPort.MAV.cs == this &&
MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error/100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RPM];
if (bytearray != null)
{
var rpm = bytearray.ByteArrayToStructure<MAVLink.mavlink_rpm_t>(6);
rpm1 = rpm.rpm1;
rpm2 = rpm.rpm2;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (int) ((rcin.rssi/255.0)*100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
ch9in = rcin.chan9_raw;
ch10in = rcin.chan10_raw;
ch11in = rcin.chan11_raw;
ch12in = rcin.chan12_raw;
ch13in = rcin.chan13_raw;
ch14in = rcin.chan14_raw;
ch15in = rcin.chan15_raw;
ch16in = rcin.chan16_raw;
//percent
rxrssi = (int) ((rcin.rssi/255.0)*100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
var timesec = imu.time_usec*1.0e-6;
var deltawall = (DateTime.Now - lastimutime).TotalSeconds;
var deltaimu = timesec - imutime;
//Console.WriteLine( + " " + deltawall + " " + deltaimu + " " + System.Threading.Thread.CurrentThread.Name);
if (speedup > 0)
speedup = (float) (speedup*0.95 + (deltaimu/deltawall)*0.05);
imutime = timesec;
lastimutime = DateTime.Now;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU2];
if (bytearray != null)
{
var imu2 = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro;
gy2 = imu2.ygyro;
gz2 = imu2.zgyro;
ax2 = imu2.xacc;
ay2 = imu2.yacc;
az2 = imu2.zacc;
mx2 = imu2.xmag;
my2 = imu2.ymag;
mz2 = imu2.zmag;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU3];
if (bytearray != null)
{
var imu3 = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu3_t>(6);
gx3 = imu3.xgyro;
gy3 = imu3.ygyro;
gz3 = imu3.zgyro;
ax3 = imu3.xacc;
ay3 = imu3.yacc;
az3 = imu3.zacc;
mx3 = imu3.xmag;
my3 = imu3.ymag;
mz3 = imu3.zmag;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.PID_TUNING];
if (bytearray != null)
{
var pid = bytearray.ByteArrayToStructure<MAVLink.mavlink_pid_tuning_t>(6);
//todo: currently only deals with single axis at once
pidff = pid.FF;
pidP = pid.P;
pidI = pid.I;
pidD = pid.D;
pidaxis = pid.axis;
piddesired = pid.desired;
pidachieved = pid.achieved;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
//alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
// heading = vfr.heading;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
try
{
if (csCallBack != null)
csCallBack(this, null);
}
catch
{
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
bs.DataSource = this;
bs.ResetBindings(false);
return;
/*
if (bs.Count > 200)
{
while (bs.Count > 3)
bs.RemoveAt(1);
//bs.Clear();
}
bs.Add(this);
/*
return;
bs.DataSource = this;
bs.ResetBindings(false);
return;
hires.Stopwatch sw = new hires.Stopwatch();
sw.Start();
bs.DataSource = this;
bs.ResetBindings(false);
sw.Stop();
var elaps = sw.Elapsed;
Console.WriteLine("1 " + elaps.ToString("0.#####") + " done ");
sw.Start();
bs.SuspendBinding();
bs.Clear();
bs.ResumeBinding();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("2 " + elaps.ToString("0.#####") + " done ");
sw.Start();
if (bs.Count > 100)
bs.Clear();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("3 " + elaps.ToString("0.#####") + " done ");
*/
}
}
catch
{
log.InfoFormat("CurrentState Binding error");
}
}
}
public MAVLinkInterface()
{
// init fields
this.BaseStream = new SerialPort();
this.packetcount = 0;
this._bytesReceivedSubj = new Subject<int>();
this._bytesSentSubj = new Subject<int>();
this.WhenPacketLost = new Subject<int>();
this.WhenPacketReceived = new Subject<int>();
this.readlock = new object();
this.mavlinkversion = 0;
this.debugmavlink = false;
this.logreadmode = false;
this.lastlogread = DateTime.MinValue;
this.logplaybackfile = null;
this.logfile = null;
this.rawlogfile = null;
this.bps1 = 0;
this.bps2 = 0;
this.bpstime = DateTime.MinValue;
this.lastbad = new byte[2];
for (int a = 0; a < MAVlist.Length; a++ )
MAVlist[a] = new MAVState();
}
public static GMapMarker getMAVMarker(MAVState MAV)
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING)
{
// colorise map marker/s based on their sysid, for common sysid/s used 1-6, 11-16, and 101-106
// its rare for ArduPilot to be used to fly more than 6 planes at a time from one console.
int which = 0; // default 0=red for other sysids
if ((MAV.sysid >= 1) && (MAV.sysid <= 6))
{
which = MAV.sysid - 1;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
if ((MAV.sysid >= 11) && (MAV.sysid <= 16))
{
which = MAV.sysid - 11;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
if ((MAV.sysid >= 101) && (MAV.sysid <= 106))
{
which = MAV.sysid - 101;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
return(new GMapMarkerPlane(which, portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing,
MAV.cs.radius * CurrentState.multiplierdist)
{
ToolTipText = MAV.cs.alt.ToString("0") + CurrentState.AltUnit + " | " + (int)MAV.cs.airspeed +
CurrentState.SpeedUnit + " | id:" + (int)MAV.sysid + " | Sats:" + (int)MAV.cs.satcount + " | HDOP:" + (float)MAV.cs.gpshdop + " | Volts: " + (float)MAV.cs.battery_voltage,
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return(new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
ToolTipText = MAV.cs.alt.ToString("0") + "\n" + MAV.sysid.ToString("sysid: 0"),
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SURFACE_BOAT)
{
return(new GMapMarkerBoat(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SUBMARINE)
{
return(new GMapMarkerSub(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return(new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing));
}
else if (MAV.cs.firmware == Firmwares.ArduTracker)
{
return(new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing));
}
else if (MAV.cs.firmware == Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
if (MAV.param.ContainsKey("AVD_W_DIST_XY") && MAV.param.ContainsKey("AVD_F_DIST_XY"))
{
var w = MAV.param["AVD_W_DIST_XY"].Value;
var f = MAV.param["AVD_F_DIST_XY"].Value;
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
danger = (int)f,
warn = (int)w
});
}
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
ToolTipText = MAV.cs.alt.ToString("0") + CurrentState.AltUnit + " | " + (int)MAV.cs.airspeed +
CurrentState.SpeedUnit + " | id:" + (int)MAV.sysid + " | Sats:" + (int)MAV.cs.satcount + " | HDOP:" + (float)MAV.cs.gpshdop + " | Volts: " + (float)MAV.cs.battery_voltage,
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return(new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else
{
// unknown type
return(new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot));
}
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLinkInterface mavinterface, MAVState MAV)
{
lock (this)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (parent != null && parent.packetsnotlost != 0)
{
if ((DateTime.Now - mavinterface.lastvalidpacket).TotalSeconds > 10)
{
linkqualitygcs = 0;
}
else
{
linkqualitygcs = (ushort)((parent.packetsnotlost / (parent.packetsnotlost + parent.packetslost)) * 100.0);
}
}
if (datetime.Second != lastsecondcounter.Second)
{
lastsecondcounter = datetime;
if (lastpos.Lat != 0 && lastpos.Lng != 0 && armed)
{
if (!mavinterface.BaseStream.IsOpen && !mavinterface.logreadmode)
distTraveled = 0;
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
timeInAir++;
if (!gotwind)
dowindcalc();
}
byte[] bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("RC_CHANNELS_SCALED Packet");
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure<MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte)MAVLink.FENCE_BREACH.NONE)
{
// fence breached
messageHigh = "Fence Breach";
messageHighTime = DateTime.Now;
}
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HIL_CONTROLS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MOUNT_STATUS];
if (bytearray != null)
{
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_mount_status_t>(6);
campointa = status.pointing_a / 100.0f;
campointb = status.pointing_b / 100.0f;
campointc = status.pointing_c / 100.0f;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.AIRSPEED_AUTOCAL];
if (bytearray != null)
{
var asac = bytearray.ByteArrayToStructure<MAVLink.mavlink_airspeed_autocal_t>(6);
asratio = asac.ratio;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];
if (bytearray != null)
{
var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);
DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
date1 = date1.AddMilliseconds(systime.time_unix_usec / 1000);
gpstime = date1;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HWSTATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.POWER_STATUS];
if (bytearray != null)
{
var power = bytearray.ByteArrayToStructure<MAVLink.mavlink_power_status_t>(6);
boardvoltage = power.Vcc;
servovoltage = power.Vservo;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
if (hb.type == (byte)MAVLink.MAV_TYPE.GCS)
{
// skip gcs hb's
// only happens on log playback - and shouldnt get them here
}
else
{
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
// for future use
landed = hb.system_status == (byte)MAVLink.MAV_STATE.STANDBY;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.comPort.MAV.cs == this && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
load = (float)sysstatus.load / 10.0f;
battery_voltage = (float)sysstatus.voltage_battery / 1000.0f;
battery_remaining = sysstatus.battery_remaining;
current = (float)sysstatus.current_battery / 100.0f;
packetdropremote = sysstatus.drop_rate_comm;
Mavlink_Sensors sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
Mavlink_Sensors sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
Mavlink_Sensors sensors_present = new Mavlink_Sensors(sysstatus.onboard_control_sensors_present);
if (sensors_health.gps != sensors_enabled.gps && sensors_present.gps)
{
messageHigh = "Bad GPS Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.gyro != sensors_enabled.gyro && sensors_present.gyro)
{
messageHigh = "Bad Gyro Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.accelerometer != sensors_enabled.accelerometer && sensors_present.accelerometer)
{
messageHigh = "Bad Accel Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.compass != sensors_enabled.compass && sensors_present.compass)
{
messageHigh = "Bad Compass Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.barometer != sensors_enabled.barometer && sensors_present.barometer)
{
messageHigh = "Bad Baro Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.optical_flow != sensors_enabled.optical_flow && sensors_present.optical_flow)
{
messageHigh = "Bad OptFlow Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.terrain != sensors_enabled.terrain && sensors_present.terrain)
{
messageHigh = "Bad or No Terrain Data";
messageHighTime = DateTime.Now;
}
else if (sensors_health.geofence != sensors_enabled.geofence && sensors_present.geofence)
{
messageHigh = "Geofence Breach";
messageHighTime = DateTime.Now;
}
else if (sensors_health.ahrs != sensors_enabled.ahrs && sensors_present.ahrs)
{
messageHigh = "Bad AHRS";
messageHighTime = DateTime.Now;
}
else if (sensors_health.rc_receiver != sensors_enabled.rc_receiver && sensors_present.rc_receiver)
{
messageHigh = "NO RC Receiver";
messageHighTime = DateTime.Now;
}
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.BATTERY2];
if (bytearray != null)
{
var bat = bytearray.ByteArrayToStructure<MAVLink.mavlink_battery2_t>(6);
_battery_voltage2 = bat.voltage;
current2 = bat.current_battery;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT];
if (bytearray != null)
{
var terrainrep = bytearray.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>(6);
ter_curalt = terrainrep.current_height;
ter_alt = terrainrep.terrain_height;
ter_load = terrainrep.loaded;
ter_pend = terrainrep.pending;
ter_space = terrainrep.spacing;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.ATTITUDE] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7;
lng = gps.lon * 1.0e-7;
altasl = gps.alt / 1000.0f;
// alt = gps.alt; // using vfr as includes baro calc
}
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0,2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.GPS_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS2_RAW];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps2_raw_t>(6);
lat2 = gps.lat * 1.0e-7;
lng2 = gps.lon * 1.0e-7;
altasl2 = gps.alt / 1000.0f;
gpsstatus2 = gps.fix_type;
gpshdop2 = (float)Math.Round((double)gps.eph / 100.0, 2);
satcount2 = gps.satellites_visible;
groundspeed2 = gps.vel * 1.0e-2f;
groundcourse2 = gps.cog * 1.0e-2f;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
alt = loc.relative_alt / 1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat / 10000000.0;
lng = loc.lon / 10000000.0;
altasl = loc.alt / 1000.0f;
}
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechEnable && MainV2.comPort.MAV.cs == this && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (int)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
//alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
// heading = vfr.heading;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
bs.DataSource = this;
bs.ResetBindings(false);
return;
/*
if (bs.Count > 200)
{
while (bs.Count > 3)
bs.RemoveAt(1);
//bs.Clear();
}
bs.Add(this);
/*
return;
bs.DataSource = this;
bs.ResetBindings(false);
return;
hires.Stopwatch sw = new hires.Stopwatch();
sw.Start();
bs.DataSource = this;
bs.ResetBindings(false);
sw.Stop();
var elaps = sw.Elapsed;
Console.WriteLine("1 " + elaps.ToString("0.#####") + " done ");
sw.Start();
bs.SuspendBinding();
bs.Clear();
bs.ResumeBinding();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("2 " + elaps.ToString("0.#####") + " done ");
sw.Start();
if (bs.Count > 100)
bs.Clear();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("3 " + elaps.ToString("0.#####") + " done ");
*/
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
public static GMapMarker getMAVMarker(MAVState MAV)
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING || MAV.aptype >= MAVLink.MAV_TYPE.VTOL_DUOROTOR && MAV.aptype <= MAVLink.MAV_TYPE.VTOL_RESERVED5)
{
// colorise map marker/s based on their sysid, for common sysid/s used 1-6, 11-16, and 101-106
// its rare for ArduPilot to be used to fly more than 6 planes at a time from one console.
int which = 0; // default 0=red for other sysids
if ((MAV.sysid >= 1) && (MAV.sysid <= 6))
{
which = MAV.sysid - 1;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
if ((MAV.sysid >= 11) && (MAV.sysid <= 16))
{
which = MAV.sysid - 11;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
if ((MAV.sysid >= 101) && (MAV.sysid <= 106))
{
which = MAV.sysid - 101;
} //1=black, 2=blue, 3=green,4=yellow,5=orange,6=red
return(new GMapMarkerPlane(which, portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing,
MAV.cs.radius * CurrentState.multiplierdist)
{
ToolTipText = ArduPilot.Common.speechConversion(MAV, "" + Settings.Instance["mapicondesc"]),
ToolTipMode = String.IsNullOrEmpty(Settings.Instance["mapicondesc"]) ? MarkerTooltipMode.Never : MarkerTooltipMode.Always,
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return(new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
ToolTipText = MAV.cs.alt.ToString("0") + "\n" + MAV.sysid.ToString("sysid: 0"),
ToolTipMode = MarkerTooltipMode.Always,
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SURFACE_BOAT)
{
return(new GMapMarkerBoat(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SUBMARINE)
{
return(new GMapMarkerSub(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return(new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing)
{
Tag = MAV
});
}
else if (MAV.cs.firmware == Firmwares.ArduTracker)
{
return(new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return(new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
Tag = MAV
});
}
else if (MAV.cs.firmware == Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
if (MAV.param.ContainsKey("AVD_W_DIST_XY") && MAV.param.ContainsKey("AVD_F_DIST_XY"))
{
var w = MAV.param["AVD_W_DIST_XY"].Value;
var f = MAV.param["AVD_F_DIST_XY"].Value;
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
danger = (int)f,
warn = (int)w,
Tag = MAV
});
}
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
ToolTipText = ArduPilot.Common.speechConversion(MAV, "" + Settings.Instance["mapicondesc"]),
ToolTipMode = String.IsNullOrEmpty(Settings.Instance["mapicondesc"]) ? MarkerTooltipMode.Never : MarkerTooltipMode.Always,
Tag = MAV
});
}
else
{
// unknown type
return(new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot)
{
Tag = MAV
});
}
}
public static GMapMarker getMAVMarker(MAVState MAV, GMapOverlay overlay = null)
{
PointLatLng portlocation = MAV.cs.Location;
if (overlay != null)
{
var existing = overlay.Markers.Where((a) => a.Tag == MAV).ToArray();
if (existing.Count() > 1)
{
existing.Skip(1).ToArray().ForEach((a) => { overlay.Markers.Remove(a); });
}
if (existing.Count() > 0)
{
var item = existing.First();
if (item is GMapMarkerPlane)
{
var itemp = (GMapMarkerPlane)item;
itemp.Position = portlocation;
itemp.Heading = MAV.cs.yaw;
itemp.Cog = MAV.cs.groundcourse;
itemp.Target = MAV.cs.target_bearing;
itemp.Nav_bearing = MAV.cs.nav_bearing;
itemp.Radius = MAV.cs.radius * CurrentState.multiplierdist;
return(null);
}
else if (item is GMapMarkerQuad)
{
var itemq = (GMapMarkerQuad)item;
itemq.Position = portlocation;
itemq.Heading = MAV.cs.yaw;
itemq.Cog = MAV.cs.groundcourse;
itemq.Target = MAV.cs.nav_bearing;
itemq.Sysid = MAV.sysid;
return(null);
}
else if (item is GMapMarkerRover)
{
var itemr = (GMapMarkerRover)item;
itemr.Position = portlocation;
itemr.Heading = MAV.cs.yaw;
itemr.Cog = MAV.cs.groundcourse;
itemr.Target = MAV.cs.target_bearing;
itemr.Nav_bearing = MAV.cs.nav_bearing;
return(null);
}
else
{
existing.ForEach((a) => overlay.Markers.Remove(a));
}
}
}
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING || MAV.aptype >= MAVLink.MAV_TYPE.VTOL_DUOROTOR && MAV.aptype <= MAVLink.MAV_TYPE.VTOL_RESERVED5)
{
return(new GMapMarkerPlane(MAV.sysid - 1, portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing,
MAV.cs.radius * CurrentState.multiplierdist)
{
ToolTipText = ArduPilot.Common.speechConversion(MAV, "" + Settings.Instance["mapicondesc"]),
ToolTipMode = String.IsNullOrEmpty(Settings.Instance["mapicondesc"]) ? MarkerTooltipMode.Never : MarkerTooltipMode.Always,
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return(new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
ToolTipText = MAV.cs.alt.ToString("0") + "\n" + MAV.sysid.ToString("sysid: 0"),
ToolTipMode = MarkerTooltipMode.Always,
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SURFACE_BOAT)
{
return(new GMapMarkerBoat(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SUBMARINE)
{
return(new GMapMarkerSub(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return(new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing)
{
Tag = MAV
});
}
else if (MAV.cs.firmware == Firmwares.ArduTracker)
{
return(new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing)
{
Tag = MAV
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return(new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
Tag = MAV
});
}
else if (MAV.cs.firmware == Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
if (MAV.param.ContainsKey("AVD_W_DIST_XY") && MAV.param.ContainsKey("AVD_F_DIST_XY"))
{
var w = MAV.param["AVD_W_DIST_XY"].Value;
var f = MAV.param["AVD_F_DIST_XY"].Value;
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
danger = (int)f,
warn = (int)w,
Tag = MAV
});
}
return(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
ToolTipText = ArduPilot.Common.speechConversion(MAV, "" + Settings.Instance["mapicondesc"]),
ToolTipMode = String.IsNullOrEmpty(Settings.Instance["mapicondesc"]) ? MarkerTooltipMode.Never : MarkerTooltipMode.Always,
Tag = MAV
});
}
else
{
// unknown type
return(new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot)
{
Tag = MAV
});
}
}
public static GMapMarker getMAVMarker(MAVState MAV)
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING)
{
return (new GMapMarkerPlane(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing, MAV.cs.radius)
{
ToolTipText = MAV.cs.alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return (new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return (new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduTracker)
{
return (new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
return (new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return (new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else
{
// unknown type
return (new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot));
}
}
public static GMapMarker getMAVMarker(MAVState MAV)
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.aptype == MAVLink.MAV_TYPE.FIXED_WING)
{
return (new GMapMarkerPlane(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing, MAV.cs.radius)
{
ToolTipText = MAV.cs.alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.aptype == MAVLink.MAV_TYPE.GROUND_ROVER)
{
return (new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.SURFACE_BOAT)
{
return (new GMapMarkerBoat(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
return (new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduTracker)
{
return (new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduCopter2 || MAV.aptype == MAVLink.MAV_TYPE.QUADROTOR)
{
if (MAV.param.ContainsKey("AVD_W_DIST_XY") && MAV.param.ContainsKey("AVD_F_DIST_XY"))
{
var w = MAV.param["AVD_W_DIST_XY"].Value;
var f = MAV.param["AVD_F_DIST_XY"].Value;
return (new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid)
{
danger = (int)f,
warn = (int)w
});
}
return (new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.COAXIAL)
{
return (new GMapMarkerSingle(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else
{
// unknown type
return (new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot));
}
}