Example #1
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 = [email protected];
                    }

                    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 = [email protected];
                    }

                    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");
                }
            }
        }
Example #2
0
        /*
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
        {
            UpdateCurrentSettings(bs, false, MainV2.comPort);
        }
        */
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
        {
            if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz
            {
                lastupdate = DateTime.Now;

                if (DateTime.Now.Second != lastwindcalc.Second)
                {
                    lastwindcalc = DateTime.Now;
                    dowindcalc();
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
                {

                    string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);

                    int ind = logdata.IndexOf('\0');
                    if (ind != -1)
                        logdata = logdata.Substring(0, ind);
                    if (messages.Count > 5)
                    {
                        messages.RemoveAt(0);
                    }
                    messages.Add(logdata + "\n");

                    mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil
                {
                    var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t();

                    object temp = hil;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6);

                    hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp);

                    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;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
                {
                    MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t();

                    object temp = nav;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6);

                    nav = (MAVLink.__mavlink_nav_controller_output_t)(temp);

                    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;
                    xtrack_error = nav.xtrack_error;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
                }
            #if MAVLINK10
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();

                    object temp = hb;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);

                    hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);

                    string oldmode = mode;

                    mode = "Unknown";

                    if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0)
                    {
                        if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING)
                        {
                            switch (hb.custom_mode)
                            {
                                case (byte)(Common.apmmodes.MANUAL):
                                    mode = "Manual";
                                    break;
                                case (byte)(Common.apmmodes.GUIDED):
                                    mode = "Guided";
                                    break;
                                case (byte)(Common.apmmodes.STABILIZE):
                                    mode = "Stabilize";
                                    break;
                                case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
                                    mode = "FBW A";
                                    break;
                                case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
                                    mode = "FBW B";
                                    break;
                                case (byte)(Common.apmmodes.AUTO):
                                    mode = "Auto";
                                    break;
                                case (byte)(Common.apmmodes.RTL):
                                    mode = "RTL";
                                    break;
                                case (byte)(Common.apmmodes.LOITER):
                                    mode = "Loiter";
                                    break;
                                case (byte)(Common.apmmodes.CIRCLE):
                                    mode = "Circle";
                                    break;
                                default:
                                    mode = "Unknown";
                                    break;
                            }
                        }
                        else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR)
                        {
                            switch (hb.custom_mode)
                            {
                                case (byte)(Common.ac2modes.STABILIZE):
                                    mode = "Stabilize";
                                    break;
                                case (byte)(Common.ac2modes.ACRO):
                                    mode = "Acro";
                                    break;
                                case (byte)(Common.ac2modes.ALT_HOLD):
                                    mode = "Alt Hold";
                                    break;
                                case (byte)(Common.ac2modes.AUTO):
                                    mode = "Auto";
                                    break;
                                case (byte)(Common.ac2modes.GUIDED):
                                    mode = "Guided";
                                    break;
                                case (byte)(Common.ac2modes.LOITER):
                                    mode = "Loiter";
                                    break;
                                case (byte)(Common.ac2modes.RTL):
                                    mode = "RTL";
                                    break;
                                case (byte)(Common.ac2modes.CIRCLE):
                                    mode = "Circle";
                                    break;
                                default:
                                    mode = "Unknown";
                                    break;
                            }
                        }
                    }

                    if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                    }
                }

                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();

                    object temp = sysstatus;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);

                    sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);

                    battery_voltage = sysstatus.voltage_battery;
                    battery_remaining = sysstatus.battery_remaining;

                    packetdropremote = sysstatus.drop_rate_comm;

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                }
                #else

                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();

                    object temp = sysstatus;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);

                    sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);

                    string oldmode = mode;

                    switch (sysstatus.mode)
                    {
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
                                    mode = "Initialising";
                                    break;
                            }
                            break;
                        case (byte)(100 + Common.ac2modes.STABILIZE):
                            mode = "Stabilize";
                            break;
                        case (byte)(100 + Common.ac2modes.ACRO):
                            mode = "Acro";
                            break;
                        case (byte)(100 + Common.ac2modes.ALT_HOLD):
                            mode = "Alt Hold";
                            break;
                        case (byte)(100 + Common.ac2modes.AUTO):
                            mode = "Auto";
                            break;
                        case (byte)(100 + Common.ac2modes.GUIDED):
                            mode = "Guided";
                            break;
                        case (byte)(100 + Common.ac2modes.LOITER):
                            mode = "Loiter";
                            break;
                        case (byte)(100 + Common.ac2modes.RTL):
                            mode = "RTL";
                            break;
                        case (byte)(100 + Common.ac2modes.CIRCLE):
                            mode = "Circle";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
                            mode = "Manual";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
                            mode = "Guided";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
                            mode = "Stabilize";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
                            mode = "FBW A"; // fall though  old
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)1:
                                    mode = "FBW A";
                                    break;
                                case (byte)2:
                                    mode = "FBW B";
                                    break;
                                case (byte)3:
                                    mode = "FBW C";
                                    break;
                            }
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
                            mode = "FBW B";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
                                    mode = "Auto";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
                                    mode = "RTL";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
                                    mode = "Loiter";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
                                    mode = "Takeoff";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
                                    mode = "Land";
                                    break;
                            }

                            break;
                        default:
                            mode = "Unknown";
                            break;
                    }

                    battery_voltage = sysstatus.vbat;
                    battery_remaining = sysstatus.battery_remaining;

                    packetdropremote = sysstatus.packet_drop;

                    if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                }
            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
                {
                    var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();

                    object temp = att;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6);

                    att = (MAVLink.__mavlink_attitude_t)(temp);

                    roll = att.roll * rad2deg;
                    pitch = att.pitch * rad2deg;
                    yaw = att.yaw * rad2deg;

                    //                    Console.WriteLine(roll + " " + pitch + " " + yaw);

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
                }
            #if MAVLINK10
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_raw_int_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);

                    lat = gps.lat * 1.0e-7f;
                    lng = gps.lon * 1.0e-7f;
                    //                alt = gps.alt; // using vfr as includes baro calc

                    gpsstatus = gps.fix_type;
                    //                    Console.WriteLine("gpsfix {0}",gpsstatus);

                    gpshdop = gps.eph;

                    groundspeed = gps.vel * 1.0e-2f;
                    groundcourse = gps.cog * 1.0e-2f;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
                }
                #else

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_raw_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_raw_t)(temp);

                    lat = gps.lat;
                    lng = gps.lon;
                    //                alt = gps.alt; // using vfr as includes baro calc

                    gpsstatus = gps.fix_type;
                    //                    Console.WriteLine("gpsfix {0}",gpsstatus);

                    gpshdop = gps.eph;

                    groundspeed = gps.v;
                    groundcourse = gps.hdg;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
                }
            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_status_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_status_t)(temp);

                    satcount = gps.satellites_visible;
                }
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
                {
                    var loc = new MAVLink.__mavlink_global_position_int_t();

                    object temp = loc;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6);

                    loc = (MAVLink.__mavlink_global_position_int_t)(temp);

                    //alt = loc.alt / 1000.0f;
                    lat = loc.lat / 10000000.0f;
                    lng = loc.lon / 10000000.0f;

                }
            #if MAVLINK10
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();

                    object temp = wpcur;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);

                    wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);

                    int oldwp = (int)wpno;

                    wpno = wpcur.seq;

                    if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
                }
                #else
                   if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
                {
                    var loc = new MAVLink.__mavlink_global_position_t();

                    object temp = loc;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6);

                    loc = (MAVLink.__mavlink_global_position_t)(temp);

                    alt = loc.alt;
                    lat = loc.lat;
                    lng = loc.lon;

                }
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t();

                    object temp = wpcur;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6);

                    wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);

                    int oldwp = (int)wpno;

                    wpno = wpcur.seq;

                    if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
                }

            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null)
                {
                    var rcin = new MAVLink.__mavlink_rc_channels_raw_t();

                    object temp = rcin;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6);

                    rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp);

                    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;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null)
                {
                    var servoout = new MAVLink.__mavlink_servo_output_raw_t();

                    object temp = servoout;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6);

                    servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp);

                    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;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null)
                {
                    var imu = new MAVLink.__mavlink_raw_imu_t();

                    object temp = imu;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6);

                    imu = (MAVLink.__mavlink_raw_imu_t)(temp);

                    gx = imu.xgyro;
                    gy = imu.ygyro;
                    gz = imu.zgyro;

                    ax = imu.xacc;
                    ay = imu.yacc;
                    az = imu.zacc;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null)
                {
                    var imu = new MAVLink.__mavlink_scaled_imu_t();

                    object temp = imu;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6);

                    imu = (MAVLink.__mavlink_scaled_imu_t)(temp);

                    gx = imu.xgyro;
                    gy = imu.ygyro;
                    gz = imu.zgyro;

                    ax = imu.xacc;
                    ay = imu.yacc;
                    az = imu.zacc;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
                {
                    MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();

                    object temp = vfr;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6);

                    vfr = (MAVLink.__mavlink_vfr_hud_t)(temp);

                    groundspeed = vfr.groundspeed;
                    airspeed = vfr.airspeed;

                    alt = vfr.alt; // this might include baro

                    //climbrate = vfr.climb;

                    if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt)
                    {
                        climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                        verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                        if (float.IsInfinity(_verticalspeed))
                            _verticalspeed = 0;
                        lastalt = DateTime.Now;
                        oldalt = alt;
                    }

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil
                {
                    var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t();

                    object temp = mem;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6);

                    mem = (MAVLink.__mavlink_meminfo_t)(temp);

                    freemem = mem.freemem;
                    brklevel = mem.brkval;

                }

            }
            //Console.WriteLine(DateTime.Now.Millisecond + " start ");
            // update form
            try
            {
                if (bs != null)
                {
                    //Console.WriteLine(DateTime.Now.Millisecond);
                    bs.DataSource = this;
                    //Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow);
                    bs.ResetBindings(false);
                    //Console.WriteLine(DateTime.Now.Millisecond + " done ");
                }
            }
            catch { Console.WriteLine("CurrentState Binding error"); }
        }
        /*
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
        {
            UpdateCurrentSettings(bs, false, MainV2.comPort);
        }
        */
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLinkInterface mavinterface)
        {
            lock (this)
            {

                if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
                {
                    lastupdate = DateTime.Now;

                    //check if valid mavinterface
                    if (mavinterface != null && mavinterface.packetsnotlost != 0)
                        linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0);

                    if (DateTime.Now.Second != lastsecondcounter.Second)
                    {
                        lastsecondcounter = DateTime.Now;

                        if (lastpos.Lat != 0 && lastpos.Lng != 0 && armed)
                        {
                            if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.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();
                    }

                    if (mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT] != null) // status text
                    {

                        var msg = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);

                        /*
            enum gcs_severity {
            SEVERITY_LOW=1,
            SEVERITY_MEDIUM,
            SEVERITY_HIGH,
            SEVERITY_CRITICAL
            };
                         */

                       byte sev = msg.severity;

                        string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT], 6, mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT].Length - 6);

                        int ind = logdata.IndexOf('\0');
                        if (ind != -1)
                            logdata = logdata.Substring(0, ind);

                        if (sev >= 3)
                        {
                            messageHigh = logdata;
                            messageHighTime = DateTime.Now;
                        }

                        try
                        {
                            while (messages.Count > 50)
                            {
                                messages.RemoveAt(0);
                            }
                            messages.Add(logdata + "\n");

                        }
                        catch { }

                        mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT] = null;
                    }

                    byte[] bytearray = mavinterface.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");

                        mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
                    }

                    bytearray = mavinterface.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;
                        }

                        mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
                    }

                    bytearray = mavinterface.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 = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];

                    if (bytearray != null)
                    {
                        var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);

                        DateTime newtime = DateTime.Now;

                          //UInt64 ms_per_week = 7000ULL*86400ULL;
             //UInt64 unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
            //UInt64 fix_time_ms = unix_offset + time_week*ms_per_week + time_week_ms;
                    }

                    bytearray = mavinterface.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 = mavinterface.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 = mavinterface.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 = mavinterface.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
                            bool 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.getConfig("speechmodeenabled") == "True")
                            {
                                MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                            }
                        }
                    }

                    bytearray = mavinterface.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 = (float)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)
                        {
                            messageHigh = "Bad GPS Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_health.gyro != sensors_enabled.gyro)
                        {
                            messageHigh = "Bad Gyro Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_health.accelerometer != sensors_enabled.accelerometer)
                        {
                            messageHigh = "Bad Accel Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_health.compass != sensors_enabled.compass)
                        {
                            messageHigh = "Bad Compass Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_health.barometer != sensors_enabled.barometer)
                        {
                            messageHigh = "Bad Baro Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_health.optical_flow != sensors_enabled.optical_flow)
                        {
                            messageHigh = "Bad OptFlow Health";
                            messageHighTime = DateTime.Now;
                        }
                        else if (sensors_present.rc_receiver != sensors_enabled.rc_receiver)
                        {
                            messageHigh = "NO RC Receiver";
                            messageHighTime = DateTime.Now;
                        }

                        mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
                    }

                    bytearray = mavinterface.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 = mavinterface.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 = mavinterface.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 = mavinterface.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-7f;
                            lng = gps.lon * 1.0e-7f;

                           // alt = gps.alt; // using vfr as includes baro calc
                        }

                        altasl = gps.alt / 1000.0f;

                        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 = mavinterface.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 = mavinterface.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 = [email protected];
                    }

                    bytearray = mavinterface.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 = [email protected];
                    }

                    bytearray = mavinterface.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.0f;
                            lng = loc.lon / 10000000.0f;
                        }
                    }

                    bytearray = mavinterface.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.getConfig("speechwaypointenabled") == "True")
                        {
                            MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                        }

                        //MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
                    }

                    bytearray = mavinterface.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 = mavinterface.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 = (float)((rcin.rssi / 255.0) * 100.0);

                        //MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
                    }

                    bytearray = mavinterface.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;

                        mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
                    }

                    bytearray = mavinterface.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 = mavinterface.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;

                        //MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
                    }

                    bytearray = mavinterface.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;

                        //MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
                    }

                    bytearray = mavinterface.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)
                    {
                        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 void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
        {
            UpdateCurrentSettings(bs, false, MainV2.comPort);
        }
        */
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
        {
            lock (locker)
            {
                if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz
                {
                    lastupdate = DateTime.Now;

                    if (DateTime.Now.Second != lastsecondcounter.Second)
                    {
                        lastsecondcounter = DateTime.Now;

                        if (lastpos.Lat != 0 && lastpos.Lng != 0)
                        {
                            if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.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, "");
                        }

                        // cant use gs, cant use alt,
                        if (ch3percent > 12)
                            timeInAir++;

                        if (!gotwind)
                            dowindcalc();
                    }

                    if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
                    {

                        string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);

                        int ind = logdata.IndexOf('\0');
                        if (ind != -1)
                            logdata = logdata.Substring(0, ind);

                        try
                        {
                            while (messages.Count > 5)
                            {
                                messages.RemoveAt(0);
                            }
                            messages.Add(logdata + "\n");

                        }
                        catch { }
                        mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
                    }

                    byte[] bytearray = mavinterface.packets[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;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
                    }

                    bytearray = mavinterface.packets[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[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
                    }

                    bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];

                    if (bytearray != null)
                    {
                        var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);

                        hwvoltage = hwstatus.Vcc;
                        i2cerrors = hwstatus.I2Cerr;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
                    }

                    bytearray = mavinterface.packets[ArdupilotMega.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;

                        //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                    }

            #if MAVLINK10

                    bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
                    if (bytearray != null)
                    {
                        var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);

                        armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;

                        string oldmode = mode;

                        mode = "Unknown";

                        if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
                        {
                            if (hb.type == (byte)MAVLink.MAV_TYPE.FIXED_WING)
                            {
                                switch (hb.custom_mode)
                                {
                                    case (byte)(Common.apmmodes.MANUAL):
                                        mode = "Manual";
                                        break;
                                    case (byte)(Common.apmmodes.GUIDED):
                                        mode = "Guided";
                                        break;
                                    case (byte)(Common.apmmodes.STABILIZE):
                                        mode = "Stabilize";
                                        break;
                                    case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
                                        mode = "FBW A";
                                        break;
                                    case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
                                        mode = "FBW B";
                                        break;
                                    case (byte)(Common.apmmodes.AUTO):
                                        mode = "Auto";
                                        break;
                                    case (byte)(Common.apmmodes.RTL):
                                        mode = "RTL";
                                        break;
                                    case (byte)(Common.apmmodes.LOITER):
                                        mode = "Loiter";
                                        break;
                                    case (byte)(Common.apmmodes.CIRCLE):
                                        mode = "Circle";
                                        break;
                                    case 16:
                                        mode = "Initialising";
                                        break;
                                    default:
                                        mode = "Unknown";
                                        break;
                                }
                            }
                            else if (hb.type == (byte)MAVLink.MAV_TYPE.QUADROTOR)
                            {
                                switch (hb.custom_mode)
                                {
                                    case (byte)(Common.ac2modes.STABILIZE):
                                        mode = "Stabilize";
                                        break;
                                    case (byte)(Common.ac2modes.ACRO):
                                        mode = "Acro";
                                        break;
                                    case (byte)(Common.ac2modes.ALT_HOLD):
                                        mode = "Alt Hold";
                                        break;
                                    case (byte)(Common.ac2modes.AUTO):
                                        mode = "Auto";
                                        break;
                                    case (byte)(Common.ac2modes.GUIDED):
                                        mode = "Guided";
                                        break;
                                    case (byte)(Common.ac2modes.LOITER):
                                        mode = "Loiter";
                                        break;
                                    case (byte)(Common.ac2modes.RTL):
                                        mode = "RTL";
                                        break;
                                    case (byte)(Common.ac2modes.CIRCLE):
                                        mode = "Circle";
                                        break;
                                    case (byte)(Common.ac2modes.LAND):
                                        mode = "Land";
                                        break;
                                    default:
                                        mode = "Unknown";
                                        break;
                                }
                            }
                        }

                        if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
                        {
                            MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                        }
                    }

                    bytearray = mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
                    if (bytearray != null)
                    {
                        var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);

                        battery_voltage = sysstatus.voltage_battery;
                        battery_remaining = sysstatus.battery_remaining;
                        current = sysstatus.current_battery;

                        packetdropremote = sysstatus.drop_rate_comm;

                        //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                    }
            #else

                bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];

                if (bytearray != null)
                {
                    var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);

                    armed = sysstatus.status;

                    string oldmode = mode;

                    switch (sysstatus.mode)
                    {
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
                                    mode = "Initialising";
                                    break;
                            }
                            break;
                        case (byte)(100 + Common.ac2modes.STABILIZE):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.STABILIZE);
                            break;
                        case (byte)(100 + Common.ac2modes.ACRO):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.ACRO);
                            break;
                        case (byte)(100 + Common.ac2modes.ALT_HOLD):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.ALT_HOLD);
                            break;
                        case (byte)(100 + Common.ac2modes.AUTO):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.AUTO);
                            break;
                        case (byte)(100 + Common.ac2modes.GUIDED):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.GUIDED);
                            break;
                        case (byte)(100 + Common.ac2modes.LOITER):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.LOITER);
                            break;
                        case (byte)(100 + Common.ac2modes.RTL):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.RTL);
                            break;
                        case (byte)(100 + Common.ac2modes.CIRCLE):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.CIRCLE);
                            break;
                        case (byte)(100 + Common.ac2modes.LAND):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.LAND);
                            break;
                        case (byte)(100 + Common.ac2modes.POSITION):
                            mode = EnumTranslator.GetDisplayText(Common.ac2modes.POSITION);
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
                            mode = "Manual";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
                            mode = "Guided";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
                            mode = "Stabilize";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
                            mode = "FBW A"; // fall though  old
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)1:
                                    mode = "FBW A";
                                    break;
                                case (byte)2:
                                    mode = "FBW B";
                                    break;
                                case (byte)3:
                                    mode = "FBW C";
                                    break;
                            }
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
                            mode = "Circle";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
                                    mode = "Auto";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
                                    mode = "RTL";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
                                    mode = "Loiter";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
                                    mode = "Takeoff";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
                                    mode = "Land";
                                    break;
                            }

                            break;
                        default:
                            mode = "Unknown";
                            break;
                    }

                    battery_voltage = sysstatus.vbat;
                    battery_remaining = sysstatus.battery_remaining;

                    packetdropremote = sysstatus.packet_drop;

                    if (oldmode != mode && MainV2.speechEnable && MainV2.speechEngine != null && MainV2.getConfig("speechmodeenabled") == "True")
                    {
                        MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                }
            #endif

                    bytearray = mavinterface.packets[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 = mavinterface.packets[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 = mavinterface.packets[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[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
                    }
                    bytearray = mavinterface.packets[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-7f;
                            lng = gps.lon * 1.0e-7f;
                        }
                        //                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[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
                    }

                    bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
                    if (bytearray != null)
                    {
                        var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
                        satcount = gps.satellites_visible;
                    }

                    bytearray = mavinterface.packets[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.fixedp;
                    }

                    bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
                    if (bytearray != null)
                    {
                        var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);

                        useLocation = true;

                        //alt = loc.alt / 1000.0f;
                        lat = loc.lat / 10000000.0f;
                        lng = loc.lon / 10000000.0f;
                    }

                    bytearray = mavinterface.packets[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.getConfig("speechwaypointenabled") == "True")
                        {
                            MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                        }

                        //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
                    }

                    bytearray = mavinterface.packets[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;
                        xtrack_error = nav.xtrack_error;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
                    }

                    bytearray = mavinterface.packets[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;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
                    }

                    bytearray = mavinterface.packets[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;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
                    }

                    bytearray = mavinterface.packets[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[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                    }

                    bytearray = mavinterface.packets[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;

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                    }

                    bytearray = mavinterface.packets[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;

                        if ((DateTime.Now - lastalt).TotalSeconds >= 0.2 && oldalt != alt)
                        {
                            climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                            verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                            if (float.IsInfinity(_verticalspeed))
                                _verticalspeed = 0;
                            lastalt = DateTime.Now;
                            oldalt = alt;
                        }

                        //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
                    }

                    bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
                    if (bytearray != null)
                    {
                        var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
                        freemem = mem.freemem;
                        brklevel = mem.brkval;
                    }
                }

                //Console.WriteLine(DateTime.Now.Millisecond + " start ");
                // update form
                try
                {
                    if (bs != null)
                    {
                        //System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
                        //Console.WriteLine(DateTime.Now.Millisecond);
                        bs.DataSource = this;
                        // Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
                        bs.ResetBindings(false);
                        //Console.WriteLine(DateTime.Now.Millisecond + " done ");
                    }
                }
                catch { log.InfoFormat("CurrentState Binding error"); }
            }
        }
