private void _transport_OnMessageReceived(object sender, UasMessage arg)
{
switch (arg.MessageId)
{
case UasMessages.Statustext:
var stat = arg as UasStatustext;
var message = new String(stat.Text);
if (message.ToLower().Contains("calibration successful") || message.ToLower().Contains("calibration failed"))
{
IsDone = true;
IsActive = false;
DoneCommand.RaiseCanExecuteChanged();
UserMessage = message;
}
break;
case UasMessages.CommandLong:
var cmd = arg as UasCommandLong;
if (cmd.Command == (ushort)MavCmd.AccelcalVehiclePos)
{
var pos = (AccelcalVehiclePos)cmd.Param1;
UserMessage = "Please place vehicle " + pos.ToString();
}
break;
}
}
public void SendMessage(UasMessage msg)
{
switch (msg.MessageId)
{
case UasMessages.CommandLong:
{
var cmd = msg as UasCommandLong;
switch (cmd.Command)
{
case 22:
DJISDKManager.Instance.ComponentManager.GetFlightControllerHandler(0, 0).StartTakeoffAsync();
break;
case 23:
DJISDKManager.Instance.ComponentManager.GetFlightControllerHandler(0, 0).StartAutoLandingAsync();
break;
}
}
break;
case UasMessages.ManualControl:
{
var cmd = msg as UasManualControl;
DJISDKManager.Instance.VirtualRemoteController.UpdateJoystickValue(cmd.Z / 1000.0f, cmd.Y / 1000.0f, cmd.X / 1000.0f, cmd.R / 1000.0f);
}
break;
}
}
public override void SendMessage(UasMessage msg)
{
mSendQueue.Enqueue(msg);
// Signal send thread
mSendSignal.Set();
}
private void SendMessage(UasMessage msg)
{
if (_connectedUasManager.Active != null)
{
_connectedUasManager.Active.Transport.SendMessage(msg);
}
}
private void DumpMsgMeta(UasMessage m)
{
var md = m.GetMetadata();
foreach (var f in md.Fields)
{
WL(" {0}: {1} ({2})", f.Name, GetFieldValue(f.Name, m), f.Description);
}
}
/// <summary>
/// Generates the buffer bytes to be sent on the wire for given message.
/// </summary>
/// <param name="msg"></param>
/// <param name="systemId"></param>
/// <param name="componentId"></param>
/// <param name="includeSignalMark">Whether to include the Packet signal in the buffer or not.</param>
/// <param name="sequenceNumber">A sequence number for the message, if needed.</param>
/// <returns></returns>
public byte[] SerializeMessage(
UasMessage msg, byte systemId, byte componentId,
bool includeSignalMark, byte sequenceNumber = 1)
{
byte mark = includeSignalMark ? PacketSignalByte : (byte)0;
return(MavLinkPacket.GetBytesForMessage(
msg, systemId, componentId, sequenceNumber, mark));
}
private void PrintMessage(UasMessage m)
{
UasMessageMetadata md = m.GetMetadata();
foreach (UasFieldMetadata f in md.Fields) //==================================================================
{
GetFieldValue(f.Name, m); //============================================================
// //f.Name, GetFieldValue(f.Name, m), f.Description
}
}
private static void PrintStandardMessage(UasMessage m)
{
UasMessageMetadata md = m.GetMetadata();
WL("{0}: {1}", m, md.Description);
foreach (UasFieldMetadata f in md.Fields)
{
WL(" {0}: {1} ({2})", f.Name, GetFieldValue(f.Name, m), f.Description);
}
}
private void GetHeartbeat(UasMessage msg)
{
var heartbeat = msg as UasHeartbeat;
Type = Convert.ToByte(heartbeat.Type);
Autopilot = Convert.ToByte(heartbeat.Autopilot);
BaseMode = Convert.ToByte(heartbeat.BaseMode);
SystemStatus = Convert.ToByte(heartbeat.SystemStatus);
CustomMode = heartbeat.CustomMode;
MavlinkVersion = heartbeat.MavlinkVersion;
}
private void GetVfrHud(UasMessage msg)
{
var vfrHud = msg as UasVfrHud;
Airspeed = vfrHud.Airspeed;
Groundspeed = vfrHud.Groundspeed;
Heading = vfrHud.Heading;
Throttle = vfrHud.Throttle;
Alt = vfrHud.Alt;
Climb = vfrHud.Climb;
}
private void GetLocalPositionNed(UasMessage msg)
{
var localPositionNed = msg as UasLocalPositionNed;
TimeBootMs = localPositionNed.TimeBootMs;
X = localPositionNed.X;
Y = localPositionNed.Y;
Z = localPositionNed.Z;
Vx = localPositionNed.Vx;
Vy = localPositionNed.Vy;
Vz = localPositionNed.Vz;
}
private void GetAltitude(UasMessage msg)
{
var altitude = msg as UasAltitude;
TimeUsec = altitude.TimeUsec;
AltitudeMonotonic = altitude.AltitudeMonotonic;
AltitudeAmsl = altitude.AltitudeAmsl;
AltitudeLocal = altitude.AltitudeLocal;
AltitudeRelative = altitude.AltitudeRelative;
AltitudeTerrain = altitude.AltitudeTerrain;
BottomClearance = altitude.BottomClearance;
}
private void GetAttitude(UasMessage msg)
{
var attitude = msg as UasAttitude;
TimeBootMs = attitude.TimeBootMs;
Roll = attitude.Roll;
Pitch = attitude.Pitch;
Yaw = attitude.Yaw;
Rollspeed = attitude.Rollspeed;
Pitchspeed = attitude.Pitchspeed;
Yawspeed = attitude.Yawspeed;
}
private static void PrintMessage(UasMessage m)
{
if (m is UasCommandLong)
{
PrintCommandLong(m as UasCommandLong);
}
else
{
PrintStandardMessage(m);
}
WL("");
}
private void GetGpsRawInt(UasMessage msg)
{
var gpsRawInt = msg as UasGpsRawInt;
TimeUsec = gpsRawInt.TimeUsec;
FixType = gpsRawInt.FixType;
Lat = gpsRawInt.Lat;
Lon = gpsRawInt.Lon;
Alt = gpsRawInt.Alt;
Eph = gpsRawInt.Eph;
Epv = gpsRawInt.Epv;
Vel = gpsRawInt.Vel;
Cog = gpsRawInt.Cog;
}
private void GetServoOutputRaw(UasMessage msg)
{
var servoOutputRaw = msg as UasServoOutputRaw;
TimeUsec = servoOutputRaw.TimeUsec;
Port = servoOutputRaw.Port;
Servo1Raw = servoOutputRaw.Servo1Raw;
Servo2Raw = servoOutputRaw.Servo2Raw;
Servo3Raw = servoOutputRaw.Servo3Raw;
Servo4Raw = servoOutputRaw.Servo4Raw;
Servo5Raw = servoOutputRaw.Servo5Raw;
Servo6Raw = servoOutputRaw.Servo6Raw;
Servo7Raw = servoOutputRaw.Servo7Raw;
Servo8Raw = servoOutputRaw.Servo8Raw;
}
public void SendMessage(UasMessage msg)
{
switch (msg.MessageId)
{
case UasMessages.CommandLong:
{
var cmd = msg as UasCommandLong;
switch (cmd.Command)
{
case 22:
_drone.Tello.Controller.TakeOff();
break;
case 23:
_drone.Tello.Controller.Land();
break;
}
}
break;
case UasMessages.GimbalControl:
break;
case UasMessages.ManualControl:
{
var cmd = msg as UasManualControl;
var x = cmd.R / 10;
var y = cmd.X / 10;
var r = cmd.Y / 10;
var z = cmd.Z / 10;
if (x != _lastX || y != _lastY || z != _lastZ || r != _lastR)
{
_drone.Tello.Controller.Set4ChannelRC(x, y, z, r);
_lastX = x;
_lastY = y;
_lastZ = z;
_lastR = r;
}
}
break;
}
}
private void GetSysStatus(UasMessage msg)
{
var sysStatus = msg as UasSysStatus;
OnboardControlSensorsPresent = Convert.ToUInt32(sysStatus.OnboardControlSensorsPresent);
OnboardControlSensorsEnabled = Convert.ToUInt32(sysStatus.OnboardControlSensorsEnabled);
OnboardControlSensorsHealth = Convert.ToUInt32(sysStatus.OnboardControlSensorsHealth);
Load = sysStatus.Load;
VoltageBattery = sysStatus.VoltageBattery;
CurrentBattery = sysStatus.CurrentBattery;
BatteryRemaining = sysStatus.BatteryRemaining;
DropRateComm = sysStatus.DropRateComm;
ErrorsComm = sysStatus.ErrorsComm;
ErrorsCount1 = sysStatus.ErrorsCount1;
ErrorsCount2 = sysStatus.ErrorsCount2;
ErrorsCount3 = sysStatus.ErrorsCount3;
ErrorsCount4 = sysStatus.ErrorsCount4;
}
private void GetBatteryStatus(UasMessage msg)
{
var batteryStatus = msg as UasBatteryStatus;
Id = batteryStatus.Id;
BatteryFunction = batteryStatus.BatteryFunction;
Type = batteryStatus.Type;
Temperature = batteryStatus.Temperature;
for (int i = 0; i < batteryStatus.Voltages.Length; i++)
{
Voltages[i] = batteryStatus.Voltages[i];
}
CurrentBattery = batteryStatus.CurrentBattery;
CurrentConsumed = batteryStatus.CurrentConsumed;
EnergyConsumed = batteryStatus.EnergyConsumed;
BatteryRemaining = batteryStatus.BatteryRemaining;
}
private void GetPositionTargetGlobalInt(UasMessage msg)
{
var positionTargetGlobalInt = msg as UasPositionTargetGlobalInt;
TimeBootMs = positionTargetGlobalInt.TimeBootMs;
CoordinateFrame = Convert.ToByte(positionTargetGlobalInt.CoordinateFrame);
TypeMask = positionTargetGlobalInt.TypeMask;
LatInt = positionTargetGlobalInt.LatInt;
LonInt = positionTargetGlobalInt.LonInt;
Alt = positionTargetGlobalInt.Alt;
Vx = positionTargetGlobalInt.Vx;
Vy = positionTargetGlobalInt.Vy;
Vz = positionTargetGlobalInt.Vz;
Afx = positionTargetGlobalInt.Afx;
Afy = positionTargetGlobalInt.Afy;
Afz = positionTargetGlobalInt.Afz;
Yaw = positionTargetGlobalInt.Yaw;
YawRate = positionTargetGlobalInt.YawRate;
}
private static object GetFieldValue(string fieldName, UasMessage m)
{
PropertyInfo p = m.GetType().GetProperty(fieldName);
if (p == null)
{
WL("MISSING FIELD: {0} on {1}", fieldName, m.GetType());
return("");
}
object result = p.GetValue(m, null);
if (result is char[])
{
return(new String((char[])result));
}
return(result);
}
private void GetFieldValue(string fieldName, UasMessage m)
{
if (fieldName == "Roll")
{
PropertyInfo p = m.GetType().GetProperty(fieldName);
Double degree = Convert.ToDouble(p.GetValue(m, null).ToString()) * (180 / Math.PI);
lblRoll.Invoke((MethodInvoker)(() => lblRoll.Text = degree.ToString()));
//lblRoll.Text= p.GetValue(m, null).ToString();
}
else if (fieldName == "Pitch")
{
PropertyInfo p = m.GetType().GetProperty(fieldName);
Double degree = Convert.ToDouble(p.GetValue(m, null).ToString()) * (180 / Math.PI);
lblPitch.Invoke((MethodInvoker)(() => lblPitch.Text = degree.ToString()));
//lblPitch.Text = p.GetValue(m, null).ToString();
}
else if (fieldName == "Yaw")
{
PropertyInfo p = m.GetType().GetProperty(fieldName);
Double degree = Convert.ToDouble(p.GetValue(m, null).ToString()) * (180 / Math.PI);
lblYaw.Invoke((MethodInvoker)(() => lblYaw.Text = degree.ToString()));
//lblYaw.Text = p.GetValue(m, null).ToString();
}
//if (fieldName != "Rxerrors" && fieldName != "TimeBootMs" && fieldName != "TimeUsec"&& fieldName != "Chan1Raw" && fieldName != "Fixed" && fieldName != "Chan2Raw" && fieldName!= "VtolState" && fieldName!= "Chan3Raw" && fieldName!= "Rssi" && fieldName!="Q" && fieldName!= "Chan4Raw")
//{
// PropertyInfo p = m.GetType().GetProperty(fieldName);
// var resulti = p.GetValue(m,null);
// object result = p.GetValue(m, null);
//}
//if (p == null)
//{
// WL("MISSING FIELD: {0} on {1}", fieldName, m.GetType());
// return "";
//}
//if (result is char[]) return new String((char[])result);
//return result;
}
private void GetHighresImu(UasMessage msg)
{
var highresImu = msg as UasHighresImu;
TimeUsec = highresImu.TimeUsec;
Xacc = highresImu.Xacc;
Yacc = highresImu.Yacc;
Zacc = highresImu.Zacc;
Xgyro = highresImu.Xgyro;
Ygyro = highresImu.Ygyro;
Zgyro = highresImu.Zgyro;
Xmag = highresImu.Xmag;
Ymag = highresImu.Ymag;
Zmag = highresImu.Zmag;
AbsPressure = highresImu.AbsPressure;
DiffPressure = highresImu.DiffPressure;
PressureAlt = highresImu.PressureAlt;
Temperature = highresImu.Temperature;
FieldsUpdated = highresImu.FieldsUpdated;
}
public override void SendMessage(UasMessage msg)
{
MessagesSent++;
_sendQueue.Enqueue(msg);
_sendSignal.Set();
}
private async Task SendMavlinkMessage(UasMessage msg)
{
byte[] buffer = _mavLinkAsyncWalker.SerializeMessage(msg, SystemId, ComponentId, true);
await _serialPort.WriteAsync(buffer);
}
private void MavlinkReceived(UasMessage msg)
{
MavlinkStatus = "Mavlink Connected";
IsMavlinkConnected = true;
}
public void Update(UasMessage msg)
{
_adapter.UpdateUas(this, msg);
}
private void _telemeteryLink_MessageParsed(object sender, UasMessage msg)
{
Connections.Active.Uas.Update(msg);
}
public async Task <InvokeResult <TMavlinkPacket> > RequestDataAsync <TMavlinkPacket>(UasMessage msg, UasMessages incomingMessageId) where TMavlinkPacket : class
{
await Task.Delay(1);
return(InvokeResult <TMavlinkPacket> .FromError("Uknown"));
}
public override void SendMessage(UasMessage msg)
{
// No messages are sent on this transport (only read from the logfile)
}
