public MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port)
{
this.mavint = mavint;
this.port = port;
if (!mavint.BaseStream.IsOpen)
{
mavint.BaseStream.Open();
}
if (mavint.getHeartBeat().Length == 0)
{
throw new Exception("No valid heartbeats read from port");
}
if (subscription.Value != null)
mavint.UnSubscribeToPacketType(subscription);
subscription = mavint.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL, ReceviedPacket,true);
bgdata = new Thread(mainloop);
bgdata.Name = "MAVLinkSerialPort";
bgdata.IsBackground = true;
bgdata.Start();
}
/// <summary>
/// main serial reader thread
/// controls
/// serial reading
/// link quality stats
/// speech voltage - custom - alt warning - data lost
/// heartbeat packet sending
///
/// and can't fall out
/// </summary>
private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 0;
int altwarningmax = 0;
bool armedstatus = false;
string lastmessagehigh = "";
DateTime speechcustomtime = DateTime.Now;
DateTime speechbatterytime = DateTime.Now;
DateTime speechlowspeedtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
//// get home point on armed status change.
//if (armedstatus != Form1.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen)
//{
// armedstatus = Form1.comPort.MAV.cs.armed;
// // status just changed to armed
// if (Form1.comPort.MAV.cs.armed == true)
// {
// try
// {
// //Form1.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(Form1.comPort.getWP(0));
// //if (MyView.current != null && MyView.current.Name == "FlightPlanner")
// //{
// // // update home if we are on flight data tab
// // FlightPlanner.updateHome();
// //}
// }
// catch
// {
// // dont hang this loop
// this.BeginInvoke((MethodInvoker)delegate { MessageBox.Show("Failed to update home location"); });
// }
// }
//}
// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second)
{
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t()
{
type = (byte)MAVLink.MAV_TYPE.GCS,
autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID,
mavlink_version = 3,
};
foreach (var port in Form1.Comports)
{
try
{
port.sendPacket(htb);
}
catch { }
}
heatbeatSend = DateTime.Now;
}
// if not connected or busy, sleep and loop
if (!comPort.BaseStream.IsOpen || comPort.giveComport == true)
{
if (!comPort.BaseStream.IsOpen)
{
// check if other ports are still open
foreach (var port in Comports)
{
if (port.BaseStream.IsOpen)
{
Console.WriteLine("Main comport shut, swapping to other mav");
comPort = port;
break;
}
}
}
System.Threading.Thread.Sleep(100);
//continue;
}
// actualy read the packets
while (comPort.BaseStream.IsOpen && comPort.BaseStream.BytesToRead > minbytes && comPort.giveComport == false)
{
try
{
comPort.readPacket();
}
catch { }
}
// update currentstate of sysids on main port
foreach (var sysid in comPort.sysidseen)
{
try
{
comPort.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, comPort, comPort.MAVlist[sysid]);
}
catch { }
}
//C_Log.writeLog(comPort.MAV.cs.pitch + " " + comPort.MAV.cs.roll + " " + comPort.MAV.cs.yaw + "\n\r");
this.Invoke((MethodInvoker)delegate
{
//update teks
//lbl_test.Visible = true;
//lbl_test.Text = "YAW :" + comPort.MAV.cs.yaw.ToString() + "\nAltitude :" + comPort.MAV.cs.alt.ToString();
//lbl_test.Text = "YAW :" + loop1.ToString() + "\nAltitude :" + loop2.ToString();
label1.Text = comPort.MAV.cs.yaw.ToString();
label2.Text = comPort.MAV.cs.pitch.ToString();
label3.Text = comPort.MAV.cs.roll.ToString();
});
////Ngirim Data
//if (USock != null)
//{
// if (USock.isConnected())
// {
// this.Invoke(sdtc);
// }
//}
//// read the other interfaces
//foreach (var port in Comports)
//{
// // skip primary interface
// if (port == comPort)
// continue;
// if (!port.BaseStream.IsOpen)
// {
// // modify array and drop out
// Comports.Remove(port);
// break;
// }
// while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes)
// {
// try
// {
// port.readPacket();
// }
// catch { }
// }
// // update currentstate of sysids on the port
// foreach (var sysid in port.sysidseen)
// {
// try
// {
// port.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, port, port.MAVlist[sysid]);
// }
// catch { }
// }
//}
//UPDATE JOYSTICK OUTPUT FROM SERIAL
//updateProgressJoystickInputSERIAL();
//COMPARE DELAY
//JoystickDelayCalculation();
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
MessageBox.Show(e.ToString());
try
{
comPort.Close();
}
catch { }
}
}
Console.WriteLine("SerialReader Done");
SerialThreadrunner.Set();
}
public bool doMotorTest(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout, MAVLinkInterface _mavreference)
{
return _mavreference.doCommand(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
}
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"); }
}
}
/// <summary>
/// Use the default sysid
/// </summary>
/// <param name="bs"></param>
/// <param name="updatenow"></param>
/// <param name="mavinterface"></param>
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLinkInterface mavinterface)
{
UpdateCurrentSettings(bs, updatenow, mavinterface, mavinterface.MAV);
}