Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM Planner - Mavlink 1.0 - AP working

This commit is contained in:
Michael Oborne 2011-10-30 07:40:31 +08:00
parent 0707198bd5
commit ff1e262ac7
17 changed files with 2486 additions and 1108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -227,6 +227,7 @@
<Compile Include="JoystickSetup.Designer.cs"> <Compile Include="JoystickSetup.Designer.cs">
<DependentUpon>JoystickSetup.cs</DependentUpon> <DependentUpon>JoystickSetup.cs</DependentUpon>
</Compile> </Compile>
<Compile Include="MAVLinkTypes.cs" />
<Compile Include="MAVLinkTypesenum.cs" /> <Compile Include="MAVLinkTypesenum.cs" />
<Compile Include="MyButton.cs"> <Compile Include="MyButton.cs">
<SubType>Component</SubType> <SubType>Component</SubType>
@ -302,7 +303,7 @@
<SubType>Component</SubType> <SubType>Component</SubType>
</Compile> </Compile>
<Compile Include="CurrentState.cs" /> <Compile Include="CurrentState.cs" />
<Compile Include="MAVLinkTypes.cs" /> <None Include="MAVLinkTypes0.9.cs" />
<Compile Include="ElevationProfile.cs"> <Compile Include="ElevationProfile.cs">
<SubType>Form</SubType> <SubType>Form</SubType>
</Compile> </Compile>

