public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool forceUpdate, MAVLinkInterface mavInterface)
{
//throw new NotImplementedException();
lock (lockObject)
{
if (DateTime.Now > lastUpdate.AddMilliseconds(50) || forceUpdate) // 20 hz
{
lastUpdate = DateTime.Now;
if ((DateTime.Now - mavInterface.lastvalidpacket).TotalSeconds > 10)
{
gcsLinkQuality = 0;
}
else
{
gcsLinkQuality = (ushort)((mavInterface.packetsnotlost / (mavInterface.packetsnotlost + mavInterface.packetslost)) * 100);
}
}
byte[] bytearray;
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 date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
date1 = date1.AddMilliseconds(systime.time_unix_usec / 1000);
gpsTime = date1;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HWSTATUS];
if (bytearray != null)
{
var hwStatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_hwstatus_t>(6);
boardVoltage = hwStatus.Vcc / 1000.0f;
i2cErrors = hwStatus.I2Cerr;
}
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)
{
// This should not happen
}
else
{
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
landed = hb.system_status == (byte)MAVLink.MAV_STATE.STANDBY;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
if (mode != hb.custom_mode)
{
mode = hb.custom_mode;
modeList.TryGetValue((int)hb.custom_mode, out flightMode);
}
}
}
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6);
systemLoad = (float)sysstatus.load / 10.0f;
batteryVoltage = (float)sysstatus.voltage_battery / 1000.0f;
batteryCurrent = (float)sysstatus.current_battery / 100.0f;
batteryRemaining = sysstatus.battery_remaining;
packetDropsRemote = sysstatus.drop_rate_comm;
MavlinkSensors sensorsEnabled = new MavlinkSensors(sysstatus.onboard_control_sensors_enabled);
MavlinkSensors sensorsHealth = new MavlinkSensors(sysstatus.onboard_control_sensors_health);
MavlinkSensors sensorsPresent = new MavlinkSensors(sysstatus.onboard_control_sensors_present);
if (sensorsHealth.gps != sensorsEnabled.gps)
{
checkAlertList("Bad GPS Health");
}
else if (sensorsHealth.gyro != sensorsEnabled.gyro)
{
checkAlertList("Bad Gyro Health");
}
else if (sensorsHealth.accelerometer != sensorsEnabled.accelerometer)
{
checkAlertList("Bad Accel Health");
}
else if (sensorsHealth.compass != sensorsEnabled.compass)
{
checkAlertList("Bad Compass Health");
}
else if (sensorsHealth.barometer != sensorsEnabled.barometer)
{
checkAlertList("Bad Baro Health");
}
else if (sensorsHealth.optical_flow != sensorsEnabled.optical_flow)
{
checkAlertList("Bad OptFlow Health");
}
else if (sensorsPresent.rc_receiver != sensorsEnabled.rc_receiver)
{
//checkAlertList("NO RC Receiver");
}
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);
pressureAbs = pres.press_abs;
temperature = pres.temperature;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure <MAVLink.mavlink_attitude_t>(6);
attitudeRoll = att.roll * rad2deg;
attitudePitch = att.pitch * rad2deg;
attitudeYaw = att.yaw * rad2deg;
}
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);
gpsStatus = gps.fix_type;
gpsHdop = (float)Math.Round((double)gps.eph / 100.0, 2);
gpsSatCount = gps.satellites_visible;
gpsGroundSpeed = gps.vel / 100.0f;
gpsGroundCourse = gps.cog / 100.0f;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure <MAVLink.mavlink_gps_status_t>(6);
gpsSatCount = 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);
radioRssi = radio.rssi;
radioRemoteRssi = radio.remrssi;
radioTxBuffer = radio.txbuf;
radioRxErrors = radio.rxerrors;
radioNoise = radio.noise;
radioRemoteNoise = radio.remnoise;
radioFixedPackages = radio.@fixed;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure <MAVLink.mavlink_radio_t>(6);
radioRssi = radio.rssi;
radioRemoteRssi = radio.remrssi;
radioTxBuffer = radio.txbuf;
radioRxErrors = radio.rxerrors;
radioNoise = radio.noise;
radioRemoteNoise = radio.remnoise;
radioFixedPackages = radio.@fixed;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>(6);
vfrAirspeed = vfr.airspeed;
vfrGroundspeeed = vfr.groundspeed;
vfrAltitude = vfr.alt;
vfrClimbRate = vfr.climb;
vfrHeading = vfr.heading;
vfrThrottle = vfr.throttle;
}
}
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool forceUpdate, MAVLinkInterface mavInterface)
{
//throw new NotImplementedException();
lock (lockObject)
{
if (DateTime.Now > lastUpdate.AddMilliseconds(50) || forceUpdate) // 20 hz
{
lastUpdate = DateTime.Now;
if ((DateTime.Now - mavInterface.lastvalidpacket).TotalSeconds > 10)
{
gcsLinkQuality = 0;
}
else
{
gcsLinkQuality = (ushort)((mavInterface.packetsnotlost / (mavInterface.packetsnotlost + mavInterface.packetslost)) * 100);
}
}
byte[] bytearray;
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 date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
date1 = date1.AddMilliseconds(systime.time_unix_usec / 1000);
gpsTime = date1;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HWSTATUS];
if (bytearray != null)
{
var hwStatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
boardVoltage = hwStatus.Vcc / 1000.0f;
i2cErrors = hwStatus.I2Cerr;
}
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)
{
// This should not happen
}
else
{
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
landed = hb.system_status == (byte)MAVLink.MAV_STATE.STANDBY;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
if (mode != hb.custom_mode)
{
mode = hb.custom_mode;
modeList.TryGetValue((int)hb.custom_mode, out flightMode);
}
}
}
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
systemLoad = (float)sysstatus.load / 10.0f;
batteryVoltage = (float)sysstatus.voltage_battery / 1000.0f;
batteryCurrent = (float)sysstatus.current_battery / 100.0f;
batteryRemaining = sysstatus.battery_remaining;
packetDropsRemote = sysstatus.drop_rate_comm;
MavlinkSensors sensorsEnabled = new MavlinkSensors(sysstatus.onboard_control_sensors_enabled);
MavlinkSensors sensorsHealth = new MavlinkSensors(sysstatus.onboard_control_sensors_health);
MavlinkSensors sensorsPresent = new MavlinkSensors(sysstatus.onboard_control_sensors_present);
if (sensorsHealth.gps != sensorsEnabled.gps)
{
checkAlertList("Bad GPS Health");
}
else if (sensorsHealth.gyro != sensorsEnabled.gyro)
{
checkAlertList("Bad Gyro Health");
}
else if (sensorsHealth.accelerometer != sensorsEnabled.accelerometer)
{
checkAlertList("Bad Accel Health");
}
else if (sensorsHealth.compass != sensorsEnabled.compass)
{
checkAlertList("Bad Compass Health");
}
else if (sensorsHealth.barometer != sensorsEnabled.barometer)
{
checkAlertList("Bad Baro Health");
}
else if (sensorsHealth.optical_flow != sensorsEnabled.optical_flow)
{
checkAlertList("Bad OptFlow Health");
}
else if (sensorsPresent.rc_receiver != sensorsEnabled.rc_receiver)
{
//checkAlertList("NO RC Receiver");
}
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);
pressureAbs = pres.press_abs;
temperature = pres.temperature;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
attitudeRoll = att.roll * rad2deg;
attitudePitch = att.pitch * rad2deg;
attitudeYaw = att.yaw * rad2deg;
}
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);
gpsStatus = gps.fix_type;
gpsHdop = (float)Math.Round((double)gps.eph / 100.0, 2);
gpsSatCount = gps.satellites_visible;
gpsGroundSpeed = gps.vel / 100.0f;
gpsGroundCourse = gps.cog / 100.0f;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
gpsSatCount = 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);
radioRssi = radio.rssi;
radioRemoteRssi = radio.remrssi;
radioTxBuffer = radio.txbuf;
radioRxErrors = radio.rxerrors;
radioNoise = radio.noise;
radioRemoteNoise = radio.remnoise;
radioFixedPackages = radio.@fixed;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
radioRssi = radio.rssi;
radioRemoteRssi = radio.remrssi;
radioTxBuffer = radio.txbuf;
radioRxErrors = radio.rxerrors;
radioNoise = radio.noise;
radioRemoteNoise = radio.remnoise;
radioFixedPackages = radio.@fixed;
}
bytearray = mavInterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
vfrAirspeed = vfr.airspeed;
vfrGroundspeeed = vfr.groundspeed;
vfrAltitude = vfr.alt;
vfrClimbRate = vfr.climb;
vfrHeading = vfr.heading;
vfrThrottle = vfr.throttle;
}
}
}