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

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
James Goppert 2011-10-30 23:12:53 -04:00
parent 33e04f1d81 0b484afbbe
commit 9d278ab355
24 changed files with 1437 additions and 2500 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/Attitude.pde
View File

@ -190,17 +190,18 @@ static void calc_nav_pitch()
static void calc_nav_roll()
{
//float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
//nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
//printf("nvrl %ld err %ld psi %f gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
long psi_dot_cmd = g.pidNavRoll.kP() * bearing_error;
nav_roll = (100 * ToDeg(atan((psi_dot_cmd * g_gps->ground_speed) / 9810000.0)));
// printf("nvrl %ld err %ld psi %ld gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega;
omega = dcm.get_gyro();

8
ArduPlane/navigation.pde
View File

@ -75,15 +75,15 @@ static void calc_airspeed_errors()
}
static void calc_bearing_error()
{/*
{
if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor;
} else {
*/
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - g_gps->ground_course;
// }
}
bearing_error = wrap_180(bearing_error);
}
@ -183,7 +183,7 @@ static void update_crosstrack(void)
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
static long get_distance(struct Location *loc1, struct Location *loc2)

3
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -227,7 +227,6 @@
<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>
@ -303,7 +302,7 @@
<SubType>Component</SubType>
</Compile>
<Compile Include="CurrentState.cs" />
<None Include="MAVLinkTypes0.9.cs" />
<Compile Include="MAVLinkTypes.cs" />
<Compile Include="ElevationProfile.cs">
<SubType>Form</SubType>
</Compile>

52
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -248,11 +248,16 @@ namespace ArdupilotMega
CIRCLE = 7,
POSITION = 8
}
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_mode_t mode)
public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, 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_system = MainV2.comPort.sysid;
mode.target = MainV2.comPort.sysid;
try
{
@ -261,14 +266,35 @@ namespace ArdupilotMega
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.apmmodes.MANUAL:
case (int)Common.apmmodes.CIRCLE:
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.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:
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein) + 16;
navmode.nav_mode = (byte)2;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
@ -280,10 +306,24 @@ 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;
@ -294,7 +334,7 @@ namespace ArdupilotMega
return true;
}
public static bool getFilefromNet(string url,string saveto) {
try
{

141
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -273,22 +273,27 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();
object temp = hb;
object temp = sysstatus;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);
hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);
string oldmode = mode;
switch (hb.custom_mode)
switch (sysstatus.mode)
{
case (byte)MAVLink.MAV_MODE.MAV_MODE_PREFLIGHT:
mode = "Initialising";
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;
}
break;
case (byte)(100 + Common.ac2modes.STABILIZE):
mode = "Stabilize";
@ -314,59 +319,69 @@ namespace ArdupilotMega
case (byte)(100 + Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)(16 + Common.apmmodes.MANUAL):
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
mode = "Manual";
break;
case (byte)(16 + Common.apmmodes.GUIDED):
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
mode = "Guided";
break;
case (byte)(16 + Common.apmmodes.STABILIZE):
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
mode = "Stabilize";
break;
case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
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;
}
break;
case (byte)(16 + Common.apmmodes.FLY_BY_WIRE_B):
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
mode = "FBW B";
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";
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;
}
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;
}
@ -390,18 +405,18 @@ namespace ArdupilotMega
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null)
{
var gps = new MAVLink.__mavlink_gps_raw_int_t();
var gps = new MAVLink.__mavlink_gps_raw_t();
object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6);
gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);
gps = (MAVLink.__mavlink_gps_raw_t)(temp);
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
lat = gps.lat;
lng = gps.lon;
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
@ -409,8 +424,8 @@ namespace ArdupilotMega
gpshdop = gps.eph;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
groundspeed = gps.v;
groundcourse = gps.hdg;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
@ -427,6 +442,7 @@ 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();
@ -437,20 +453,35 @@ 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[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
{
ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();
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();
object temp = wpcur;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6);
wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);
int oldwp = (int)wpno;

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