48
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -249,15 +249,10 @@ namespace ArdupilotMega
POSITION = 8 POSITION = 8
} }
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode) public static bool translateMode(string modein, ref MAVLink.__mavlink_set_mode_t mode)
{ {
//MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t();
navmode.target = MainV2.comPort.sysid;
navmode.nav_mode = 255;
//MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t(); //MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
mode.target = MainV2.comPort.sysid; mode.target_system = MainV2.comPort.sysid;
try try
{ {
@ -266,35 +261,14 @@ namespace ArdupilotMega
switch ((int)Enum.Parse(Common.getModes(), modein)) switch ((int)Enum.Parse(Common.getModes(), modein))
{ {
case (int)Common.apmmodes.MANUAL: case (int)Common.apmmodes.MANUAL:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL; case (int)Common.apmmodes.CIRCLE:
break;
case (int)Common.apmmodes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.apmmodes.STABILIZE: case (int)Common.apmmodes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST1;
break;
// AUTO MODES
case (int)Common.apmmodes.AUTO: case (int)Common.apmmodes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.RTL: case (int)Common.apmmodes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.LOITER: case (int)Common.apmmodes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
// FBW
case (int)Common.apmmodes.FLY_BY_WIRE_A: case (int)Common.apmmodes.FLY_BY_WIRE_A:
navmode.nav_mode = (byte)1;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
case (int)Common.apmmodes.FLY_BY_WIRE_B: case (int)Common.apmmodes.FLY_BY_WIRE_B:
navmode.nav_mode = (byte)2; mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein) + 16;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break; break;
default: default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein)); MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
@ -306,24 +280,10 @@ namespace ArdupilotMega
switch ((int)Enum.Parse(Common.getModes(), modein)) switch ((int)Enum.Parse(Common.getModes(), modein))
{ {
case (int)Common.ac2modes.GUIDED: case (int)Common.ac2modes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.ac2modes.STABILIZE: case (int)Common.ac2modes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
// AUTO MODES
case (int)Common.ac2modes.AUTO: case (int)Common.ac2modes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.RTL: case (int)Common.ac2modes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.LOITER: case (int)Common.ac2modes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
default: default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein)); MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return false; return false;

125
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -273,28 +273,23 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
} }
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
{ {
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();
object temp = sysstatus; object temp = hb;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);
string oldmode = mode; string oldmode = mode;
switch (sysstatus.mode) switch (hb.custom_mode)
{ {
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT: case (byte)MAVLink.MAV_MODE.MAV_MODE_PREFLIGHT:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
mode = "Initialising"; mode = "Initialising";
break; break;
}
break;
case (byte)(100 + Common.ac2modes.STABILIZE): case (byte)(100 + Common.ac2modes.STABILIZE):
mode = "Stabilize"; mode = "Stabilize";
break; break;
@ -319,69 +314,59 @@ namespace ArdupilotMega
case (byte)(100 + Common.ac2modes.CIRCLE): case (byte)(100 + Common.ac2modes.CIRCLE):
mode = "Circle"; mode = "Circle";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: case (byte)(16 + Common.apmmodes.MANUAL):
mode = "Manual"; mode = "Manual";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED: case (byte)(16 + Common.apmmodes.GUIDED):
mode = "Guided"; mode = "Guided";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1: case (byte)(16 + Common.apmmodes.STABILIZE):
mode = "Stabilize"; mode = "Stabilize";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2: case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A"; // fall though old
switch (sysstatus.nav_mode)
{
case (byte)1:
mode = "FBW A"; mode = "FBW A";
break; break;
case (byte)2: case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_B):
mode = "FBW B"; mode = "FBW B";
break; break;
case (byte)3: case (byte)(16 + Common.apmmodes.AUTO):
mode = "FBW C";
break;
}
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
mode = "FBW B";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
mode = "Auto"; mode = "Auto";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING: case (byte)(16 + Common.apmmodes.RTL):
mode = "RTL"; mode = "RTL";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD: case (byte)(16 + Common.apmmodes.LOITER):
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
mode = "Loiter"; mode = "Loiter";
break; break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF: case (byte)(16 + Common.apmmodes.CIRCLE):
mode = "Takeoff"; mode = "Circle";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
mode = "Land";
break;
}
break; break;
default: default:
mode = "Unknown"; mode = "Unknown";
break; break;
} }
battery_voltage = sysstatus.vbat;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.packet_drop;
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{ {
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
} }
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();
object temp = sysstatus;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
} }
@ -405,18 +390,18 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
} }
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null) if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
{ {
var gps = new MAVLink.__mavlink_gps_raw_t(); var gps = new MAVLink.__mavlink_gps_raw_int_t();
object temp = gps; object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6); MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);
gps = (MAVLink.__mavlink_gps_raw_t)(temp); gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);
lat = gps.lat; lat = gps.lat * 1.0e-7f;
lng = gps.lon; lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc // alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type; gpsstatus = gps.fix_type;
@ -424,8 +409,8 @@ namespace ArdupilotMega
gpshdop = gps.eph; gpshdop = gps.eph;
groundspeed = gps.v; groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.hdg; groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
} }
@ -442,7 +427,6 @@ namespace ArdupilotMega
satcount = gps.satellites_visible; satcount = gps.satellites_visible;
} }
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null) if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
{ {
var loc = new MAVLink.__mavlink_global_position_int_t(); var loc = new MAVLink.__mavlink_global_position_int_t();
@ -453,35 +437,20 @@ namespace ArdupilotMega
loc = (MAVLink.__mavlink_global_position_int_t)(temp); loc = (MAVLink.__mavlink_global_position_int_t)(temp);
alt = loc.alt / 1000.0f; //alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f; lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f; lng = loc.lon / 10000000.0f;
} }
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null) if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
{ {
var loc = new MAVLink.__mavlink_global_position_t(); ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();
object temp = loc;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6);
loc = (MAVLink.__mavlink_global_position_t)(temp);
alt = loc.alt;
lat = loc.lat;
lng = loc.lon;
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null)
{
ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t();
object temp = wpcur; object temp = wpcur;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6); MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);
int oldwp = (int)wpno; int oldwp = (int)wpno;

