public static void SIM_InjectPosition(double lat, double lng, double alt, SMission?mission, bool refreshMission)
{
mode = APMModes.Quadrotor.ROTOR_AUTO;
dronePosition.altFromGround = alt;
dronePosition.heading = 57;
GPSHdop = 0.9;
dronePosition.lat = lat;
dronePosition.lng = lng;
dronePosition.alt = alt;
satCount = 18;
if (OnDroneReady != null)
{
OnDroneReady(true);
}
if (refreshMission)
{
if (mission.HasValue)
{
missionItems = mission.Value.Items.ToArray();
if (OnMissionUpdated != null)
{
OnMissionUpdated(12, 12);
}
}
}
}
public static void ChangeFlightMode(APMModes.Quadrotor mode)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t();
cmd.target_system = sysID;
cmd.base_mode = 1; // Not sure what this is
cmd.custom_mode = (uint)mode;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
SendPacket(commandBytes);
System.Threading.Thread.Sleep(10);
SendPacket(commandBytes);
}
public static String ModeToString(APMModes.Quadrotor mode)
{
switch (mode)
{
case Quadrotor.ROTOR_STABILIZE:
return("Stabilize");
case Quadrotor.ROTOR_ACRO:
return("Acro");
case Quadrotor.ROTOR_ALT_HOLD:
return("Alt Hold");
case Quadrotor.ROTOR_AUTO:
return("Auto");
case Quadrotor.ROTOR_GUIDED:
return("Guided");
case Quadrotor.ROTOR_LOITER:
return("Loiter");
case Quadrotor.ROTOR_RTL:
return("RTL");
case Quadrotor.ROTOR_CIRCLE:
return("Circle");
case Quadrotor.ROTOR_LAND:
return("Land");
case Quadrotor.ROTOR_TOY:
return("Toy");
case Quadrotor.ROTOR_SPORT:
return("Sport");
case Quadrotor.ROTOR_AUTOTUNE:
return("Autotune");
case Quadrotor.ROTOR_POSHOLD:
return("Pos Hold");
case Quadrotor.ROTOR_TES:
return("Third Eye");
}
return("Error");
}
public static void UpdateLastFlightMode(APMModes.Quadrotor mode)
{
lastMode = mode;
}
private static void HandlePacket(object packet)
{
if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet;
droneState = (MAVLink.MAV_STATE)parsed.system_status;
//MAVLink.MAV_MODE
mode = (APMModes.Quadrotor)parsed.custom_mode;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
// dronePosition.lat = (double)gps.lat / 10000000;
// dronePosition.lng = (double)gps.lon / 10000000;
// dronePosition.alt = (double)gps.alt / 1000;
dronePosition.altFromGround = (double)gps.relative_alt / 1000;
dronePosition.heading = (double)gps.hdg / 100;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
GPSHdop = (double)gps.eph / 100;
dronePosition.lat = (double)gps.lat / 10000000;
dronePosition.lng = (double)gps.lon / 10000000;
dronePosition.alt = (double)gps.alt / 1000;
satCount = gps.satellites_visible;
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
missionWaypointsCount = paramValue.count;
missionItems = new SMissionItem[missionWaypointsCount];
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet;
SMissionItem missionItem = new SMissionItem();
missionItem.command = (MAVLink.MAV_CMD)paramValue.command;
// if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF)
// // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH))
// {
// missionItem.lat = dronePosition.lat;
// missionItem.lng = dronePosition.lng;
// }
// else
{
missionItem.lat = paramValue.x;
missionItem.lng = paramValue.y;
}
missionItem.p1 = paramValue.param1;
missionItem.p2 = paramValue.param2;
missionItem.p3 = paramValue.param3;
missionItem.p4 = paramValue.param4;
missionItem.alt = paramValue.z;
// Is this the home?
missionItem.IsHome = (paramValue.seq == 0);
missionItems[paramValue.seq] = missionItem;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet;
SetMissionItem(missionReq.seq);
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id);
name = name.Trim().Replace("\0", "");
double val = paramValue.param_value;
if (OnParamUpdate != null)
{
OnParamUpdate(name, val, paramValue.param_index);
}
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
// Check that all estimations are good
EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance))));
EKFFlags = ekf.flags;
IsDroneReady();
}
}
