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 9465617b02
commit bd55536fa4
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">
<DependentUpon>JoystickSetup.cs</DependentUpon>
</Compile>
<Compile Include="MAVLinkTypes.cs" />
<Compile Include="MAVLinkTypesenum.cs" />
<Compile Include="MyButton.cs">
<SubType>Component</SubType>
@ -302,7 +303,7 @@
<SubType>Component</SubType>
</Compile>
<Compile Include="CurrentState.cs" />
<Compile Include="MAVLinkTypes.cs" />
<None Include="MAVLinkTypes0.9.cs" />
<Compile Include="ElevationProfile.cs">
<SubType>Form</SubType>
</Compile>

52
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -248,16 +248,11 @@ namespace ArdupilotMega
CIRCLE = 7,
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();
mode.target = MainV2.comPort.sysid;
mode.target_system = MainV2.comPort.sysid;
try
{
@ -266,35 +261,14 @@ namespace ArdupilotMega
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.apmmodes.MANUAL:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
case (int)Common.apmmodes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.apmmodes.CIRCLE:
case (int)Common.apmmodes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST1;
break;
// AUTO MODES
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:
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:
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:
navmode.nav_mode = (byte)1;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
case (int)Common.apmmodes.FLY_BY_WIRE_B:
navmode.nav_mode = (byte)2;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein) + 16;
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
@ -306,24 +280,10 @@ namespace ArdupilotMega
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.ac2modes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.ac2modes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
// AUTO MODES
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:
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:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return false;
@ -334,7 +294,7 @@ namespace ArdupilotMega
return true;
}
public static bool getFilefromNet(string url,string saveto) {
try
{

141
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -273,27 +273,22 @@ namespace ArdupilotMega
//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;
switch (sysstatus.mode)
switch (hb.custom_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
mode = "Initialising";
break;
}
case (byte)MAVLink.MAV_MODE.MAV_MODE_PREFLIGHT:
mode = "Initialising";
break;
case (byte)(100 + Common.ac2modes.STABILIZE):
mode = "Stabilize";
@ -319,69 +314,59 @@ namespace ArdupilotMega
case (byte)(100 + Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
case (byte)(16 + Common.apmmodes.MANUAL):
mode = "Manual";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
case (byte)(16 + Common.apmmodes.GUIDED):
mode = "Guided";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
case (byte)(16 + Common.apmmodes.STABILIZE):
mode = "Stabilize";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
mode = "FBW A"; // fall though old
switch (sysstatus.nav_mode)
{
case (byte)1:
mode = "FBW A";
break;
case (byte)2:
mode = "FBW B";
break;
case (byte)3:
mode = "FBW C";
break;
}
case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_B):
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";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
mode = "RTL";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
mode = "Loiter";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
mode = "Takeoff";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
mode = "Land";
break;
}
case (byte)(16 + Common.apmmodes.AUTO):
mode = "Auto";
break;
case (byte)(16 + Common.apmmodes.RTL):
mode = "RTL";
break;
case (byte)(16 + Common.apmmodes.LOITER):
mode = "Loiter";
break;
case (byte)(16 + Common.apmmodes.CIRCLE):
mode = "Circle";
break;
default:
mode = "Unknown";
break;
}
battery_voltage = sysstatus.vbat;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.packet_drop;
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{
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;
}
@ -405,18 +390,18 @@ namespace ArdupilotMega
//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;
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;
lng = gps.lon;
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
@ -424,8 +409,8 @@ namespace ArdupilotMega
gpshdop = gps.eph;
groundspeed = gps.v;
groundcourse = gps.hdg;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
@ -442,7 +427,6 @@ namespace ArdupilotMega
satcount = gps.satellites_visible;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
{
var loc = new MAVLink.__mavlink_global_position_int_t();
@ -453,35 +437,20 @@ namespace ArdupilotMega
loc = (MAVLink.__mavlink_global_position_int_t)(temp);
alt = loc.alt / 1000.0f;
//alt = loc.alt / 1000.0f;
lat = loc.lat / 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();
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();
ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();
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;

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

@ -113,18 +113,9 @@ namespace ArdupilotMega.GCSViews
//foreach (object obj in Enum.GetValues(typeof(MAVLink.MAV_ACTION)))
{
list.Add("RETURN");
list.Add("HALT");
list.Add("CONTINUE");
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");
list.Add("LOITER_UNLIM");
list.Add("RETURN_TO_LAUNCH");
list.Add("PREFLIGHT_CALIBRATION");
}
CMB_action.DataSource = list;
@ -684,7 +675,8 @@ namespace ArdupilotMega.GCSViews
try
{
((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"); }
((Button)sender).Enabled = true;
@ -784,6 +776,7 @@ namespace ArdupilotMega.GCSViews
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (int)(intalt / MainV2.cs.multiplierdist * 100); // back to m
gotohere.lat = (int)(gotolocation.Lat * 10000000);
gotohere.lng = (int)(gotolocation.Lng * 10000000);
@ -959,20 +952,7 @@ namespace ArdupilotMega.GCSViews
private void BUT_setmode_Click(object sender, EventArgs e)
{
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(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);
}
MainV2.comPort.setMode(CMB_modes.Text);
}
private void BUT_setwp_Click(object sender, EventArgs e)
@ -1007,7 +987,7 @@ namespace ArdupilotMega.GCSViews
try
{
((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"); }
((Button)sender).Enabled = true;
@ -1018,7 +998,7 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_RETURN);
MainV2.comPort.setMode("RTL");
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1029,7 +1009,7 @@ namespace ArdupilotMega.GCSViews
try
{
((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"); }
((Button)sender).Enabled = true;
@ -1505,9 +1485,10 @@ namespace ArdupilotMega.GCSViews
MessageBox.Show("Bad Lat/Long");
return;
}
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);
}
}
}

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

@ -1176,6 +1176,7 @@ namespace ArdupilotMega.GCSViews
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = (int)(float.Parse(TXT_homelat.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
@ -1287,10 +1288,10 @@ namespace ArdupilotMega.GCSViews
foreach (Locationwp temp in cmds)
{
i++;
/*if (temp.id == 0 && i != 0) // 0 and not home
if (temp.id == 0 && i != 0) // 0 and not home
break;
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)
{
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_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();
@ -776,7 +776,7 @@ namespace ArdupilotMega.GCSViews
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.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = ygyro; // pitch - yes
@ -790,15 +790,13 @@ namespace ArdupilotMega.GCSViews
//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.hdg = ((float)DATA[19][2]);
gps.lat = ((float)DATA[20][0]);
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
gps.eph = 0;
gps.epv = 0;
gps.cog = (ushort)(DATA[19][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100);
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
@ -826,7 +824,7 @@ namespace ArdupilotMega.GCSViews
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.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
@ -837,13 +835,13 @@ namespace ArdupilotMega.GCSViews
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
gps.alt = ((float)(imudata2.altitude * ft2m));
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg);
gps.lat = ((float)imudata2.latitude);
gps.lon = ((float)imudata2.longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m);
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
@ -867,7 +865,7 @@ namespace ArdupilotMega.GCSViews
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.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
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);
gps.alt = ((float)(aeroin.Model_fPosZ));
gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg);
gps.lat = ((float)aeroin.Model_fLatitude);
gps.lon = ((float)aeroin.Model_fLongitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)));
gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100);
gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7);
gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
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 yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
@ -915,7 +913,7 @@ namespace ArdupilotMega.GCSViews
att.pitch = fdm.theta;
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.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
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);
gps.alt = ((float)(fdm.altitude * ft2m));
gps.alt = ((int)(fdm.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360);
//Console.WriteLine(gps.hdg);
gps.lat = ((float)fdm.latitude * rad2deg);
gps.lon = ((float)fdm.longitude * rad2deg);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m);
gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7);
gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100);
asp.airspeed = fdm.vcas * kts2fps * ft2m;
}
@ -995,15 +992,13 @@ namespace ArdupilotMega.GCSViews
att.roll = (DATA[18][1]);
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.hdg = ((float)DATA[18][2]);
gps.lat = ((float)DATA[20][0]);
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
gps.eph = 0;
gps.epv = 0;
gps.cog = (ushort)(DATA[18][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100));
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
}
@ -1041,7 +1036,7 @@ namespace ArdupilotMega.GCSViews
{
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();
// 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();
@ -206,15 +206,15 @@ namespace ArdupilotMega.HIL
att.pitchspeed = (float)pitch_rate * deg2rad;
att.yawspeed = (float)yaw_rate * deg2rad;
gps.alt = ((float)(altitude));
gps.alt = ((int)(altitude * 1000));
gps.fix_type = 3;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ;
gps.lat = ((float)latitude);
gps.lon = ((float)longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100);
gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(latitude* 1.0e7);
gps.lon = (int)(longitude * 1.0e7);
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);
@ -228,7 +228,7 @@ namespace ArdupilotMega.HIL
if (framecount % 10 == 0)
{// 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" );
}

14
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -326,20 +326,8 @@ namespace ArdupilotMega
{
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"); }
});

176
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -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_WAYPOINTPLANNER;
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
packet[5] = messageType;
int i = 6;
@ -471,7 +471,7 @@ namespace ArdupilotMega
modifyParamForDisplay(false, paramname, ref value);
Array.Resize(ref temp, 15);
Array.Resize(ref temp, 16);
req.param_id = temp;
req.param_value = (value);
@ -582,6 +582,7 @@ namespace ArdupilotMega
MainV2.givecomport = false;
throw new Exception("Timeout on read - getParamList");
}
System.Windows.Forms.Application.DoEvents();
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
@ -639,7 +640,7 @@ namespace ArdupilotMega
}
else
{
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead);
Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BaseStream.BytesToRead);
}
//stopwatch.Stop();
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
@ -708,12 +709,12 @@ namespace ArdupilotMega
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_component = compid;
req.type = 0;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_ACK, req);
generatePacket(MAVLINK_MSG_ID_MISSION_ACK, req);
}
public bool setWPCurrent(ushort index)
@ -721,13 +722,13 @@ namespace ArdupilotMega
MainV2.givecomport = true;
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_component = compid;
req.seq = index;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req);
DateTime start = DateTime.Now;
int retrys = 5;
@ -739,7 +740,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("setWPCurrent Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_SET_CURRENT, req);
start = DateTime.Now;
retrys--;
continue;
@ -751,7 +752,7 @@ namespace ArdupilotMega
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT)
if (buffer[5] == MAVLINK_MSG_ID_MISSION_CURRENT)
{
MainV2.givecomport = false;
return true;
@ -759,20 +760,21 @@ namespace ArdupilotMega
}
}
}
public bool doAction(MAV_ACTION actionid)
public bool doCommand(MAV_CMD actionid)
{
MainV2.givecomport = true;
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.action = (byte)actionid;
req.command = (ushort)actionid;
generatePacket(MAVLINK_MSG_ID_ACTION, req);
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
DateTime start = DateTime.Now;
int retrys = 3;
@ -780,11 +782,7 @@ namespace ArdupilotMega
int timeout = 2000;
// imu calib take a little while
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)
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
{
retrys = 1;
timeout = 6000;
@ -797,7 +795,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("doAction Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_ACTION, req);
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
start = DateTime.Now;
retrys--;
continue;
@ -809,9 +807,17 @@ namespace ArdupilotMega
buffer = readPacket();
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;
return true;
@ -971,13 +977,13 @@ namespace ArdupilotMega
MainV2.givecomport = true;
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_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
@ -989,7 +995,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
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;
retrys--;
continue;
@ -1002,16 +1008,24 @@ namespace ArdupilotMega
buffer = readPacket();
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;
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
{
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;
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_component = compid;
@ -1036,7 +1050,7 @@ namespace ArdupilotMega
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
@ -1048,7 +1062,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("getWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
@ -1061,10 +1075,10 @@ namespace ArdupilotMega
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
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);
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object temp = (object)wp;
@ -1072,7 +1086,7 @@ namespace ArdupilotMega
ByteArrayToStructure(buffer, ref temp, 6);
wp = (__mavlink_waypoint_t)(temp);
wp = (__mavlink_mission_item_t)(temp);
loc.options = (byte)(wp.frame & 0x1);
loc.id = (byte)(wp.command);
@ -1095,7 +1109,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);
@ -1242,14 +1256,14 @@ namespace ArdupilotMega
public void setWPTotal(ushort wp_total)
{
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_component = compid; // MAVLINK_MSG_ID_WAYPOINT_COUNT
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_COUNT
req.count = wp_total;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
DateTime start = DateTime.Now;
int retrys = 3;
@ -1261,7 +1275,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("setWPTotal Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req);
start = DateTime.Now;
retrys--;
continue;
@ -1272,12 +1286,23 @@ namespace ArdupilotMega
byte[] buffer = readPacket();
if (buffer.Length > 9)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0)
if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
param["WP_TOTAL"] = (float)wp_total - 1;
param["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
__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["CMD_TOTAL"] = (float)wp_total - 1;
MainV2.givecomport = false;
return;
}
}
else
{
@ -1297,10 +1322,10 @@ namespace ArdupilotMega
public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current)
{
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_component = compid; // MAVLINK_MSG_ID_WAYPOINT
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
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);
// request
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
DateTime start = DateTime.Now;
int retrys = 6;
@ -1389,7 +1414,7 @@ namespace ArdupilotMega
if (retrys > 0)
{
Console.WriteLine("setWP Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
start = DateTime.Now;
retrys--;
continue;
@ -1400,22 +1425,28 @@ namespace ArdupilotMega
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
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_WAYPOINT_REQUEST)
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
{
__mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t();
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t();
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);
ans = (__mavlink_waypoint_request_t)(temp);
ans = (__mavlink_mission_request_t)(temp);
if (ans.seq == (index + 1))
{
@ -1436,7 +1467,7 @@ namespace ArdupilotMega
}
}
}
public void setMountConfigure(MAV_MOUNT_MODE mountmode,bool stabroll,bool stabpitch,bool stabyaw)
{
__mavlink_mount_configure_t req = new __mavlink_mount_configure_t();
@ -1476,27 +1507,22 @@ namespace ArdupilotMega
System.Threading.Thread.Sleep(20);
generatePacket(MAVLINK_MSG_ID_MOUNT_CONTROL, req);
}
public void setMode(string modein)
{
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 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);
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>
@ -1670,6 +1696,12 @@ 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;
@ -1737,15 +1769,15 @@ namespace ArdupilotMega
Console.WriteLine(logdata);
}
if (temp[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
if (temp[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
// clear old
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;
@ -1753,7 +1785,7 @@ namespace ArdupilotMega
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());
}

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();
htb.type = (byte)MAVLink.MAV_TYPE.MAV_GENERIC;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT_TYPE.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 2;
htb.type = (byte)MAVLink.MAV_TYPE.MAV_TYPE_GCS;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 3;
comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_HEARTBEAT, htb);
heatbeatsend = DateTime.Now;
@ -1622,7 +1622,8 @@ namespace ArdupilotMega
}
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");
return true;
}

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

@ -1029,7 +1029,8 @@ namespace ArdupilotMega.Setup
{
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";
}

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

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

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

@ -22,10 +22,15 @@
</ATT>
<NTUN>
<F1>WP Dist</F1>
<F2>WP Verify</F2>
<F3>Target Bear</F3>
<F4>Long Err</F4>
<F5>Lat Err</F5>
<F2>Target Bear</F2>
<F3>Long Err</F3>
<F4>Lat Err</F4>
<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>
<CTUN>
<F1>Yaw Sensor</F1>

13
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -22,10 +22,15 @@
</ATT>
<NTUN>
<F1>WP Dist</F1>
<F2>WP Verify</F2>
<F3>Target Bear</F3>
<F4>Long Err</F4>
<F5>Lat Err</F5>
<F2>Target Bear</F2>
<F3>Long Err</F3>
<F4>Lat Err</F4>
<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>
<CTUN>
<F1>Yaw Sensor</F1>

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