41
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -113,18 +113,9 @@ namespace ArdupilotMega.GCSViews
//foreach (object obj in Enum.GetValues(typeof(MAVLink.MAV_ACTION))) //foreach (object obj in Enum.GetValues(typeof(MAVLink.MAV_ACTION)))
{ {
list.Add("RETURN"); list.Add("LOITER_UNLIM");
list.Add("HALT"); list.Add("RETURN_TO_LAUNCH");
list.Add("CONTINUE"); list.Add("PREFLIGHT_CALIBRATION");
list.Add("SET_MANUAL");
list.Add("SET_AUTO");
list.Add("STORAGE_READ");
list.Add("STORAGE_WRITE");
list.Add("CALIBRATE_RC");
list.Add("NAVIGATE");
list.Add("LOITER");
list.Add("TAKEOFF");
list.Add("CALIBRATE_GYRO");
} }
CMB_action.DataSource = list; CMB_action.DataSource = list;
@ -684,7 +675,8 @@ namespace ArdupilotMega.GCSViews
try try
{ {
((Button)sender).Enabled = false; ((Button)sender).Enabled = false;
comPort.doAction((MAVLink.MAV_ACTION)Enum.Parse(typeof(MAVLink.MAV_ACTION), "MAV_ACTION_" + CMB_action.Text)); int fixme;
//comPort.doCommand((MAVLink.MAV_ACTION)Enum.Parse(typeof(MAVLink.MAV_ACTION), "MAV_ACTION_" + CMB_action.Text));
} }
catch { MessageBox.Show("The Command failed to execute"); } catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true; ((Button)sender).Enabled = true;
@ -784,6 +776,7 @@ namespace ArdupilotMega.GCSViews
Locationwp gotohere = new Locationwp(); Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (int)(intalt / MainV2.cs.multiplierdist * 100); // back to m gotohere.alt = (int)(intalt / MainV2.cs.multiplierdist * 100); // back to m
gotohere.lat = (int)(gotolocation.Lat * 10000000); gotohere.lat = (int)(gotolocation.Lat * 10000000);
gotohere.lng = (int)(gotolocation.Lng * 10000000); gotohere.lng = (int)(gotolocation.Lng * 10000000);
@ -959,20 +952,7 @@ namespace ArdupilotMega.GCSViews
private void BUT_setmode_Click(object sender, EventArgs e) private void BUT_setmode_Click(object sender, EventArgs e)
{ {
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t(); MainV2.comPort.setMode(CMB_modes.Text);
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
if (Common.translateMode(CMB_modes.Text, 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);
}
} }
private void BUT_setwp_Click(object sender, EventArgs e) private void BUT_setwp_Click(object sender, EventArgs e)
@ -1007,7 +987,7 @@ namespace ArdupilotMega.GCSViews
try try
{ {
((Button)sender).Enabled = false; ((Button)sender).Enabled = false;
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_AUTO); MainV2.comPort.setMode("AUTO");
} }
catch { MessageBox.Show("The Command failed to execute"); } catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true; ((Button)sender).Enabled = true;
@ -1018,7 +998,7 @@ namespace ArdupilotMega.GCSViews
try try
{ {
((Button)sender).Enabled = false; ((Button)sender).Enabled = false;
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_RETURN); MainV2.comPort.setMode("RTL");
} }
catch { MessageBox.Show("The Command failed to execute"); } catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true; ((Button)sender).Enabled = true;
@ -1029,7 +1009,7 @@ namespace ArdupilotMega.GCSViews
try try
{ {
((Button)sender).Enabled = false; ((Button)sender).Enabled = false;
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_MANUAL); MainV2.comPort.setMode("MANUAL");
} }
catch { MessageBox.Show("The Command failed to execute"); } catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true; ((Button)sender).Enabled = true;
@ -1508,6 +1488,7 @@ namespace ArdupilotMega.GCSViews
MainV2.comPort.setMountConfigure(MAVLink.MAV_MOUNT_MODE.MAV_MOUNT_MODE_GPS_POINT, true, true, true); MainV2.comPort.setMountConfigure(MAVLink.MAV_MOUNT_MODE.MAV_MOUNT_MODE_GPS_POINT, true, true, true);
MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true); MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true);
} }
} }
} }

5
Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs
View File

