public override void SendCommand()
{
if (masterpos.Lat == 0 || masterpos.Lng == 0)
{
return;
}
//Console.WriteLine(DateTime.Now);
//Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt);
int a = 0;
foreach (var port in MainV2.Comports.ToArray())
{
foreach (var mav in port.MAVlist)
{
if (mav == Leader)
{
continue;
}
PointLatLngAlt target = new PointLatLngAlt(masterpos);
try
{
int utmzone = (int)((masterpos.Lng - -186.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone,
masterpos.Lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] pll1 = { target.Lng, target.Lat };
double[] p1 = trans.MathTransform.Transform(pll1);
double heading = -Leader.cs.yaw;
double length = offsets[mav].length();
var x = ((Vector3)offsets[mav]).x;
var y = ((Vector3)offsets[mav]).y;
// add offsets to utm
p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad);
p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad);
// convert back to wgs84
IMathTransform inversedTransform = trans.MathTransform.Inverse();
double[] point = inversedTransform.Transform(p1);
target.Lat = point[1];
target.Lng = point[0];
target.Alt += ((Vector3)offsets[mav]).z;
if (mav.cs.firmware == Firmwares.ArduPlane)
{
// get distance from target position
var dist = target.GetDistance(mav.cs.Location);
// get bearing to target
var targyaw = mav.cs.Location.GetBearing(target);
var targettrailer = target.newpos(Leader.cs.yaw, Math.Abs(dist) * -0.25);
var targetleader = target.newpos(Leader.cs.yaw, 10 + dist);
var yawerror = wrap_180(targyaw - mav.cs.yaw);
var mavleadererror = wrap_180(Leader.cs.yaw - mav.cs.yaw);
if (dist < 100)
{
targyaw = mav.cs.Location.GetBearing(targetleader);
yawerror = wrap_180(targyaw - mav.cs.yaw);
var targBearing = mav.cs.Location.GetBearing(target);
// check the bearing for the leader and target are within 45 degrees.
if (Math.Abs(wrap_180(targBearing - targyaw)) > 45)
{
dist *= -1;
}
}
else
{
targyaw = mav.cs.Location.GetBearing(targettrailer);
yawerror = wrap_180(targyaw - mav.cs.yaw);
}
// display update
mav.GuidedMode.x = (float)target.Lat;
mav.GuidedMode.y = (float)target.Lng;
mav.GuidedMode.z = (float)target.Alt;
MAVLink.mavlink_set_attitude_target_t att_target = new MAVLink.mavlink_set_attitude_target_t();
att_target.target_system = mav.sysid;
att_target.target_component = mav.compid;
att_target.type_mask = 0xff;
Tuple <PID, PID, PID, PID> pid;
if (pids.ContainsKey(mav))
{
pid = pids[mav];
}
else
{
pid = new Tuple <PID, PID, PID, PID>(
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1, 0, 0.00f, 15, 20, 0.1f, 0),
new PID(0.01f, 0.001f, 0, 0.5f, 20, 0.1f, 0));
pids.Add(mav, pid);
}
var rollp = pid.Item1;
var pitchp = pid.Item2;
var yawp = pid.Item3;
var thrustp = pid.Item4;
var newroll = 0d;
var newpitch = 0d;
if (true)
{
var altdelta = target.Alt - mav.cs.alt;
newpitch = altdelta;
att_target.type_mask -= 0b00000010;
pitchp.set_input_filter_all((float)altdelta);
newpitch = pitchp.get_pid();
}
if (true)
{
var leaderturnrad = Leader.cs.radius;
var mavturnradius = leaderturnrad - x;
{
var distToTarget = mav.cs.Location.GetDistance(target);
var bearingToTarget = mav.cs.Location.GetBearing(target);
// bearing stability
if (distToTarget < 30)
{
bearingToTarget = mav.cs.Location.GetBearing(targetleader);
}
// fly in from behind
if (distToTarget > 100)
{
bearingToTarget = mav.cs.Location.GetBearing(targettrailer);
}
var bearingDelta = wrap_180(bearingToTarget - mav.cs.yaw);
var tangent90 = bearingDelta > 0 ? 90 : -90;
newroll = 0;
// if the delta is > 90 then we are facing the wrong direction
if (Math.Abs(bearingDelta) < 85)
{
var insideAngle = Math.Abs(tangent90 - bearingDelta);
var angleCenter = 180 - insideAngle * 2;
// sine rule
var sine1 = Math.Max(distToTarget, 40) /
Math.Sin(angleCenter * MathHelper.deg2rad);
var radius = sine1 * Math.Sin(insideAngle * MathHelper.deg2rad);
// average calced + leader offset turnradius - acts as a FF
radius = (Math.Abs(radius) + Math.Abs(mavturnradius)) / 2;
var angleBank = ((mav.cs.groundspeed * mav.cs.groundspeed) / radius) / 9.8;
angleBank *= MathHelper.rad2deg;
if (bearingDelta > 0)
{
newroll = Math.Abs(angleBank);
}
else
{
newroll = -Math.Abs(angleBank);
}
}
newroll += MathHelper.constrain(bearingDelta, -20, 20);
}
// tr = gs2 / (9.8 * x)
// (9.8 * x) * tr = gs2
// 9.8 * x = gs2 / tr
// (gs2/tr)/9.8 = x
var angle = ((mav.cs.groundspeed * mav.cs.groundspeed) / mavturnradius) / 9.8;
//newroll = angle * MathHelper.rad2deg;
// 1 degree of roll for ever 1 degree of yaw error
//newroll += MathHelper.constrain(yawerror, -20, 20);
//rollp.set_input_filter_all((float)yawdelta);
}
// do speed
if (true)
{
//att_target.thrust = (float) MathHelper.mapConstrained(dist, 0, 40, 0, 1);
att_target.type_mask -= 0b01000000;
// in m out 0-1
thrustp.set_input_filter_all((float)dist);
// prevent buildup prior to being close
if (dist > 40)
{
thrustp.reset_I();
}
// 0.1 demand + pid results
att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1);
}
Quaternion q = new Quaternion();
q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad);
att_target.q = new float[4];
att_target.q[0] = (float)q.q1;
att_target.q[1] = (float)q.q2;
att_target.q[2] = (float)q.q3;
att_target.q[3] = (float)q.q4;
//0b0= rpy
att_target.type_mask -= 0b10000101;
//att_target.type_mask -= 0b10000100;
Console.WriteLine("sysid {0} - {1} dist {2} r {3} p {4} y {5}", mav.sysid,
att_target.thrust, dist, newroll, newpitch, (targyaw - mav.cs.yaw));
/* Console.WriteLine("rpyt {0} {1} {2} {3} I {4} {5} {6} {7}",
* rollp.get_pid(), pitchp.get_pid(), yawp.get_pid(), thrustp.get_pid(),
* rollp.get_i(), pitchp.get_i(), yawp.get_i(), thrustp.get_i());
*/
port.sendPacket(att_target, mav.sysid, mav.compid);
}
else
{
Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz);
// do pos/vel
port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true,
true, false, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x,
vel.y, vel.z, 0, 0);
// do yaw
if (!gimbal)
{
// within 3 degrees dont send
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw,
100.0f, 0, 0, 0, 0, 0, false);
}
}
else
{
// gimbal direction
if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3)
{
port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false);
}
}
}
//Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt);
}
catch (Exception ex)
{
Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString());
}
a++;
}
}
}
public void setOffsets(MAVState mav, double x, double y, double z)
{
offsets[mav] = new Vector3(x, y, z);
log.Info(mav.ToString() + " " + offsets[mav].ToString());
}