public void sendParamUpdate(string parameterName, float parameterValue, MAVLink.MAV_PARAM_TYPE type)
{
MAVLink.mavlink_param_set_t paramset = new MAVLink.mavlink_param_set_t();
paramset.target_component = (byte)this.componentId;
paramset.target_system = (byte)this.systemId;
paramset.param_value = parameterValue;
// have to set fixed 16 byte value for param_id
byte[] byteArray = new byte[16];
for (int i = 0; i < 16; i++)
{
if (i < parameterName.Length)
{
byteArray[i] = (byte)parameterName[i];
}
else
{
byteArray[i] = (byte)'\0';
}
}
paramset.param_id = byteArray;
paramset.param_type = (byte)type;
byte[] packet = this.mavlinkParse.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.PARAM_SET, paramset);
this.port.Write(packet, 0, packet.Length);
}
private MAVLink.mavlink_param_set_t mavlinkSingleParameter()
{
MAVLink.mavlink_param_set_t param = new MAVLink.mavlink_param_set_t();
param.target_system = sysid;
param.target_component = compid;
form1.SetIdString(ref param.param_id, singleCommand);
param.param_type = (byte)MAVLink.MAV_PARAM_TYPE.REAL32;
param.param_value = mavlinkSingleValue;
return(param);
}
/// <summary>
/// Sends frequency to the Pi fro processing.
/// </summary>
/// <param name="frequency"></param>
/// <returns></returns>
public static bool SendMavLinkFrequency(float frequency)
{
vhf_freq = frequency;
//Struct for sending collar frequency to RDF system
//Has to be exactly 16 bytes
byte[] paramid = new byte[16];
paramid[0] = (byte)'V';
paramid[1] = (byte)'H';
paramid[2] = (byte)'F';
paramid[3] = (byte)'_';
paramid[4] = (byte)'F';
paramid[5] = (byte)'R';
paramid[6] = (byte)'E';
paramid[7] = (byte)'Q';
paramid[8] = (byte)'\0';
MAVLink.mavlink_param_set_t setFreq = new MAVLink.mavlink_param_set_t();
setFreq.target_system = (byte)system_id; //Drone
setFreq.target_component = (byte)comp_id; //Pi
setFreq.param_id = paramid;
setFreq.param_value = frequency * 1000000;
setFreq.param_type = (byte)MAVLink.MAV_PARAM_TYPE.REAL32;
MavLinkCom.sendPacket(setFreq, MavLinkCom.sysidcurrent, MavLinkCom.compidcurrent);
CommandTimeoutTimer.Interval = 1000;
CommandTimeoutTimer.Enabled = true;
while (!vhf_freq_state_changed &&
!command_timeout)
{
;
}
bool retVal;
if (vhf_freq_state_changed)
{
retVal = true;
}
else
{
retVal = false;
}
CommandTimeoutTimer.Enabled = false;
command_timeout = false;
vhf_freq_state_changed = false;
return(retVal);
}
public static bool SendMavLinkLNAGain(int gain)
{
lna_gain = gain;
//Setting LNA Gain ID
byte[] paramid = new byte[16];
paramid[0] = (byte)'L';
paramid[1] = (byte)'N';
paramid[2] = (byte)'A';
paramid[3] = (byte)'_';
paramid[4] = (byte)'G';
paramid[5] = (byte)'A';
paramid[6] = (byte)'I';
paramid[7] = (byte)'N';
paramid[8] = (byte)'\0';
MAVLink.mavlink_param_set_t setLNAGain = new MAVLink.mavlink_param_set_t();
setLNAGain.target_system = (byte)system_id; //Drone
setLNAGain.target_component = (byte)comp_id; //Pi
setLNAGain.param_id = paramid;
setLNAGain.param_value = gain;
setLNAGain.param_type = (byte)MAVLink.MAV_PARAM_TYPE.INT32;
MavLinkCom.sendPacket(setLNAGain, MavLinkCom.sysidcurrent, MavLinkCom.compidcurrent);
CommandTimeoutTimer.Interval = 1000;
CommandTimeoutTimer.Enabled = true;
while (!lna_gain_state_changed &&
!command_timeout)
{
;
}
bool retVal;
if (lna_gain_state_changed)
{
retVal = true;
}
else
{
retVal = false;
}
CommandTimeoutTimer.Enabled = false;
command_timeout = false;
lna_gain_state_changed = false;
return(retVal);
}
public static void WriteParameter(string name, double val)
{
byte[] paramID;
MAVLink.mavlink_param_set_t req = new MAVLink.mavlink_param_set_t();
req.target_system = 0;
req.target_component = 0;
paramID = ASCIIEncoding.ASCII.GetBytes(name + "\0");
// I'm not sure why this is needed, but it is
Array.Resize(ref paramID, 16);
req.param_id = paramID;
req.param_value = (float)val;
byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.PARAM_SET, req);
SendPacket(bytes);
}
private MAVLink.mavlink_param_set_t comms_WriteMavlink(object sender, CommandEventArgs e)
{
MAVLink.mavlink_param_set_t param = new MAVLink.mavlink_param_set_t();
if (!storedColors[e.Index].changed)
{
return(param);
}
param.target_system = comms.sysid;
param.target_component = comms.compid;
form1.SetIdString(ref param.param_id, "color_" + e.Index.ToString());
param.param_type = (byte)MAVLink.MAV_PARAM_TYPE.UINT32;
ColorStore color = storedColors[e.Index];
param.param_value = BitConverter.ToSingle(new byte[] {
(byte)(color.hsv.hue & 0xff),
(byte)((color.hsv.hue >> 8) & 0xff),
(byte)(color.hsv.sat),
(byte)(color.hsv.val)
}, 0);
return(param);
}
private MAVLink.mavlink_param_set_t comms_WriteMavlink(object sender, CommandEventArgs e)
{
MAVLink.mavlink_param_set_t param = new MAVLink.mavlink_param_set_t();
if (!storedModes[e.Index].isChanged())
{
return(param);
}
param.target_system = comms.sysid;
param.target_component = comms.compid;
form1.SetIdString(ref param.param_id, "mode_" + e.Index.ToString());
param.param_type = (byte)MAVLink.MAV_PARAM_TYPE.UINT32;
ModeColorStore mode = storedModes[e.Index];
param.param_value = BitConverter.ToSingle(new byte[] {
(byte)(mode.get(0) + ((mode.get(1) << 5) & 0xe0)),
(byte)(mode.get(2) + ((mode.get(1) << 2) & 0xe0)),
(byte)(mode.get(3) + ((mode.get(4) << 5) & 0xe0)),
(byte)(mode.get(5) + ((mode.get(4) << 2) & 0xe0))
}, 0);
return(param);
}
public void startslcan(byte canport)
{
but_slcanmode1.Enabled = false;
but_slcanmode2.Enabled = false;
try
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
if (CustomMessageBox.Show(
"You are not currently connected via mavlink. Please make sure the device is already in slcan mode or this is the slcan serialport.",
"SLCAN", CustomMessageBox.MessageBoxButtons.OKCancel) != CustomMessageBox.DialogResult.OK)
{
return;
}
}
if (MainV2.comPort.BaseStream.IsOpen)
{
var cport = MainV2.comPort.MAV.param["CAN_SLCAN_CPORT"].Value;
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_CPORT", canport, true);
if (cport == 0)
{
CustomMessageBox.Show("Reboot required" + " after setting CPORT. Please reboot!",
Strings.ERROR);
return;
}
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_TIMOUT", 2, true);
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_P" + canport + "_DRIVER", 1);
//MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_SERNUM", 0, true); // usb
// blind send
var paramname = "CAN_SLCAN_SERNUM";
var req = new MAVLink.mavlink_param_set_t
{
target_system = (byte)MainV2.comPort.sysidcurrent,
target_component = (byte)MainV2.comPort.compidcurrent,
param_type = (byte)MainV2.comPort
.MAVlist[(byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent]
.param_types[paramname],
param_id = paramname.MakeBytesSize(16)
};
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
}
catch
{
}
{
// grab the connected port
var port = MainV2.comPort.BaseStream;
// place an invalid port in its place
if (port != null)
{
MainV2.comPort.BaseStream = new Comms.SerialPort()
{
PortName = port.PortName, BaudRate = port.BaudRate
}
}
;
//check if we started from within mavlink - if not get settings from menu and create port
if (port == null || !port.IsOpen)
{
port = new Comms.SerialPort()
{
PortName = MainV2._connectionControl.CMB_serialport.Text,
BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text)
};
}
if (can == null)
{
can = new uavcan();
}
can.SourceNode = 127;
can.NodeAdded += (id, msg) =>
{
this.BeginInvoke((Action) delegate
{
allnodes.Add(new UAVCANModel()
{
ID = id,
Name = "?",
Health = msg.health.ToString(),
Mode = msg.mode.ToString(),
Uptime = TimeSpan.FromSeconds(msg.uptime_sec),
VSC = msg.vendor_specific_status_code
});
});
};
if (!port.IsOpen)
{
try
{
port.Open();
}
catch (Exception e)
{
CustomMessageBox.Show(Strings.CheckPortSettingsOr);
return;
}
}
if (chk_log.Checked)
{
can.LogFile = Settings.Instance.LogDir + Path.DirectorySeparatorChar +
DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".can";
}
var prd = new ProgressReporterDialogue();
prd.UpdateProgressAndStatus(-1, "Trying to connect");
prd.DoWork += sender => can.StartSLCAN(port.BaseStream);
prd.btnCancel.Click += (sender, args) =>
{
prd.doWorkArgs.CancelAcknowledged = true;
port.Close();
};
prd.RunBackgroundOperationAsync();
if (prd.doWorkArgs.CancelRequested || prd.doWorkArgs.ErrorMessage != null)
{
return;
}
can.SetupFileServer();
can.SetupDynamicNodeAllocator();
can.MessageReceived += (frame, msg, transferID) =>
{
if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_NodeStatus))
{
var ns = msg as UAVCAN.uavcan.uavcan_protocol_NodeStatus;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
if (nodes.Count() > 0 && nodes.First().Name == "?")
{
var statetracking = new UAVCAN.uavcan.statetracking();
// get node info
UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(UAVCAN.uavcan.uavcan_transmit_chunk_handler, statetracking);
var slcan = can.PackageMessage(frame.SourceNode, 30, 0, gnireq);
can.WriteToStream(slcan);
}
foreach (var item in nodes)
{
switch (ns.health)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
item.Health = "OK";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
item.Health = "WARNING";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
item.Health = "ERROR";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
item.Health = "CRITICAL";
break;
}
switch (ns.mode)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL:
item.Mode = "OPERATIONAL";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION:
item.Mode = "INITIALIZATION";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE:
item.Mode = "MAINTENANCE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:
item.Mode = "SOFTWARE_UPDATE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE:
item.Mode = "OFFLINE";
break;
}
item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec);
}
_updatePending = true;
this.BeginInvoke((Action) delegate
{
if (_updatePending)
{
_updatePending = false;
uAVCANModelBindingSource.ResetBindings(false);
}
});
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
foreach (var item in nodes)
{
item.Name = ASCIIEncoding.ASCII.GetString(gnires.name, 0, gnires.name_len);
item.HardwareVersion = gnires.hardware_version.major + "." + gnires.hardware_version.minor;
item.SoftwareVersion = gnires.software_version.major + "." + gnires.software_version.minor + "." + gnires.software_version.vcs_commit.ToString("X");
item.SoftwareCRC = gnires.software_version.image_crc;
item.HardwareUID = gnires.hardware_version.unique_id.Select(a => a.ToString("X2")).Aggregate((a, b) =>
{
return(a + " " + b);
});
item.RawMsg = gnires;
item.VSC = gnires.status.vendor_specific_status_code;
}
_updatePending = true;
this.BeginInvoke((Action) delegate
{
if (_updatePending)
{
_updatePending = false;
uAVCANModelBindingSource.ResetBindings(false);
}
});
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_debug_LogMessage))
{
var debug = msg as UAVCAN.uavcan.uavcan_protocol_debug_LogMessage;
this.BeginInvoke((Action) delegate()
{
DGDebug.Rows.Insert(0, new object[]
{
frame.SourceNode, debug.level.value,
ASCIIEncoding.ASCII.GetString(debug.source, 0, debug.source_len),
ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len)
});
if (DGDebug.Rows.Count > 100)
{
DGDebug.Rows.RemoveAt(DGDebug.Rows.Count - 1);
}
});
}
};
}
}
UAVCAN.uavcan can = new UAVCAN.uavcan();
private void LogPacket(object packet, bool ingoing, int sysId, int compId)
{
if (threadDone)
{
return;
}
if (listViewMessages.InvokeRequired)
{
try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); }
catch { };
return;
}
List <string> fields = new List <string>();
fields.Add(sysId.ToString());
fields.Add(compId.ToString());
if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked))
{
return;
}
string messageName = packet.ToString().Replace("MAVLink+mavlink_", "");
if (IsMessageFilteredOut(messageName))
{
return;
}
if (listViewMessages.IsDisposed)
{
return;
}
if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
fields.Add("GPS Raw Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
fields.Add(gps.vel.ToString());
fields.Add(gps.satellites_visible.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
fields.Add("GPS Global Position Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet;
fields.Add("Attitude");
fields.Add((att.pitch * 180.0 / Math.PI).ToString());
fields.Add((att.roll * 180.0 / Math.PI).ToString());
fields.Add((att.yaw * 180.0 / Math.PI).ToString());
fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString());
fields.Add((att.rollspeed * 180.0 / Math.PI).ToString());
fields.Add((att.yawspeed * 180.0 / Math.PI).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t))
{
MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet;
fields.Add("Scaled IMU");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t))
{
MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet;
fields.Add("Scaled IMU3");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t))
{
MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet;
fields.Add("Scaled IMU2");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t))
{
MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet;
fields.Add("System Status");
fields.Add(status.voltage_battery.ToString());
}
// else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t))
// {
// MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet;
// listViewMessages.Items.Add(new ListViewItem(new string[] {
// "Autopilot Version",
// ver.version.ToString(),
// ver.custom_version.ToString(),
// ver.capabilities.ToString()}));
// }
else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet;
fields.Add("Heartbeat");
fields.Add(hb.autopilot.ToString());
fields.Add(hb.system_status.ToString());
fields.Add(hb.mavlink_version.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t))
{
MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet;
fields.Add("Status Text");
fields.Add(ASCIIEncoding.ASCII.GetString(status.text));
fields.Add(status.severity.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
fields.Add("Param Value");
fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id));
fields.Add(paramValue.param_value.ToString());
fields.Add(paramValue.param_count.ToString());
fields.Add(paramValue.param_index.ToString());
fields.Add(paramValue.param_type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t))
{
MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet;
fields.Add("Param Request Read");
fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id));
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t))
{
MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet;
fields.Add("Param Set");
fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id));
fields.Add(paramSet.param_value.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
fields.Add("Mission Count");
fields.Add(paramValue.count.ToString());
fields.Add(paramValue.target_component.ToString());
fields.Add(paramValue.target_system.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet;
fields.Add("Mission Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet;
fields.Add("Mission Request Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t))
{
MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet;
fields.Add("Ack");
fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString());
fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t))
{
MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet;
fields.Add("Mission Ack");
fields.Add(paramValue.type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t))
{
MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet;
fields.Add("Radio Status");
fields.Add(radio.rssi.ToString());
fields.Add(radio.remrssi.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
fields.Add("EKF Status");
fields.Add(ekf.flags.ToString());
fields.Add(ekf.velocity_variance.ToString());
fields.Add(ekf.pos_horiz_variance.ToString());
fields.Add(ekf.pos_vert_variance.ToString());
fields.Add(ekf.compass_variance.ToString());
fields.Add(ekf.terrain_alt_variance.ToString());
}
else
{
fields.Add(messageName);
//Log(packet.ToString());
}
if (ingoing)
{
listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray())));
}
else
{
listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray())));
}
}
private MAVLink.mavlink_param_set_t comms_WriteMavlink(object sender, CommandEventArgs e)
{
float f;
int i;
MAVLink.mavlink_param_set_t param = new MAVLink.mavlink_param_set_t();
if (!storedParams.changed[e.Index])
{
return(param);
}
param.target_system = comms.sysid;
param.target_component = comms.compid;
form1.SetIdString(ref param.param_id, paramList[e.Index]);
param.param_type = (byte)MAVLink.MAV_PARAM_TYPE.REAL32;
if (e.Index == SYSID && int.TryParse(txtSysid.Text, out i))
{
param.param_value = i;
}
else if (e.Index == HEARTBEAT)
{
param.param_value = (chkHeartBeat.Checked ? 1 : 0);
}
else if (e.Index == BRIGHTNESS)
{
param.param_value = trkBright.Value;
}
else if (e.Index == ANIMATION)
{
param.param_value = (chkAnim.Checked ? 1 : 0);
}
else if (e.Index == LOWCELL && float.TryParse(txtLowcell.PlainText, out f))
{
param.param_value = f;
}
else if (e.Index == LOWPCT && int.TryParse(txtLowpct.PlainText, out i))
{
param.param_value = i;
}
else if (e.Index == MINSATS && int.TryParse(txtMinsats.Text, out i))
{
param.param_value = i;
}
else if (e.Index == DEADBAND && int.TryParse(txtDeadband.PlainText, out i))
{
param.param_value = i;
}
else if (e.Index == BAUD)
{
param.param_value = getBaud(cbxMavlink.SelectedIndex, BAUD);
}
else if (e.Index == SOFTBAUD)
{
param.param_value = getBaud(cbxSoftserial.SelectedIndex, SOFTBAUD);
}
else
{
param.target_system = 0;
param.target_component = 0;
}
//SYSID changed? Need to change our current sysid
if (e.Index == SYSID)
{
form1.ChangeSysid((byte)param.param_value);
}
return(param);
}
public void startslcan(byte canport)
{
but_slcanmode1.Enabled = false;
but_mavlinkcanmode2.Enabled = false;
but_mavlinkcanmode2_2.Enabled = false;
but_filter.Enabled = false;
try
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
if (CustomMessageBox.Show(
"You are not currently connected via mavlink. Please make sure the device is already in slcan mode or this is the slcan serialport.",
"SLCAN", CustomMessageBox.MessageBoxButtons.OKCancel) != CustomMessageBox.DialogResult.OK)
{
return;
}
}
if (MainV2.comPort.BaseStream.IsOpen)
{
var cport = MainV2.comPort.MAV.param["CAN_SLCAN_CPORT"].Value;
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_CPORT", canport, true);
if (cport == 0)
{
CustomMessageBox.Show("Reboot required" + " after setting CPORT. Please reboot!",
Strings.ERROR);
return;
}
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_TIMOUT", 2, true);
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_P" + canport + "_DRIVER", 1);
//MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_SERNUM", 0, true); // usb
// blind send
var paramname = "CAN_SLCAN_SERNUM";
var req = new MAVLink.mavlink_param_set_t
{
target_system = (byte)MainV2.comPort.sysidcurrent,
target_component = (byte)MainV2.comPort.compidcurrent,
param_type = (byte)MainV2.comPort
.MAVlist[(byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent]
.param_types[paramname],
param_id = paramname.MakeBytesSize(16)
};
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
}
catch
{
}
// grab the connected port
var port = MainV2.comPort.BaseStream;
// place an invalid port in its place
if (port != null)
{
MainV2.comPort.BaseStream = new Comms.SerialPort()
{
PortName = port.PortName, BaudRate = port.BaudRate
}
}
;
SetupSLCanPort(port);
}
//Timeout - retry or fail..
private void comTimer_Timeout(object sender, EventArgs e)
{
//Waiting for "<Enter>" boot message
if (waitForBoot)
{
stopTimer();
if (CurrentMode == CommMode.MAVLINK)
{
//Mavlink - Send ping
if (bootCounter == 0)
{
OnProgressChanged(commType, 0, "Discovery - Sending ping.");
}
else
{
OnProgressChanged(commType, 0, "Discovery - attempt " + bootCounter.ToString() + ": Timeout waiting for ping, Mavpixel not responding.");
}
mavlinkSendPing(bootCounter);
}
else
{
crlfPress(); //CLI - Press enter
}
if (++bootCounter < 10)
{
startTimer(ref waitForBoot);
}
//Timed out waiting for boot message. Try anyway.
else
{
startTimer(ref waitForPrompt);
}
}
//Waiting to see if any more data will arrive after finding a single prompt
else if (checkEmpty)
{
stopTimer();
OnGotData(readCommandIndex, mavPixReply);
OnCompleted(commType, true, sentCommandIndex);
}
//Timed out waiting for a prompt
else if (waitForPrompt || sendingCommands)
{
bool sending = sendingCommands;
stopTimer();
//timed out waiting for a new prompt
if (sending)
{
int progress = (int)((float)sentCommandIndex / (float)commandsToSend * 32);
if (comAttempts <= Properties.Settings.Default.comRetries)
{
string command;
OnProgressChanged(commType, progress, "Configuration - attempt " + comAttempts.ToString() + ": Timeout sending " + dataName + ", Mavpixel not responding.");
comAttempts++;
if (CurrentMode == CommMode.CLI)
{
//CLI - try pressing enter and sending command again
crlfPress();
if (singleCommand != "")
{
command = singleCommand;
}
else
{
command = OnWriteCommand(sentCommandIndex);
}
serial.WriteLine(command);
mavPixReply = "";
}
else if (CurrentMode == CommMode.MAVLINK)
{
//Mavlink - try sending parameter again
MAVLink.mavlink_param_set_t param = OnWriteMavlink(sentCommandIndex);
byte[] p = form1.Mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.PARAM_SET, param);
serial.Write(p, p.Length);
}
startTimer(ref sendingCommands);
MessageCounter++;
}
else
{
OnProgressChanged(commType, progress, "Configuration: Timeout sending " + dataName + ", Mavpixel not responding.");
OnCompleted(commType, false, sentCommandIndex);
}
}
else //timed out waiting for initial prompt
{
if (comAttempts <= Properties.Settings.Default.comRetries)
{
// CLI / Mavlink - last resort, try waiting for CLI prompt
OnProgressChanged(commType, 0, "Configuration - attempt " + comAttempts.ToString() + ": Timeout fetching " + dataName + ", Mavpixel not responding.");
crlfPress();
mavPixReply = "";
comAttempts++;
startTimer(ref waitForPrompt);
}
else
{
OnProgressChanged(commType, 0, "Configuration: Timeout fetching " + dataName + ", Mavpixel not responding.");
OnCompleted(commType, false, 0);
}
}
}
//Timed out waiting for a reply to a command
else if (collectingReply)
{
stopTimer();
if (comAttempts <= Properties.Settings.Default.comRetries)
{
OnProgressChanged(commType, 0, "Configuration - attempt " + comAttempts.ToString() + ": Timeout fetching " + dataName + ", Lost contact with Mavpixel.");
comAttempts++;
if (CurrentMode == CommMode.MAVLINK)
{
//Mavlink - Request parameter(s) again
getAllMavlinkParams = false;
mavlinkNextParamFetch();
startTimer(ref collectingReply);
}
else
{
//CLI - Press enter and wait for prompt
crlfPress();
mavPixReply = "";
startTimer(ref waitForPrompt);
}
}
else
{
//Lost contact with Mavpixel
OnProgressChanged(commType, 0, "Configuration: Timeout fetching " + dataName + ", Lost contact with Mavpixel.");
OnCompleted(commType, false, sentCommandIndex);
}
}
}