@ -1176,6 +1176,7 @@ namespace ArdupilotMega.GCSViews
try try
{ {
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = (int)(float.Parse(TXT_homelat.Text) * 10000000); home.lat = (int)(float.Parse(TXT_homelat.Text) * 10000000);
home.lng = (int)(float.Parse(TXT_homelng.Text) * 10000000); home.lng = (int)(float.Parse(TXT_homelng.Text) * 10000000);
home.alt = (int)(float.Parse(TXT_homealt.Text) / MainV2.cs.multiplierdist * 100); // use saved home home.alt = (int)(float.Parse(TXT_homealt.Text) / MainV2.cs.multiplierdist * 100); // use saved home
@ -1287,10 +1288,10 @@ namespace ArdupilotMega.GCSViews
foreach (Locationwp temp in cmds) foreach (Locationwp temp in cmds)
{ {
i++; i++;
/*if (temp.id == 0 && i != 0) // 0 and not home if (temp.id == 0 && i != 0) // 0 and not home
break; break;
if (temp.id == 255 && i != 0) // bad record - never loaded any WP's - but have started the board up. if (temp.id == 255 && i != 0) // bad record - never loaded any WP's - but have started the board up.
break;*/ break;
if (i + 1 >= Commands.Rows.Count) if (i + 1 >= Commands.Rows.Count)
{ {
Commands.Rows.Add(); Commands.Rows.Add();

77
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -698,7 +698,7 @@ namespace ArdupilotMega.GCSViews
ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t(); ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t(); ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -776,7 +776,7 @@ namespace ArdupilotMega.GCSViews
double head = DATA[18][2] - 90; double head = DATA[18][2] - 90;
imu.usec = ((ulong)DateTime.Now.ToBinary()); imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xgyro = xgyro; // roll - yes imu.xgyro = xgyro; // roll - yes
imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = ygyro; // pitch - yes imu.ygyro = ygyro; // pitch - yes
@ -790,15 +790,13 @@ namespace ArdupilotMega.GCSViews
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc); //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
gps.alt = ((float)(DATA[20][2] * ft2m)); gps.alt = (int)(DATA[20][2] * ft2m * 1000);
gps.fix_type = 3; gps.fix_type = 3;
gps.hdg = ((float)DATA[19][2]); gps.cog = (ushort)(DATA[19][2] * 100);
gps.lat = ((float)DATA[20][0]); gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = ((float)DATA[20][1]); gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.usec = ((ulong)0); gps.time_usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704)); gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100);
gps.eph = 0;
gps.epv = 0;
asp.airspeed = ((float)(DATA[3][6] * 0.44704)); asp.airspeed = ((float)(DATA[3][6] * 0.44704));
@ -826,7 +824,7 @@ namespace ArdupilotMega.GCSViews
chkSensor.Checked = true; chkSensor.Checked = true;
imu.usec = ((ulong)DateTime.Now.Ticks); imu.time_usec = ((ulong)DateTime.Now.Ticks);
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2)); imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293)); imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0; imu.xmag = 0;
@ -837,13 +835,13 @@ namespace ArdupilotMega.GCSViews
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293)); imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0; imu.zmag = 0;
gps.alt = ((float)(imudata2.altitude * ft2m)); gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3; gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg); gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = ((float)imudata2.latitude); gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = ((float)imudata2.longitude); gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.usec = ((ulong)DateTime.Now.Ticks); gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m); gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
//FileStream stream = File.OpenWrite("fgdata.txt"); //FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes); //stream.Write(data, 0, receviedbytes);
@ -867,7 +865,7 @@ namespace ArdupilotMega.GCSViews
att.yawspeed = (aeroin.Model_fAngVelZ); att.yawspeed = (aeroin.Model_fAngVelZ);
imu.usec = ((ulong)DateTime.Now.ToBinary()); imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes
@ -884,13 +882,13 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc); Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc);
gps.alt = ((float)(aeroin.Model_fPosZ)); gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
gps.fix_type = 3; gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg); gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100);
gps.lat = ((float)aeroin.Model_fLatitude); gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7);
gps.lon = ((float)aeroin.Model_fLongitude); gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7);
gps.usec = ((ulong)DateTime.Now.Ticks); gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX))); gps.vel = (ushort)(Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)) * 100);
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY; float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX; float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
@ -915,7 +913,7 @@ namespace ArdupilotMega.GCSViews
att.pitch = fdm.theta; att.pitch = fdm.theta;
att.yaw = fdm.psi; att.yaw = fdm.psi;
imu.usec = ((ulong)DateTime.Now.ToBinary()); imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes
@ -929,14 +927,13 @@ namespace ArdupilotMega.GCSViews
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc); //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
gps.alt = ((float)(fdm.altitude * ft2m)); gps.alt = ((int)(fdm.altitude * ft2m * 1000));
gps.fix_type = 3; gps.fix_type = 3;
gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360); gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100);
//Console.WriteLine(gps.hdg); gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7);
gps.lat = ((float)fdm.latitude * rad2deg); gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7);
gps.lon = ((float)fdm.longitude * rad2deg); gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.usec = ((ulong)DateTime.Now.Ticks); gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100);
gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m);
asp.airspeed = fdm.vcas * kts2fps * ft2m; asp.airspeed = fdm.vcas * kts2fps * ft2m;
} }
@ -995,15 +992,13 @@ namespace ArdupilotMega.GCSViews
att.roll = (DATA[18][1]); att.roll = (DATA[18][1]);
att.yaw = (DATA[19][2]); att.yaw = (DATA[19][2]);
gps.alt = ((float)(DATA[20][2] * ft2m)); gps.alt = ((int)(DATA[20][2] * ft2m * 1000));
gps.fix_type = 3; gps.fix_type = 3;
gps.hdg = ((float)DATA[18][2]); gps.cog = (ushort)(DATA[18][2] * 100);
gps.lat = ((float)DATA[20][0]); gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = ((float)DATA[20][1]); gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.usec = ((ulong)0); gps.time_usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704)); gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100));
gps.eph = 0;
gps.epv = 0;
asp.airspeed = ((float)(DATA[3][6] * 0.44704)); asp.airspeed = ((float)(DATA[3][6] * 0.44704));
} }
@ -1041,7 +1036,7 @@ namespace ArdupilotMega.GCSViews
{ {
lastgpsupdate = DateTime.Now; lastgpsupdate = DateTime.Now;
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, gps); comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT, gps);
} }
} }

