public MAVLinkInterface()
{
// init fields
this.BaseStream = new SerialPort();
this.packetcount = 0;
this.packetspersecond = new double[0x100];
this.packetspersecondbuild = new DateTime[0x100];
this._bytesReceivedSubj = new Subject<int>();
this._bytesSentSubj = new Subject<int>();
this.WhenPacketLost = new Subject<int>();
this.WhenPacketReceived = new Subject<int>();
this.readlock = new object();
this.lastvalidpacket = DateTime.MinValue;
this.mavlinkversion = 0;
this.debugmavlink = false;
this.logreadmode = false;
this.lastlogread = DateTime.MinValue;
this.logplaybackfile = null;
this.logfile = null;
this.rawlogfile = null;
this.bps1 = 0;
this.bps2 = 0;
this.bpstime = DateTime.MinValue;
this.lastbad = new byte[2];
for (int a = 0; a < MAVlist.Length; a++ )
MAVlist[a] = new MAVState();
}
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.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;
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;
}
}
}
//TUTUP - ADAM
//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;
//TUTUP - ADAM
//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"); }
}
}