Inheritance: MAVLink
Esempio n. 1
0
        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);
        }
Esempio n. 2
0
        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));
            }
        }
Esempio n. 3
0
        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();
        }
Esempio n. 5
0
        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));
            }
        }
Esempio n. 6
0
        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"); }
            }
        }
Esempio n. 7
0
        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
                });
            }
        }
Esempio n. 8
0
        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
                });
            }
        }
Esempio n. 9
0
        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));
            }
        }
Esempio n. 10
0
        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));
            }
        }