18
Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs
View File

@ -193,7 +193,7 @@ namespace ArdupilotMega.HIL
update_position(); update_position();
// send to apm // send to apm
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t(); ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
@ -206,15 +206,15 @@ namespace ArdupilotMega.HIL
att.pitchspeed = (float)pitch_rate * deg2rad; att.pitchspeed = (float)pitch_rate * deg2rad;
att.yawspeed = (float)yaw_rate * deg2rad; att.yawspeed = (float)yaw_rate * deg2rad;
gps.alt = ((float)(altitude)); gps.alt = ((int)(altitude * 1000));
gps.fix_type = 3; gps.fix_type = 3;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y))); gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100);
gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ; gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100);
gps.lat = ((float)latitude); gps.lat = (int)(latitude* 1.0e7);
gps.lon = ((float)longitude); gps.lon = (int)(longitude * 1.0e7);
gps.usec = ((ulong)DateTime.Now.Ticks); gps.time_usec = ((ulong)DateTime.Now.Ticks);
asp.airspeed = gps.v; asp.airspeed = gps.vel;
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_ATTITUDE, att); MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_ATTITUDE, att);
@ -228,7 +228,7 @@ namespace ArdupilotMega.HIL
if (framecount % 10 == 0) if (framecount % 10 == 0)
{// 50 / 10 = 5 hz {// 50 / 10 = 5 hz
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, gps); MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT, gps);
//Console.WriteLine(DateTime.Now.Millisecond + " GPS" ); //Console.WriteLine(DateTime.Now.Millisecond + " GPS" );
} }

14
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -326,20 +326,8 @@ namespace ArdupilotMega
{ {
try try
{ {
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t(); MainV2.comPort.setMode(but.mode);
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
if (Common.translateMode(but.mode, 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"); } catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); }
}); });

162
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

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

1553
Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs
View File

File diff suppressed because it is too large Load Diff

1459
Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs Normal file
View File

File diff suppressed because it is too large Load Diff

9
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -1064,9 +1064,9 @@ namespace ArdupilotMega
MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t(); MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t();
htb.type = (byte)MAVLink.MAV_TYPE.MAV_GENERIC; htb.type = (byte)MAVLink.MAV_TYPE.MAV_TYPE_GCS;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT_TYPE.MAV_AUTOPILOT_ARDUPILOTMEGA; htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 2; htb.mavlink_version = 3;
comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_HEARTBEAT, htb); comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_HEARTBEAT, htb);
heatbeatsend = DateTime.Now; heatbeatsend = DateTime.Now;
@ -1622,7 +1622,8 @@ namespace ArdupilotMega
} }
if (keyData == (Keys.Control | Keys.Y)) // for ryan beall if (keyData == (Keys.Control | Keys.Y)) // for ryan beall
{ {
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE); int fixme;
//MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE"); MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE");
return true; return true;
} }

3
Tools/ArdupilotMegaPlanner/Setup/Setup.cs
View File