@ -113,9 +113,18 @@ namespace ArdupilotMega.GCSViews
//foreach (object obj in Enum.GetValues(typeof(MAVLink.MAV_ACTION)))
{
list.Add("LOITER_UNLIM");
list.Add("RETURN_TO_LAUNCH");
list.Add("PREFLIGHT_CALIBRATION");
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");
}
CMB_action.DataSource = list;
@ -675,8 +684,7 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
int fixme;
//comPort.doCommand((MAVLink.MAV_ACTION)Enum.Parse(typeof(MAVLink.MAV_ACTION), "MAV_ACTION_" + CMB_action.Text));
comPort.doAction((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;
@ -776,7 +784,6 @@ 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);
@ -952,7 +959,20 @@ namespace ArdupilotMega.GCSViews
private void BUT_setmode_Click(object sender, EventArgs e)
{
MainV2.comPort.setMode(CMB_modes.Text);
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);
}
}
private void BUT_setwp_Click(object sender, EventArgs e)
@ -987,7 +1007,7 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
MainV2.comPort.setMode("AUTO");
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_AUTO);
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -998,7 +1018,7 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
MainV2.comPort.setMode("RTL");
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_RETURN);
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1009,7 +1029,7 @@ namespace ArdupilotMega.GCSViews
try
{
((Button)sender).Enabled = false;
MainV2.comPort.setMode("MANUAL");
comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_SET_MANUAL);
}
catch { MessageBox.Show("The Command failed to execute"); }
((Button)sender).Enabled = true;
@ -1485,10 +1505,9 @@ 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,7 +1176,6 @@ 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
@ -1288,10 +1287,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_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_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.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.usec = ((ulong)DateTime.Now.ToBinary());
imu.xgyro = xgyro; // roll - yes
imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = ygyro; // pitch - yes
@ -790,13 +790,15 @@ namespace ArdupilotMega.GCSViews
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
gps.alt = (int)(DATA[20][2] * ft2m * 1000);
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
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);
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;
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
@ -824,7 +826,7 @@ namespace ArdupilotMega.GCSViews
chkSensor.Checked = true;
imu.time_usec = ((ulong)DateTime.Now.Ticks);
imu.usec = ((ulong)DateTime.Now.Ticks);
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
@ -835,13 +837,13 @@ namespace ArdupilotMega.GCSViews
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.alt = ((float)(imudata2.altitude * ft2m));
gps.fix_type = 3;
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);
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);
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
@ -865,7 +867,7 @@ namespace ArdupilotMega.GCSViews
att.yawspeed = (aeroin.Model_fAngVelZ);
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.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
@ -882,13 +884,13 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc);
gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
gps.alt = ((float)(aeroin.Model_fPosZ));
gps.fix_type = 3;
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);
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)));
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
@ -913,7 +915,7 @@ namespace ArdupilotMega.GCSViews
att.pitch = fdm.theta;
att.yaw = fdm.psi;
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.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
@ -927,13 +929,14 @@ namespace ArdupilotMega.GCSViews
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
gps.alt = ((int)(fdm.altitude * ft2m * 1000));
gps.alt = ((float)(fdm.altitude * ft2m));
gps.fix_type = 3;
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);
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);
asp.airspeed = fdm.vcas * kts2fps * ft2m;
}
@ -992,13 +995,15 @@ namespace ArdupilotMega.GCSViews
att.roll = (DATA[18][1]);
att.yaw = (DATA[19][2]);
gps.alt = ((int)(DATA[20][2] * ft2m * 1000));
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
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));
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;
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
}
@ -1036,7 +1041,7 @@ namespace ArdupilotMega.GCSViews
{
lastgpsupdate = DateTime.Now;
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT, gps);
comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, 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_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_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 = ((int)(altitude * 1000));
gps.alt = ((float)(altitude));
gps.fix_type = 3;
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);
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);
asp.airspeed = gps.vel;
asp.airspeed = gps.v;
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_INT, gps);
MainV2.comPort.generatePacket(ArdupilotMega.MAVLink.MAVLINK_MSG_ID_GPS_RAW, gps);
//Console.WriteLine(DateTime.Now.Millisecond + " GPS" );
}