Example #5
0
        public void doRefresh(ref System.Windows.Forms.DataGridView dgv)
        {
            OnStateChanged(new StatusEventArgs(StatusType.none, "dataset refreshing..."));

            if (bFiltered)
                return;
            int iRow = 0, iCol = 0;
            if (dgv.Rows.Count > 0)
            {
                iRow = dgv.CurrentCell.RowIndex;
                iCol = dgv.CurrentCell.ColumnIndex;
            }
            dgv.ResetBindings();
            //dgv.DataSource=null;
            using (SQLiteConnection connection = new SQLiteConnection(connectionString))
            {
                connection.Open();
                System.Data.DataSet ds = new DataSet();
                SQLiteDataAdapter da = new SQLiteDataAdapter("SELECT * FROM licensedata", connection);
                da.Fill(ds);
                dgv.DataSource = ds.Tables[0];
            }
            if (dgv.Rows.Count > 0)
            {
                dgv.CurrentCell = dgv.Rows[iRow].Cells[iCol];
            }
            OnStateChanged(new StatusEventArgs(StatusType.none, "...dataset refreshed"));
        }
Example #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.CAMERA_FEEDBACK];

                    if (bytearray != null)
                    {
                        var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_camera_feedback_t>(6);

                        if (status.img_idx > previousPictureNumber)
                        {
                            MainV2.instance.FlightData.BeginInvoke(new System.Windows.Forms.MethodInvoker(delegate
                            {
                                photoTime = DateTime.Now; //time picture was taken

                                double CamLatDbl = status.lat * 1E-7;
                                double CamLngDbl = status.lng * 1E-7;

                                Bitmap GoodCamIcon = new Bitmap(MissionPlanner.Properties.Resources.CamIconGreen, 40, 40);
                                Bitmap BadCamIcon = new Bitmap(MissionPlanner.Properties.Resources.CamIconYellow, 40, 40);


                                PointLatLng CamMessagePoint = new PointLatLng(CamLatDbl - .00007, CamLngDbl); //offset .00007 so the icon is centered on plane's location
                                GMarkerGoogle CamPoint = new GMarkerGoogle(CamMessagePoint, GMarkerGoogleType.none);
                                //drop red (bad) icon if roll is above 35 degrees
                                if(status.roll > 35 || status.roll < -35)
                                {
                                    CamPoint = new GMarkerGoogle(CamMessagePoint, BadCamIcon);
                                }

                                //otheriwse drop green icon
                                else
                                {
                                    CamPoint = new GMarkerGoogle(CamMessagePoint, GoodCamIcon);             
                                }

                                MainV2.instance.FlightData.CamPoints.Markers.Add(CamPoint);

                                //draw the footprint for the images -- not ready yet
                                //MainV2.instance.FlightData.FootprintPolyVisible.Polygons.Add(MainV2.instance.FlightData.FootprintPolyHidden.Polygons[status.img_idx]);

                            }));

                            previousPictureNumber = status.img_idx;
                        }
                    }
                    


                    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;
                            //mwright flight time remaining updates hb.base_mode 153 is auto mode run on seperate thread to avoid deadlock

                            if (hb.base_mode.ToString() != "153")
                            {
                                boolVar = true;
                            }
                            if (hb.base_mode.ToString() == "153")
                            {   
                                
                              
                                if (boolVar == true)
                                {   boolVar = false;

                                ThreadStart updateflighttime = new ThreadStart(work.dowork);
                                Thread newthread = null;
                                if (threadCreated == false)
                                {
                                     newthread = new Thread(updateflighttime);
                                     threadCreated = true;
                                }
                               if (threadCreated == true)
                                   {
                                     if (newthread.IsAlive == false)
                                    {
                                        
                                        newthread.Start();
                                    }
                                }
                               }
                                
                                //problems with writing to seperate thread
                                                           
                            }

                            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.LASER_POSITION != sensors_enabled.LASER_POSITION && sensors_present.LASER_POSITION)
                        {
                            messageHigh = "Bad LiDAR 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 = [email protected];
                    }

                    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 = [email protected];
                    }

                    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;
                        currentwp = 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.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"); }
            }
        }