@ -1029,7 +1029,8 @@ namespace ArdupilotMega.Setup
{ {
try try
{ {
MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC); int fixme;
//MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC);
BUT_levelac2.Text = "Complete"; BUT_levelac2.Text = "Complete";
} }

2
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application
View File

@ -11,7 +11,7 @@
<dsig:Transform Algorithm="urn:schemas-microsoft-com:HashTransforms.Identity" /> <dsig:Transform Algorithm="urn:schemas-microsoft-com:HashTransforms.Identity" />
</dsig:Transforms> </dsig:Transforms>
<dsig:DigestMethod Algorithm="http://www.w3.org/2000/09/xmldsig#sha1" /> <dsig:DigestMethod Algorithm="http://www.w3.org/2000/09/xmldsig#sha1" />
<dsig:DigestValue>S+dMQOC9TeJyQiYvhw37LpJxZU0=</dsig:DigestValue> <dsig:DigestValue>N7WYfTP50GvYcUT/jKrADGfpBj8=</dsig:DigestValue>
</hash> </hash>
</dependentAssembly> </dependentAssembly>
</dependency> </dependency>

13
Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml
View File

@ -22,10 +22,15 @@
</ATT> </ATT>
<NTUN> <NTUN>
<F1>WP Dist</F1> <F1>WP Dist</F1>
<F2>WP Verify</F2> <F2>Target Bear</F2>
<F3>Target Bear</F3> <F3>Long Err</F3>
<F4>Long Err</F4> <F4>Lat Err</F4>
<F5>Lat Err</F5> <F5>nav lon</F5>
<F6>nav lat</F6>
<F7>nav lon I</F7>
<F8>nav lat I</F8>
<F9>Loiter Lon I</F9>
<F10>Loiter Lat I</F10>
</NTUN> </NTUN>
<CTUN> <CTUN>
<F1>Yaw Sensor</F1> <F1>Yaw Sensor</F1>

13
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -22,10 +22,15 @@
</ATT> </ATT>
<NTUN> <NTUN>
<F1>WP Dist</F1> <F1>WP Dist</F1>
<F2>WP Verify</F2> <F2>Target Bear</F2>
<F3>Target Bear</F3> <F3>Long Err</F3>
<F4>Long Err</F4> <F4>Lat Err</F4>
<F5>Lat Err</F5> <F5>nav lon</F5>
<F6>nav lat</F6>
<F7>nav lon I</F7>
<F8>nav lat I</F8>
<F9>Loiter Lon I</F9>
<F10>Loiter Lat I</F10>
</NTUN> </NTUN>
<CTUN> <CTUN>
<F1>Yaw Sensor</F1> <F1>Yaw Sensor</F1>

9
Tools/ArdupilotMegaPlanner/mavlinklist.pl
View File

@ -2,8 +2,8 @@ $dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/";
$dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/"; $dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/";
# mavlink 1.0 with old structs # mavlink 1.0 with old structs
$dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/common/"; $dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include_v1.0/common/";
$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/"; $dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/";
opendir(DIR,$dir) || die print $!; opendir(DIR,$dir) || die print $!;
@files2 = readdir(DIR); @files2 = readdir(DIR);
@ -59,9 +59,13 @@ foreach $file (@files) {
} }
if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) { if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) {
if ($line =~ /MAVLINK_MSG_ID_([0-9]+)_LEN/) {
next;
} else {
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n"; print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
$no = $2; $no = $2;
} }
}
if ($line =~ /typedef struct(.*)/) { if ($line =~ /typedef struct(.*)/) {
if ($1 =~ /__mavlink_system|param_union/) { if ($1 =~ /__mavlink_system|param_union/) {
last; last;
@ -81,6 +85,7 @@ foreach $file (@files) {
$line =~ s/typedef/public/; $line =~ s/typedef/public/;
$line =~ s/uint8_t/public byte/; $line =~ s/uint8_t/public byte/;
$line =~ s/int8_t/public byte/; $line =~ s/int8_t/public byte/;
$line =~ s/char/public byte/;
$line =~ s/^\s+float/public float/; $line =~ s/^\s+float/public float/;
$line =~ s/uint16_t/public ushort/; $line =~ s/uint16_t/public ushort/;
$line =~ s/uint32_t/public uint/; $line =~ s/uint32_t/public uint/;