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