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 } MAVLink.MAVLinkMessage mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED); if (mavLinkMessage != null) // hil mavlink 0.9 { var hil = mavLinkMessage.ToStructure<MAVLink.mavlink_rc_channels_scaled_t>(); 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.clearPacket((uint) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION); if (mavLinkMessage != null) { var version = mavLinkMessage.ToStructure<MAVLink.mavlink_autopilot_version_t>(); //#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); capabilities = (MAVLink.MAV_PROTOCOL_CAPABILITY)version.capabilities; MAV.clearPacket((uint)MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.FENCE_STATUS); if (mavLinkMessage != null) { var fence = mavLinkMessage.ToStructure<MAVLink.mavlink_fence_status_t>(); if (fence.breach_status != (byte) MAVLink.FENCE_BREACH.NONE) { // fence breached messageHigh = "Fence Breach"; messageHighTime = DateTime.Now; } MAV.clearPacket((uint)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.HIL_CONTROLS); if (mavLinkMessage != null) // hil mavlink 0.9 and 1.0 { var hil = mavLinkMessage.ToStructure<MAVLink.mavlink_hil_controls_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.OPTICAL_FLOW); if (mavLinkMessage != null) { var optflow = mavLinkMessage.ToStructure<MAVLink.mavlink_optical_flow_t>(); 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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.MOUNT_STATUS); if (mavLinkMessage != null) { var status = mavLinkMessage.ToStructure<MAVLink.mavlink_mount_status_t>(); campointa = status.pointing_a/100.0f; campointb = status.pointing_b/100.0f; campointc = status.pointing_c/100.0f; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.VIBRATION); if (mavLinkMessage != null) { var vibe = mavLinkMessage.ToStructure<MAVLink.mavlink_vibration_t>(); vibeclip0 = vibe.clipping_0; vibeclip1 = vibe.clipping_1; vibeclip2 = vibe.clipping_2; vibex = vibe.vibration_x; vibey = vibe.vibration_y; vibez = vibe.vibration_z; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.AIRSPEED_AUTOCAL); if (mavLinkMessage != null) { var asac = mavLinkMessage.ToStructure<MAVLink.mavlink_airspeed_autocal_t>(); asratio = asac.ratio; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME); if (mavLinkMessage != null) { var systime = mavLinkMessage.ToStructure<MAVLink.mavlink_system_time_t>(); DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); try { date1 = date1.AddMilliseconds(systime.time_unix_usec/1000); gpstime = date1; } catch { } } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.HWSTATUS); if (mavLinkMessage != null) { var hwstatus = mavLinkMessage.ToStructure<MAVLink.mavlink_hwstatus_t>(); hwvoltage = hwstatus.Vcc/1000.0f; i2cerrors = hwstatus.I2Cerr; //MAVLink.packets[(byte)MAVLink.MSG_NAMES.HWSTATUS); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.EKF_STATUS_REPORT); if (mavLinkMessage != null) { var ekfstatusm = mavLinkMessage.ToStructure<MAVLink.mavlink_ekf_status_report_t>(); // > 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; } } } } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RANGEFINDER); if (mavLinkMessage != null) { var sonar = mavLinkMessage.ToStructure<MAVLink.mavlink_rangefinder_t>(); sonarrange = sonar.distance; sonarvoltage = sonar.voltage; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.POWER_STATUS); if (mavLinkMessage != null) { var power = mavLinkMessage.ToStructure<MAVLink.mavlink_power_status_t>(); boardvoltage = power.Vcc; servovoltage = power.Vservo; voltageflag = (MAVLink.MAV_POWER_STATUS) power.flags; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.WIND); if (mavLinkMessage != null) { var wind = mavLinkMessage.ToStructure<MAVLink.mavlink_wind_t>(); gotwind = true; wind_dir = (wind.direction + 360)%360; wind_vel = wind.speed*multiplierspeed; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.HEARTBEAT); if (mavLinkMessage != null) { var hb = mavLinkMessage.ToStructure<MAVLink.mavlink_heartbeat_t>(); 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; // saftey switch if (armed && sensors_enabled.motor_control == false && sensors_enabled.seen) { messageHigh = "(SAFE)"; messageHighTime = DateTime.Now; } // 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 && Settings.Instance.GetBoolean("speechmodeenabled")) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechmode"])); } } } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SYS_STATUS); if (mavLinkMessage != null) { var sysstatus = mavLinkMessage.ToStructure<MAVLink.mavlink_sys_status_t>(); 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; sensors_enabled.Value = sysstatus.onboard_control_sensors_enabled; sensors_health.Value = sysstatus.onboard_control_sensors_health; sensors_present.Value = 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) { bool reporterror = true; if (Settings.Instance["norcreceiver"] != null) reporterror = !bool.Parse(Settings.Instance["norcreceiver"]); if (reporterror) { messageHigh = Strings.NORCReceiver; messageHighTime = DateTime.Now; } } MAV.clearPacket((uint)MAVLink.MAVLINK_MSG_ID.SYS_STATUS); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.BATTERY2); if (mavLinkMessage != null) { var bat = mavLinkMessage.ToStructure<MAVLink.mavlink_battery2_t>(); _battery_voltage2 = bat.voltage / 1000.0f; current2 = bat.current_battery / 100.0f; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SCALED_PRESSURE); if (mavLinkMessage != null) { var pres = mavLinkMessage.ToStructure<MAVLink.mavlink_scaled_pressure_t>(); press_abs = pres.press_abs; press_temp = pres.temperature; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT); if (mavLinkMessage != null) { var terrainrep = mavLinkMessage.ToStructure<MAVLink.mavlink_terrain_report_t>(); ter_curalt = terrainrep.current_height; ter_alt = terrainrep.terrain_height; ter_load = terrainrep.loaded; ter_pend = terrainrep.pending; ter_space = terrainrep.spacing; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SENSOR_OFFSETS); if (mavLinkMessage != null) { var sensofs = mavLinkMessage.ToStructure<MAVLink.mavlink_sensor_offsets_t>(); 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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.ATTITUDE); if (mavLinkMessage != null) { var att = mavLinkMessage.ToStructure<MAVLink.mavlink_attitude_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT); if (mavLinkMessage != null) { var loc = mavLinkMessage.ToStructure<MAVLink.mavlink_global_position_int_t>(); // 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; } } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT); if (mavLinkMessage != null) { var gps = mavLinkMessage.ToStructure<MAVLink.mavlink_gps_raw_int_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.GPS2_RAW); if (mavLinkMessage != null) { var gps = mavLinkMessage.ToStructure<MAVLink.mavlink_gps2_raw_t>(); 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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.GPS_STATUS); if (mavLinkMessage != null) { var gps = mavLinkMessage.ToStructure<MAVLink.mavlink_gps_status_t>(); satcount = gps.satellites_visible; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RADIO); if (mavLinkMessage != null) { var radio = mavLinkMessage.ToStructure<MAVLink.mavlink_radio_t>(); rssi = radio.rssi; remrssi = radio.remrssi; txbuffer = radio.txbuf; rxerrors = radio.rxerrors; noise = radio.noise; remnoise = radio.remnoise; fixedp = radio.@fixed; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RADIO_STATUS); if (mavLinkMessage != null) { var radio = mavLinkMessage.ToStructure<MAVLink.mavlink_radio_status_t>(); rssi = radio.rssi; remrssi = radio.remrssi; txbuffer = radio.txbuf; rxerrors = radio.rxerrors; noise = radio.noise; remnoise = radio.remnoise; fixedp = radio.@fixed; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT); if (mavLinkMessage != null) { var wpcur = mavLinkMessage.ToStructure<MAVLink.mavlink_mission_current_t>(); 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 && Settings.Instance.GetBoolean("speechwaypointenabled")) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechwaypoint"])); } //MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT); if (mavLinkMessage != null) { var nav = mavLinkMessage.ToStructure<MAVLink.mavlink_nav_controller_output_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RPM); if (mavLinkMessage != null) { var rpm = mavLinkMessage.ToStructure<MAVLink.mavlink_rpm_t>(); rpm1 = rpm.rpm1; rpm2 = rpm.rpm2; //MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_RAW); if (mavLinkMessage != null) { var rcin = mavLinkMessage.ToStructure<MAVLink.mavlink_rc_channels_raw_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS); if (mavLinkMessage != null) { var rcin = mavLinkMessage.ToStructure<MAVLink.mavlink_rc_channels_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW); if (mavLinkMessage != null) { var servoout = mavLinkMessage.ToStructure<MAVLink.mavlink_servo_output_raw_t>(); 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.clearPacket((uint)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.RAW_IMU); if (mavLinkMessage != null) { var imu = mavLinkMessage.ToStructure<MAVLink.mavlink_raw_imu_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SCALED_IMU); if (mavLinkMessage != null) { var imu = mavLinkMessage.ToStructure<MAVLink.mavlink_scaled_imu_t>(); 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); } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SCALED_IMU2); if (mavLinkMessage != null) { var imu2 = mavLinkMessage.ToStructure<MAVLink.mavlink_scaled_imu2_t>(); 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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.SCALED_IMU3); if (mavLinkMessage != null) { var imu3 = mavLinkMessage.ToStructure<MAVLink.mavlink_scaled_imu3_t>(); 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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.PID_TUNING); if (mavLinkMessage != null) { var pid = mavLinkMessage.ToStructure<MAVLink.mavlink_pid_tuning_t>(); //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; } mavLinkMessage = MAV.getPacket((uint) MAVLink.MAVLINK_MSG_ID.VFR_HUD); if (mavLinkMessage != null) { var vfr = mavLinkMessage.ToStructure<MAVLink.mavlink_vfr_hud_t>(); groundspeed = vfr.groundspeed; airspeed = vfr.airspeed; //alt = vfr.alt; // this might include baro ch3percent = vfr.throttle; if (sensors_present.revthrottle && sensors_enabled.revthrottle && sensors_health.revthrottle) if (ch3percent > 0) ch3percent *= -1; //Console.WriteLine(alt); //climbrate = vfr.climb; // heading = vfr.heading; //MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD); } mavLinkMessage = MAV.getPacket((uint)MAVLink.MAVLINK_MSG_ID.MEMINFO); if (mavLinkMessage != null) { var mem = mavLinkMessage.ToStructure<MAVLink.mavlink_meminfo_t>(); 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"); } } }