|
|
|
|
@ -378,7 +378,7 @@ namespace ArdupilotMega
@@ -378,7 +378,7 @@ namespace ArdupilotMega
|
|
|
|
|
packet[1] = (byte)data.Length; |
|
|
|
|
packet[2] = packetcount; |
|
|
|
|
packet[3] = 255; // this is always 255 - MYGCS |
|
|
|
|
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; |
|
|
|
|
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_WAYPOINTPLANNER; |
|
|
|
|
packet[5] = messageType; |
|
|
|
|
|
|
|
|
|
int i = 6; |
|
|
|
|
@ -471,7 +471,7 @@ namespace ArdupilotMega
@@ -471,7 +471,7 @@ namespace ArdupilotMega
|
|
|
|
|
|
|
|
|
|
modifyParamForDisplay(false, paramname, ref value); |
|
|
|
|
|
|
|
|
|
Array.Resize(ref temp, 16); |
|
|
|
|
Array.Resize(ref temp, 15); |
|
|
|
|
|
|
|
|
|
req.param_id = temp; |
|
|
|
|
req.param_value = (value); |
|
|
|
|
@ -582,7 +582,6 @@ namespace ArdupilotMega
@@ -582,7 +582,6 @@ namespace ArdupilotMega
|
|
|
|
|
MainV2.givecomport = false; |
|
|
|
|
throw new Exception("Timeout on read - getParamList"); |
|
|
|
|
} |
|
|
|
|
System.Windows.Forms.Application.DoEvents(); |
|
|
|
|
byte[] buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
@ -640,7 +639,7 @@ namespace ArdupilotMega
@@ -640,7 +639,7 @@ namespace ArdupilotMega
|
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BaseStream.BytesToRead); |
|
|
|
|
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead); |
|
|
|
|
} |
|
|
|
|
//stopwatch.Stop(); |
|
|
|
|
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); |
|
|
|
|
@ -709,12 +708,12 @@ namespace ArdupilotMega
@@ -709,12 +708,12 @@ namespace ArdupilotMega
|
|
|
|
|
|
|
|
|
|
public void setWPACK() |
|
|
|
|
{ |
|
|
|
|
MAVLink.__mavlink_mission_ack_t req = new MAVLink.__mavlink_mission_ack_t(); |
|
|
|
|
MAVLink.__mavlink_waypoint_ack_t req = new MAVLink.__mavlink_waypoint_ack_t(); |
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; |
|
|
|
|
req.type = 0; |
|
|
|
|
|
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_ACK, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_ACK, req); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
public bool setWPCurrent(ushort index) |
|
|
|
|
@ -722,13 +721,13 @@ namespace ArdupilotMega
@@ -722,13 +721,13 @@ namespace ArdupilotMega
|
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
byte[] buffer; |
|
|
|
|
|
|
|
|
|
__mavlink_mission_set_current_t req = new __mavlink_mission_set_current_t(); |
|
|
|
|
__mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; |
|
|
|
|
req.seq = index; |
|
|
|
|
|
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 5; |
|
|
|
|
@ -740,7 +739,7 @@ namespace ArdupilotMega
@@ -740,7 +739,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("setWPCurrent Retry " + retrys); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -752,7 +751,7 @@ namespace ArdupilotMega
@@ -752,7 +751,7 @@ namespace ArdupilotMega
|
|
|
|
|
buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_MISSION_CURRENT) |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT) |
|
|
|
|
{ |
|
|
|
|
MainV2.givecomport = false; |
|
|
|
|
return true; |
|
|
|
|
@ -761,20 +760,19 @@ namespace ArdupilotMega
@@ -761,20 +760,19 @@ namespace ArdupilotMega
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
public bool doCommand(MAV_CMD actionid) |
|
|
|
|
public bool doAction(MAV_ACTION actionid) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
byte[] buffer; |
|
|
|
|
|
|
|
|
|
__mavlink_command_long_t req = new __mavlink_command_long_t(); |
|
|
|
|
__mavlink_action_t req = new __mavlink_action_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target = sysid; |
|
|
|
|
req.target_component = compid; |
|
|
|
|
|
|
|
|
|
req.command = (ushort)actionid; |
|
|
|
|
req.action = (byte)actionid; |
|
|
|
|
|
|
|
|
|
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_ACTION, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 3; |
|
|
|
|
@ -782,7 +780,11 @@ namespace ArdupilotMega
@@ -782,7 +780,11 @@ namespace ArdupilotMega
|
|
|
|
|
int timeout = 2000; |
|
|
|
|
|
|
|
|
|
// imu calib take a little while |
|
|
|
|
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) |
|
|
|
|
if (actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_ACC || |
|
|
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_GYRO || |
|
|
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_MAG || |
|
|
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_PRESSURE || |
|
|
|
|
actionid == MAV_ACTION.MAV_ACTION_REBOOT) |
|
|
|
|
{ |
|
|
|
|
retrys = 1; |
|
|
|
|
timeout = 6000; |
|
|
|
|
@ -795,7 +797,7 @@ namespace ArdupilotMega
@@ -795,7 +797,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("doAction Retry " + retrys); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_ACTION, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -807,17 +809,9 @@ namespace ArdupilotMega
@@ -807,17 +809,9 @@ namespace ArdupilotMega
|
|
|
|
|
buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_COMMAND_ACK) |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_ACTION_ACK) |
|
|
|
|
{ |
|
|
|
|
__mavlink_command_ack_t ack = new __mavlink_command_ack_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)ack; |
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
ack = (__mavlink_command_ack_t)(temp); |
|
|
|
|
|
|
|
|
|
if (ack.result == (byte)MAV_RESULT.MAV_RESULT_ACCEPTED) |
|
|
|
|
if (buffer[7] == 1) |
|
|
|
|
{ |
|
|
|
|
MainV2.givecomport = false; |
|
|
|
|
return true; |
|
|
|
|
@ -977,13 +971,13 @@ namespace ArdupilotMega
@@ -977,13 +971,13 @@ namespace ArdupilotMega
|
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
byte[] buffer; |
|
|
|
|
|
|
|
|
|
__mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t(); |
|
|
|
|
__mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; |
|
|
|
|
|
|
|
|
|
// request list |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 6; |
|
|
|
|
@ -995,7 +989,7 @@ namespace ArdupilotMega
@@ -995,7 +989,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -1008,24 +1002,16 @@ namespace ArdupilotMega
@@ -1008,24 +1002,16 @@ namespace ArdupilotMega
|
|
|
|
|
buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT) |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT) |
|
|
|
|
{ |
|
|
|
|
__mavlink_mission_count_t count = new __mavlink_mission_count_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)count; |
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
count = (__mavlink_mission_count_t)(temp); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Console.WriteLine("wpcount: " + count.count); |
|
|
|
|
Console.WriteLine("wpcount: " + buffer[9]); |
|
|
|
|
MainV2.givecomport = false; |
|
|
|
|
return (byte)count.count; // should be ushort, but apm has limited wp count < byte |
|
|
|
|
return buffer[9]; // should be ushort, but apm has limited wp count < byte |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_MISSION_COUNT + " " + this.BaseStream.BytesToRead); |
|
|
|
|
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this.BaseStream.BytesToRead); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
@ -1040,7 +1026,7 @@ namespace ArdupilotMega
@@ -1040,7 +1026,7 @@ namespace ArdupilotMega
|
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
Locationwp loc = new Locationwp(); |
|
|
|
|
|
|
|
|
|
__mavlink_mission_request_t req = new __mavlink_mission_request_t(); |
|
|
|
|
__mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; |
|
|
|
|
@ -1050,7 +1036,7 @@ namespace ArdupilotMega
@@ -1050,7 +1036,7 @@ namespace ArdupilotMega
|
|
|
|
|
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); |
|
|
|
|
|
|
|
|
|
// request |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 5; |
|
|
|
|
@ -1062,7 +1048,7 @@ namespace ArdupilotMega
@@ -1062,7 +1048,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("getWP Retry " + retrys); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -1075,10 +1061,10 @@ namespace ArdupilotMega
@@ -1075,10 +1061,10 @@ namespace ArdupilotMega
|
|
|
|
|
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ITEM) |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT) |
|
|
|
|
{ |
|
|
|
|
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); |
|
|
|
|
__mavlink_mission_item_t wp = new __mavlink_mission_item_t(); |
|
|
|
|
__mavlink_waypoint_t wp = new __mavlink_waypoint_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)wp; |
|
|
|
|
|
|
|
|
|
@ -1086,7 +1072,7 @@ namespace ArdupilotMega
@@ -1086,7 +1072,7 @@ namespace ArdupilotMega
|
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
wp = (__mavlink_mission_item_t)(temp); |
|
|
|
|
wp = (__mavlink_waypoint_t)(temp); |
|
|
|
|
|
|
|
|
|
loc.options = (byte)(wp.frame & 0x1); |
|
|
|
|
loc.id = (byte)(wp.command); |
|
|
|
|
@ -1109,7 +1095,7 @@ namespace ArdupilotMega
@@ -1109,7 +1095,7 @@ namespace ArdupilotMega
|
|
|
|
|
case (byte)MAV_CMD.LOITER_TURNS: |
|
|
|
|
case (byte)MAV_CMD.TAKEOFF: |
|
|
|
|
case (byte)MAV_CMD.DO_SET_HOME: |
|
|
|
|
//case (byte)MAV_CMD.DO_SET_ROI: |
|
|
|
|
case (byte)MAV_CMD.DO_SET_ROI: |
|
|
|
|
loc.alt = (int)((wp.z) * 100); |
|
|
|
|
loc.lat = (int)((wp.x) * 10000000); |
|
|
|
|
loc.lng = (int)((wp.y) * 10000000); |
|
|
|
|
@ -1256,14 +1242,14 @@ namespace ArdupilotMega
@@ -1256,14 +1242,14 @@ namespace ArdupilotMega
|
|
|
|
|
public void setWPTotal(ushort wp_total) |
|
|
|
|
{ |
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
__mavlink_mission_count_t req = new __mavlink_mission_count_t(); |
|
|
|
|
__mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_COUNT |
|
|
|
|
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT_COUNT |
|
|
|
|
|
|
|
|
|
req.count = wp_total; |
|
|
|
|
|
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 3; |
|
|
|
|
@ -1275,7 +1261,7 @@ namespace ArdupilotMega
@@ -1275,7 +1261,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("setWPTotal Retry " + retrys); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -1286,24 +1272,13 @@ namespace ArdupilotMega
@@ -1286,24 +1272,13 @@ namespace ArdupilotMega
|
|
|
|
|
byte[] buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 9) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST) |
|
|
|
|
{ |
|
|
|
|
__mavlink_mission_request_t request = new __mavlink_mission_request_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)request; |
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
request = (__mavlink_mission_request_t)(temp); |
|
|
|
|
|
|
|
|
|
if (request.seq == 0) |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0) |
|
|
|
|
{ |
|
|
|
|
param["WP_TOTAL"] = (float)wp_total - 1; |
|
|
|
|
param["CMD_TOTAL"] = (float)wp_total - 1; |
|
|
|
|
MainV2.givecomport = false; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); |
|
|
|
|
@ -1322,10 +1297,10 @@ namespace ArdupilotMega
@@ -1322,10 +1297,10 @@ namespace ArdupilotMega
|
|
|
|
|
public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current) |
|
|
|
|
{ |
|
|
|
|
MainV2.givecomport = true; |
|
|
|
|
__mavlink_mission_item_t req = new __mavlink_mission_item_t(); |
|
|
|
|
__mavlink_waypoint_t req = new __mavlink_waypoint_t(); |
|
|
|
|
|
|
|
|
|
req.target_system = sysid; |
|
|
|
|
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM |
|
|
|
|
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT |
|
|
|
|
|
|
|
|
|
req.command = loc.id; |
|
|
|
|
req.param1 = loc.p1; |
|
|
|
|
@ -1402,7 +1377,7 @@ namespace ArdupilotMega
@@ -1402,7 +1377,7 @@ namespace ArdupilotMega
|
|
|
|
|
Console.WriteLine("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); |
|
|
|
|
|
|
|
|
|
// request |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req); |
|
|
|
|
|
|
|
|
|
DateTime start = DateTime.Now; |
|
|
|
|
int retrys = 6; |
|
|
|
|
@ -1414,7 +1389,7 @@ namespace ArdupilotMega
@@ -1414,7 +1389,7 @@ namespace ArdupilotMega
|
|
|
|
|
if (retrys > 0) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("setWP Retry " + retrys); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req); |
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req); |
|
|
|
|
start = DateTime.Now; |
|
|
|
|
retrys--; |
|
|
|
|
continue; |
|
|
|
|
@ -1425,28 +1400,22 @@ namespace ArdupilotMega
@@ -1425,28 +1400,22 @@ namespace ArdupilotMega
|
|
|
|
|
byte[] buffer = readPacket(); |
|
|
|
|
if (buffer.Length > 5) |
|
|
|
|
{ |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK) |
|
|
|
|
{ |
|
|
|
|
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)ans; |
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
ans = (__mavlink_mission_ack_t)(temp); |
|
|
|
|
|
|
|
|
|
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT),ans.type.ToString())); |
|
|
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK) |
|
|
|
|
{ //__mavlink_waypoint_request_t |
|
|
|
|
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST) |
|
|
|
|
else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST) |
|
|
|
|
{ |
|
|
|
|
__mavlink_mission_request_t ans = new __mavlink_mission_request_t(); |
|
|
|
|
__mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t(); |
|
|
|
|
|
|
|
|
|
object temp = (object)ans; |
|
|
|
|
|
|
|
|
|
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6); |
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6); |
|
|
|
|
|
|
|
|
|
ans = (__mavlink_mission_request_t)(temp); |
|
|
|
|
ans = (__mavlink_waypoint_request_t)(temp); |
|
|
|
|
|
|
|
|
|
if (ans.seq == (index + 1)) |
|
|
|
|
{ |
|
|
|
|
@ -1512,17 +1481,22 @@ namespace ArdupilotMega
@@ -1512,17 +1481,22 @@ namespace ArdupilotMega
|
|
|
|
|
{ |
|
|
|
|
try |
|
|
|
|
{ |
|
|
|
|
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t(); |
|
|
|
|
|
|
|
|
|
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t(); |
|
|
|
|
|
|
|
|
|
if (Common.translateMode(modein, ref mode)) |
|
|
|
|
if (Common.translateMode(modein, ref navmode, ref mode)) |
|
|
|
|
{ |
|
|
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); |
|
|
|
|
System.Threading.Thread.Sleep(10); |
|
|
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); |
|
|
|
|
System.Threading.Thread.Sleep(10); |
|
|
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); |
|
|
|
|
System.Threading.Thread.Sleep(10); |
|
|
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); } |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/// <summary> |
|
|
|
|
@ -1696,12 +1670,6 @@ namespace ArdupilotMega
@@ -1696,12 +1670,6 @@ namespace ArdupilotMega
|
|
|
|
|
crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (temp.Length > 5 && temp[1] != MAVLINK_MESSAGE_LENGTHS[temp[5]]) |
|
|
|
|
{ |
|
|
|
|
Console.WriteLine("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", temp.Length,temp[5]); |
|
|
|
|
return new byte[0]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff)) |
|
|
|
|
{ |
|
|
|
|
int packetno = 0; |
|
|
|
|
@ -1769,15 +1737,15 @@ namespace ArdupilotMega
@@ -1769,15 +1737,15 @@ namespace ArdupilotMega
|
|
|
|
|
Console.WriteLine(logdata); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (temp[5] == MAVLINK_MSG_ID_MISSION_COUNT) |
|
|
|
|
if (temp[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT) |
|
|
|
|
{ |
|
|
|
|
// clear old |
|
|
|
|
wps = new PointLatLngAlt[wps.Length]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (temp[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM) |
|
|
|
|
if (temp[5] == MAVLink.MAVLINK_MSG_ID_WAYPOINT) |
|
|
|
|
{ |
|
|
|
|
__mavlink_mission_item_t wp = new __mavlink_mission_item_t(); |
|
|
|
|
__mavlink_waypoint_t wp = new __mavlink_waypoint_t(); |
|
|
|
|
|
|
|
|
|
object structtemp = (object)wp; |
|
|
|
|
|
|
|
|
|
@ -1785,7 +1753,7 @@ namespace ArdupilotMega
@@ -1785,7 +1753,7 @@ namespace ArdupilotMega
|
|
|
|
|
|
|
|
|
|
ByteArrayToStructure(temp, ref structtemp, 6); |
|
|
|
|
|
|
|
|
|
wp = (__mavlink_mission_item_t)(structtemp); |
|
|
|
|
wp = (__mavlink_waypoint_t)(structtemp); |
|
|
|
|
|
|
|
|
|
wps[wp.seq] = new PointLatLngAlt(wp.x,wp.y,wp.z,wp.seq.ToString()); |
|
|
|
|
} |
|
|
|
|
|
Michael Oborne
13 years ago