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()); }