/*
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"); }
}
}
private static void LoadChildren(System.Windows.Forms.BindingSource parentBS, System.Windows.Forms.BindingSource childBS)
{
if (!parentBS.get_IsBindingSuspended())
{
LocalAddress parent = parentBS.get_Current() as LocalAddress;
if (parent != null)
{
childBS.SuspendBinding();
if (parent == LocalAddress.Null)
{
childBS.Clear();
childBS.Add(LocalAddress.Null);
}
else
{
ObjectList<LocalAddress> list = new ObjectList<LocalAddress>();
ObjectList<LocalAddress> children = Mappers.LocalAddressMapper.GetChildren(parent);
if (children.get_Count() == 1)
{
list.AddRange(children);
list.Add(LocalAddress.Null);
}
else
{
list.Add(LocalAddress.Null);
list.AddRange(children);
}
childBS.set_DataSource(list);
}
int index = childBS.IndexOf(LocalAddress.Null);
childBS.ResumeBinding();
childBS.set_Position(((childBS.get_Count() == 2) && !((LocalAddress) childBS.get_Item(0)).Name.Equals(string.Empty)) ? ((int) 0) : index);
}
}
}
private static void LoadChildren(System.Windows.Forms.BindingSource parentBS, System.Windows.Forms.BindingSource childBS)
{
if (!parentBS.get_IsBindingSuspended())
{
LocalAddress parent = parentBS.get_Current() as LocalAddress;
if (parent != null)
{
childBS.SuspendBinding();
if (parent == LocalAddress.Null)
{
childBS.Clear();
}
else
{
childBS.set_DataSource(House.GetChildrenAddress(parent));
}
int num = childBS.Add(LocalAddress.Null);
childBS.ResumeBinding();
childBS.set_Position((childBS.get_Count() == 2) ? ((int) 0) : num);
}
}
}
private static void LoadChildren(System.Windows.Forms.BindingSource parentBS, System.Windows.Forms.BindingSource childBS)
{
if (!parentBS.get_IsBindingSuspended())
{
LocalAddress parent = parentBS.get_Current() as LocalAddress;
if (parent != null)
{
childBS.SuspendBinding();
if (parent == LocalAddress.Null)
{
childBS.Clear();
}
else
{
childBS.set_DataSource(Mappers.LocalAddressMapper.GetChildren(parent));
}
int num = childBS.Add(LocalAddress.Null);
childBS.ResumeBinding();
childBS.set_Position(((childBS.get_Count() == 2) && !((LocalAddress) childBS.get_Item(0)).Name.Equals(string.Empty)) ? ((int) 0) : num);
}
}
}