14
Tools/ArdupilotMegaPlanner/Joystick.cs
View File

@ -326,8 +326,20 @@ namespace ArdupilotMega
{
try
{
MainV2.comPort.setMode(but.mode);
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(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"); }
});

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

1553
Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs
View File

File diff suppressed because it is too large Load Diff

1459
Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs
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_TYPE_GCS;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 3;
htb.type = (byte)MAVLink.MAV_TYPE.MAV_GENERIC;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT_TYPE.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 2;
comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_HEARTBEAT, htb);
heatbeatsend = DateTime.Now;
@ -1622,8 +1622,7 @@ namespace ArdupilotMega
}
if (keyData == (Keys.Control | Keys.Y)) // for ryan beall
{
int fixme;
//MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE);
MainV2.comPort.doAction(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,8 +1029,7 @@ namespace ArdupilotMega.Setup
{
try
{
int fixme;
//MainV2.comPort.doCommand(MAVLink.MAV_ACTION.MAV_ACTION_CALIBRATE_ACC);
MainV2.comPort.doAction(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>N7WYfTP50GvYcUT/jKrADGfpBj8=</dsig:DigestValue>
<dsig:DigestValue>S+dMQOC9TeJyQiYvhw37LpJxZU0=</dsig:DigestValue>
</hash>
</dependentAssembly>
</dependency>

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

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

13
Tools/ArdupilotMegaPlanner/dataflashlog.xml
View File

@ -22,15 +22,10 @@
</ATT>
<NTUN>
<F1>WP Dist</F1>
<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>
<F2>WP Verify</F2>
<F3>Target Bear</F3>
<F4>Long Err</F4>
<F5>Lat Err</F5>
</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_v1.0/common/";
$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/";
$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/";
opendir(DIR,$dir) || die print $!;
@files2 = readdir(DIR);
@ -59,12 +59,8 @@ foreach $file (@files) {
}
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";
$no = $2;
}
print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n";
$no = $2;
}
if ($line =~ /typedef struct(.*)/) {
if ($1 =~ /__mavlink_system|param_union/) {
@ -85,7 +81,6 @@ 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/;

30
Tools/autotest/ArduCopter.parm Normal file
View File

@ -0,0 +1,30 @@
RC1_MAX 2000.000000
RC1_MIN 1000.000000
RC1_TRIM 1500.000000
RC2_MAX 2000.000000
RC2_MIN 1000.000000
RC2_TRIM 1500.000000
RC3_MAX 2000.000000
RC3_MIN 1000.000000
RC3_TRIM 1500.000000
RC4_MAX 2000.000000
RC4_MIN 1000.000000
RC4_TRIM 1500.000000
RC5_MAX 2000.000000
RC5_MIN 1000.000000
RC5_TRIM 1500.000000
RC6_MAX 2000.000000
RC6_MIN 1000.000000
RC6_TRIM 1500.000000
RC7_MAX 2000.000000
RC7_MIN 1000.000000
RC7_TRIM 1500.000000
RC8_MAX 2000.000000
RC8_MIN 1000.000000
RC8_TRIM 1500.000000
FLTMODE1 3
FLTMODE2 1
FLTMODE3 2
FLTMODE4 6
FLTMODE5 5
FLTMODE6 0

1
Tools/autotest/README Normal file
View File

@ -0,0 +1 @@
This is an automated test suite for APM

143
Tools/autotest/arducopter.py Normal file
View File

@ -0,0 +1,143 @@
# fly ArduCopter in SIL
import util, pexpect, sys, time, math, shutil
sys.path.insert(0, 'pymavlink')
import mavutil
HOME_LOCATION='-35.362938,149.165085,650,270'
def arm_motors(mavproxy):
'''arm motors'''
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.expect('STABILIZE>')
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 2000\n')
mavproxy.expect('APM: ARMING MOTORS')
mavproxy.send('rc 4 1500\n')
print("MOTORS ARMED OK")
def disarm_motors(mavproxy):
'''disarm motors'''
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 1000\n')
mavproxy.expect('APM: DISARMING MOTORS')
mavproxy.send('rc 4 1500\n')
print("MOTORS DISARMED OK")
def takeoff(mavproxy, mav):
'''takeoff get to 30m altitude'''
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.expect('STABILIZE>')
mavproxy.send('rc 3 1500\n')
mavproxy.send('status\n')
m = mav.recv_match(type='VFR_HUD', condition='VFR_HUD.alt>=30', blocking=True)
print("Altitude %u" % m.alt)
print("TAKEOFF COMPLETE")
def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
mavproxy.expect('LOITER>')
mavproxy.send('status\n')
mavproxy.expect('>')
m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
tstart = time.time()
tholdstart = time.time()
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Altitude %u" % m.alt)
if math.fabs(m.alt - start_altitude) > maxaltchange:
tholdstart = time.time()
if time.time() - tholdstart > holdtime:
print("Loiter OK for %u seconds" % holdtime)
return True
print("Loiter FAILED")
return False
def land(mavproxy, mav, timeout=60):
'''land the quad'''
mavproxy.send('switch 6\n')
mavproxy.expect('STABILIZE>')
mavproxy.send('status\n')
mavproxy.expect('>')
mavproxy.send('rc 3 1300\n')
tstart = time.time()
while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Altitude %u" % m.alt)
if m.alt <= 0:
print("LANDED OK")
return True
print("LANDING FAILED")
return False
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in range(1,9):
mavproxy.send('rc %u 1500\n' % chan)
# zero throttle
mavproxy.send('rc 3 1000\n')
def fly_ArduCopter():
'''fly ArduCopter in SIL'''
util.rmfile('eeprom.bin')
sil = util.start_SIL('ArduCopter')
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
mavproxy.expect('Please Run Setup')
# we need to restart it after eeprom erase
mavproxy.close()
sil.close()
sil = util.start_SIL('ArduCopter')
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:14550 --quadcopter')
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
mavproxy.expect("Ready to FLY")
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load autotest/ArduCopter.parm\n")
mavproxy.expect('Loaded [0-9]+ parameters')
# start hil_quad.py
hquad = pexpect.spawn('HILTest/hil_quad.py --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION,
logfile=sys.stdout, timeout=10)
hquad.expect('Starting at')
# get a mavlink connection going
mav = mavutil.mavlink_connection('127.0.0.1:14550', robust_parsing=True)
failed = False
try:
mav.wait_heartbeat()
mav.recv_match(type='GPS_RAW')
setup_rc(mavproxy)
arm_motors(mavproxy)
takeoff(mavproxy, mav)
loiter(mavproxy, mav)
land(mavproxy, mav)
disarm_motors(mavproxy)
except Exception, e:
failed = True
mavproxy.close()
sil.close()
hquad.close()
shutil.copy(logfile, "buildlogs/ArduCopter-test.mavlog")
util.run_cmd("pymavlink/examples/mavtogpx.py buildlogs/ArduCopter-test.mavlog")
util.run_cmd("bin/gpxtokml buildlogs/ArduCopter-test.mavlog.gpx")
if failed:
print("FAILED: %s" % e)
sys.exit(1)

76
Tools/autotest/autotest.py Executable file
View File

@ -0,0 +1,76 @@
#!/usr/bin/env python
# APM automatic test suite
# Andrew Tridgell, October 2011
import pexpect, os, util, sys, shutil, arducopter
import optparse, fnmatch
os.putenv('TMPDIR', util.relcwd('tmp'))
def get_default_params(atype):
'''get default parameters'''
util.rmfile('eeprom.bin')
sil = util.start_SIL(atype)
mavproxy = util.start_MAVProxy_SIL(atype)
idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
if idx == 0:
# we need to restart it after eeprom erase
mavproxy.close()
sil.close()
sil = util.start_SIL(atype)
mavproxy = util.start_MAVProxy_SIL(atype)
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
parmfile = mavproxy.match.group(1)
dest = os.path.join('buildlogs/%s.defaults.txt' % atype)
shutil.copy(parmfile, dest)
mavproxy.close()
sil.close()
print("Saved defaults for %s to %s" % (atype, dest))
############## main program #############
parser = optparse.OptionParser("autotest")
parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)')
parser.add_option("--list", action='store_true', default=False, help='list the available steps')
opts, args = parser.parse_args()
steps = [
'build.ArduPlane',
'build.ArduCopter',
'defaults.ArduPlane',
'defaults.ArduCopter',
'fly.ArduCopter'
]
skipsteps = opts.skip.split(',')
def skip_step(step):
'''see if a step should be skipped'''
for skip in skipsteps:
if fnmatch.fnmatch(step, skip):
return True
return False
# kill any previous instance
util.kill('ArduCopter.elf')
util.kill('ArduPilot.elf')
for step in steps:
if skip_step(step):
continue
if step == 'build.ArduPlane':
util.build_SIL('ArduPlane')
elif step == 'build.ArduCopter':
util.build_SIL('ArduCopter')
elif step == 'defaults.ArduPlane':
get_default_params('ArduPlane')
elif step == 'defaults.ArduCopter':
get_default_params('ArduCopter')
elif step == 'fly.ArduCopter':
arducopter.fly_ArduCopter()
else:
raise RuntimeError("Unknown step %s" % step)
util.kill('ArduCopter.elf')
util.kill('ArduPilot.elf')

64
Tools/autotest/util.py Normal file
View File

@ -0,0 +1,64 @@
# utility code for autotest
import os, pexpect, sys
from subprocess import call, check_call,Popen, PIPE
def relhome(path):
'''return a path relative to $HOME'''
return os.path.join(os.getenv('HOME'), path)
def relcwd(path):
'''return a path relative to $CWD'''
return os.path.join(os.getcwd(), path)
def run_cmd(cmd, dir=".", show=False, output=False, checkfail=True):
'''run a shell command'''
if show:
print("Running: '%s' in '%s'" % (cmd, dir))
if output:
return Popen([cmd], shell=True, stdout=PIPE, cwd=dir).communicate()[0]
elif checkfail:
return check_call(cmd, shell=True, cwd=dir)
else:
return call(cmd, shell=True, cwd=dir)
def rmfile(path):
'''remove a file if it exists'''
try:
os.unlink('eeprom.bin')
except Exception:
pass
def deltree(path):
'''delete a tree of files'''
run_cmd('rm -rf %s' % path)
def build_SIL(atype):
'''build desktop SIL'''
run_cmd("make -f ../libraries/Desktop/Makefile.desktop clean hil",
dir=relcwd('APM/' + atype),
checkfail=True)
def start_SIL(atype):
'''launch a SIL instance'''
ret = pexpect.spawn('tmp/%s.build/%s.elf' % (atype, atype),
logfile=sys.stdout, timeout=5)
ret.expect('Waiting for connection')
return ret
def start_MAVProxy_SIL(atype, options=''):
'''launch mavproxy connected to a SIL instance'''
MAVPROXY = relcwd('MAVProxy/mavproxy.py')
ret = pexpect.spawn('%s --master=tcp:127.0.0.1:5760 --aircraft=test.%s %s' % (
MAVPROXY,atype,options),
logfile=sys.stdout, timeout=60)
return ret
def kill(name):
'''kill a process'''
run_cmd('killall -9 -q %s' % name, checkfail=False)