1677 lines
62 KiB
C#
1677 lines
62 KiB
C#
// #define LOG_SEQUENCE_NUMBER
|
||
//#define LOG_PACKETS
|
||
|
||
using Plane.Protocols;
|
||
using Plane.Communication;
|
||
using TaskTools.Utilities;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using System.Diagnostics;
|
||
using System.IO;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace Plane.Copters
|
||
{
|
||
#if PRIVATE
|
||
public
|
||
#else
|
||
internal
|
||
#endif
|
||
partial class PlaneCopter
|
||
{
|
||
uint iErrorData;
|
||
ulong iDataCount;
|
||
private int _recnumber = 0;
|
||
private int _sendnumber = 0;
|
||
|
||
public bool IsConnected { get { return Connection.IsOpen; } }
|
||
|
||
public string Id { get; set; }
|
||
|
||
public byte mavlinkversion { get; set; }
|
||
public MAVLink.MAV_TYPE aptype { get; set; }
|
||
public MAVLink.MAV_AUTOPILOT apname { get; set; }
|
||
private bool CommOK = true;
|
||
public byte sysid { get; set; }
|
||
public byte compid { get; set; }
|
||
public byte recvpacketcount { get; set; }
|
||
public bool armed { get; set; }
|
||
public bool failsafe { get; set; }
|
||
|
||
public uint mode { get; set; }
|
||
public uint commModuleMode { get; set; }
|
||
public byte commModuleVersion { get; set; }
|
||
|
||
public bool isUnlocked { get; set; }
|
||
|
||
public float battery_voltage { get; set; }
|
||
public byte battery_remaining { get; set; }
|
||
public float current_battery { get; set; }
|
||
public float freemem { get; set; }
|
||
public float brklevel { get; set; }
|
||
|
||
public bool useLocation { get; set; }
|
||
public float gpsalt { get; set; }
|
||
public byte gpsstatus { get; set; }
|
||
public float gpshdop { get; set; }
|
||
public byte satcount { get; set; }
|
||
public float retain { get; set; }
|
||
public float groundspeed { get; set; }
|
||
public float groundcourse { get; set; }
|
||
public double lat { get; set; }
|
||
public double lng { get; set; }
|
||
public float nav_roll { get; set; }
|
||
public float nav_pitch { get; set; }
|
||
public short nav_bearing { get; set; }
|
||
public short target_bearing { get; set; }
|
||
public float wp_dist { get; set; }
|
||
public float alt_error { get; set; }
|
||
public float aspd_error { get; set; }
|
||
public float xtrack_error { get; set; }
|
||
public float roll { get; set; }
|
||
public float pitch { get; set; }
|
||
public float yaw { get; set; }
|
||
public short heading { get; set; }
|
||
public float airspeed { get; set; }
|
||
public float alt { get { return altorigin - altoffset; } }
|
||
public float altoffset = 0;
|
||
public float altorigin { get; private set; } = 0;
|
||
public float ch3percent { get; set; }
|
||
|
||
public ushort ch1in { get; set; }
|
||
public ushort ch2in { get; set; }
|
||
public ushort ch3in { get; set; }
|
||
public ushort ch4in { get; set; }
|
||
public ushort ch5in { get; set; }
|
||
public ushort ch6in { get; set; }
|
||
public ushort ch7in { get; set; }
|
||
public ushort ch8in { get; set; }
|
||
public float rxrssi { get; set; }
|
||
|
||
public byte remrssi { get; set; }
|
||
|
||
public uint timebootms { get; set; }
|
||
|
||
private int DataTimeOut = 0;
|
||
|
||
public Dictionary<string, float?> param = new Dictionary<string, float?>();
|
||
public Dictionary<string, MAVLink.MAV_PARAM_TYPE> param_types = new Dictionary<string, MAVLink.MAV_PARAM_TYPE>();
|
||
|
||
public IConnection Connection { get; set; }
|
||
|
||
public ushort FlightDistance2D { get; private set; }
|
||
|
||
private SynchronizationContext _uiSyncContext;
|
||
private bool _autoSendHeartbeat = true;
|
||
private bool _autoRequestData = true;
|
||
|
||
public PlaneCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true)
|
||
{
|
||
this.Connection = connection;
|
||
_uiSyncContext = uiSyncContext;
|
||
_autoSendHeartbeat = autoSendHeartbeat;
|
||
_autoRequestData = autoRequestData;
|
||
}
|
||
|
||
public async Task ConnectAsync()
|
||
{
|
||
//sysid = 2;
|
||
SendReq = false;
|
||
CommOK = true;
|
||
await Connection.OpenAsync().ConfigureAwait(false);
|
||
StartCommunication();
|
||
}
|
||
|
||
public async Task DisconnectAsync()
|
||
{
|
||
#if DEBUG && !NETFX_CORE && LOG_SEQUENCE_NUMBER
|
||
File.AppendAllText($"SequenceNumbers_{Id}.log", _sequenceLog.ToString());
|
||
#endif
|
||
await StopDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0);
|
||
CommOK = false;
|
||
Connection.Close();
|
||
}
|
||
|
||
public async Task GetCopterDataAsync()
|
||
{
|
||
//停止所有数据流
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false);
|
||
//需要电池电压，电流，GPS数据
|
||
/*
|
||
* 目前接收的数据是
|
||
* gps 38 字节
|
||
hud 28
|
||
SYS_STATUS 39
|
||
MISSION_CURRENT 10
|
||
HEARTBEAT 17
|
||
共132字节
|
||
5hz 592/s 太高
|
||
*/
|
||
await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 1).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据
|
||
//不需要姿态信息
|
||
// await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30
|
||
//需要高度和机头朝向数据
|
||
await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 1).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74
|
||
//不需要通道数据
|
||
//await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据
|
||
//不需要原始数据
|
||
// await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, ratesensors).ConfigureAwait(false); // 请求 传感器原始值
|
||
|
||
/* //已经停止所有数据了，不用单独停止
|
||
// Disable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. |
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER, 0).ConfigureAwait(false);
|
||
// Disable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. |
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.POSITION, 0).ConfigureAwait(false);
|
||
// Disable EXTRA3 (Dependent on the autopilot |)
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA3, 0).ConfigureAwait(false);
|
||
// Disable Plane Stream
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.GHOST_STREAM, 0).ConfigureAwait(false);
|
||
// Disable RAW_SENSORS
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 0).ConfigureAwait(false); //
|
||
// 5 Disable MSG_ATTITUDE roll、pitch、yaw、rollspeed、pitchspeed、yawspeed-- - 30
|
||
await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 0).ConfigureAwait(false); //
|
||
*/
|
||
|
||
}
|
||
|
||
private async void StartCommunication()
|
||
{
|
||
try
|
||
{
|
||
var writingTask = _autoSendHeartbeat ? StartSendingHeartbeatsAsync() : TaskUtils.CompletedTask;
|
||
var readingTask = StartReadingPacketsAsync();
|
||
await Task.WhenAll(writingTask, readingTask).ConfigureAwait(false);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
RaiseMessageCreated(ex.ToString());
|
||
}
|
||
}
|
||
|
||
#if DEBUG && LOG_PACKETS
|
||
|
||
private Stopwatch _sendHeartbeatStopwatch = new Stopwatch();
|
||
private TimeSpan _sendHeartbeatTimeSpan;
|
||
|
||
#endif
|
||
|
||
public async Task StartSendingHeartbeatsAsync()
|
||
{
|
||
await Task.Run(async () =>
|
||
{
|
||
while (CommOK)
|
||
{
|
||
if (!IsConnected)
|
||
{
|
||
break;
|
||
}
|
||
// if (heatbeatSend.Second != DateTime.Now.Second)
|
||
if (heatbeatSend.AddMilliseconds(1000) <= DateTime.Now)
|
||
{
|
||
var htb = new MAVLink.mavlink_heartbeat_t()
|
||
{
|
||
type = (byte)MAVLink.MAV_TYPE.GCS,
|
||
autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC,
|
||
mavlink_version = 3,
|
||
};
|
||
// await SendPacketAsync(htb).ConfigureAwait(false); //发心跳包
|
||
#if DEBUG && LOG_PACKETS
|
||
if (!_sendHeartbeatStopwatch.IsRunning)
|
||
{
|
||
_sendHeartbeatStopwatch.Start();
|
||
}
|
||
else
|
||
{
|
||
var elapsed = _sendHeartbeatStopwatch.Elapsed;
|
||
Debug.WriteLine(elapsed - _sendHeartbeatTimeSpan, "--------------------------发送心跳间隔");
|
||
_sendHeartbeatTimeSpan = elapsed;
|
||
}
|
||
#endif
|
||
heatbeatSend = DateTime.Now;
|
||
DataTimeOut += 1;
|
||
if (DataTimeOut > 3)
|
||
{
|
||
bInitChannel = false;
|
||
SendReq = false;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
await Task.Delay(100).ConfigureAwait(false);
|
||
}
|
||
}
|
||
}).ConfigureAwait(false);
|
||
}
|
||
|
||
private async Task StartReadingPacketsAsync()
|
||
{
|
||
await Task.Run(async () =>
|
||
{
|
||
while (CommOK)
|
||
{
|
||
if (!IsConnected)
|
||
{
|
||
RaiseEventOnUIThread(ConnectionBroken);
|
||
break;
|
||
}
|
||
|
||
var packet = await ReadPacketAsync().ConfigureAwait(false);
|
||
if (packet != null)
|
||
{
|
||
PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
|
||
var handled = true;
|
||
// Debug.WriteLine(packet[5],"收到数据类型");
|
||
switch (packet[5])
|
||
{
|
||
case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0
|
||
AnalyzeHeartbeatPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_SYS_STATUS: //1
|
||
AnalyzeSysStatusPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT: //24
|
||
AnalyzeGpsRawIntPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_VFR_HUD: //74
|
||
AnalyzeVfrHudPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_STATUSTEXT: //253
|
||
AnalyzeStatusTextPacket(packet);
|
||
break;
|
||
|
||
////以上为基本数据，
|
||
|
||
case MAVLink.MAVLINK_MSG_ID_RADIO: //109
|
||
AnalyzeRadioPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_PARAM_VALUE: //22
|
||
AnalyzeParamValuePacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_MEMINFO: //152
|
||
AnalyzeMemInfoPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT: //42
|
||
AnalyzeMissionCurrentPacket(packet);
|
||
break;
|
||
|
||
case MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //33
|
||
AnalyzeGlobalPositionIntPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //62
|
||
AnalyzeNavControllerOutputPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_ATTITUDE: //30
|
||
AnalyzeAttitudePacket(packet);
|
||
break;
|
||
|
||
case MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW: //35
|
||
AnalyzeRCChannelsRawPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_RAW_IMU: //没有用27
|
||
AnalyzeRawImuPacket(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_SCALED_IMU2: //没有用 116
|
||
AnalyzeScaledImu2Packet(packet);
|
||
break;
|
||
case MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS: //150
|
||
AnalyzeSensorOffsetsPacket(packet);
|
||
break;
|
||
|
||
default:
|
||
handled = false;
|
||
UnhandledPacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
|
||
break;
|
||
}
|
||
if (handled)
|
||
{
|
||
PacketHandled?.Invoke(this, new PacketReceivedEventArgs(packet));
|
||
}
|
||
}
|
||
}
|
||
}).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task GetParamsListAsync()
|
||
{
|
||
MAVLink.mavlink_param_request_list_t req = new MAVLink.mavlink_param_request_list_t();
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
|
||
}
|
||
|
||
public static List<KeyValuePair<int, string>> getModesList()
|
||
{
|
||
var flightModes = EnumTranslator.Translate<ac2modes_cn>();
|
||
return flightModes.ToList();
|
||
}
|
||
|
||
// 王海, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。
|
||
//volatile object readlock = new object();
|
||
|
||
#if DEBUG && LOG_PACKETS
|
||
private StringBuilder _log = new StringBuilder();
|
||
private Stopwatch _positionStopwatch = new Stopwatch();
|
||
private TimeSpan _lastPositionTimeSpan;
|
||
#endif
|
||
|
||
#if DEBUG && LOG_SEQUENCE_NUMBER
|
||
private StringBuilder _sequenceLog = new StringBuilder("-----------------------\n");
|
||
#endif
|
||
|
||
private byte[] _readBuffer = new byte[263];
|
||
|
||
/// <summary>
|
||
/// 异步读取数据包。// 王海, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。
|
||
/// </summary>
|
||
/// <returns>表示此异步操作的 <see cref="Task{TResult}"/> 实例，其结果为读取到的数据包。</returns>
|
||
private async Task<byte[]> ReadPacketAsync()
|
||
{
|
||
if (!Connection.IsOpen)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
int length = 0;
|
||
int readnumber = 0; //_recnumber;
|
||
try
|
||
{
|
||
readnumber = await Connection.ReadAsync(_readBuffer, 0, 1);
|
||
_recnumber += readnumber;
|
||
if (readnumber == 0)
|
||
{
|
||
return null;
|
||
}
|
||
if (_readBuffer[0] == MAVLink.MAVLINK_STX)
|
||
{
|
||
readnumber = await Connection.ReadAsync(_readBuffer, 1, 5);
|
||
_recnumber += readnumber;
|
||
if (readnumber == 0)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
#if DEBUG && LOG_SEQUENCE_NUMBER
|
||
// _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
|
||
_sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
|
||
#endif
|
||
// packet length
|
||
length = 6 + _readBuffer[1] + 2; // header + data + checksum
|
||
readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6);
|
||
_recnumber += readnumber;
|
||
if (readnumber == 0)
|
||
{
|
||
return null;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
#if DEBUG && LOG_PACKETS
|
||
_log.AppendLine("---------------");
|
||
_log.AppendLine("unexpected head: " + _readBuffer[0]);
|
||
#endif
|
||
|
||
return null;
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
return null;
|
||
}
|
||
|
||
uint crcvalue = BitConverter.ToUInt16(_readBuffer, length - 2);
|
||
|
||
#if DEBUG && LOG_PACKETS
|
||
_log.AppendLine("---------------");
|
||
for (int i = 0; i < 6; i++)
|
||
{
|
||
_log.Append(_readBuffer[i]).Append(" ");
|
||
}
|
||
_log.AppendLine();
|
||
for (int i = 6; i < length - 2; i++)
|
||
{
|
||
_log.Append(_readBuffer[i]).Append(" ");
|
||
}
|
||
_log.AppendLine();
|
||
for (int i = length - 2; i < length; i++)
|
||
{
|
||
_log.Append(_readBuffer[i]).Append(" ");
|
||
}
|
||
_log.AppendLine();
|
||
#endif
|
||
byte messageType = _readBuffer[5];
|
||
|
||
#if DEBUG && LOG_PACKETS
|
||
if (messageType == 33)
|
||
{
|
||
if (_positionStopwatch.IsRunning)
|
||
{
|
||
var elapsed = _positionStopwatch.Elapsed;
|
||
Debug.WriteLine((elapsed - _lastPositionTimeSpan).TotalMilliseconds, "-----------------------------------------------position packet");
|
||
_lastPositionTimeSpan = elapsed;
|
||
}
|
||
else
|
||
{
|
||
_positionStopwatch.Start();
|
||
}
|
||
}
|
||
#endif
|
||
|
||
ushort checksum = MavlinkCRC.crc_calculate(_readBuffer, length - 2);
|
||
checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
|
||
iDataCount += 1;
|
||
|
||
#if DEBUG && LOG_SEQUENCE_NUMBER
|
||
_sequenceLog.Append('\t').Append(crcvalue == checksum).AppendLine();
|
||
#endif
|
||
|
||
if (crcvalue == checksum)
|
||
{
|
||
return _readBuffer;
|
||
}
|
||
else
|
||
{
|
||
#if DEBUG && LOG_PACKETS
|
||
_log.AppendLine("crc not passed");
|
||
#endif
|
||
|
||
iErrorData += 1;
|
||
var handler = ErrorDataEvent;
|
||
if (handler != null)
|
||
{
|
||
RunOnUIThread(() => handler(iErrorData, iDataCount, _readBuffer[5]));
|
||
}
|
||
return null;
|
||
}
|
||
}
|
||
|
||
public MAVState MAV = new MAVState();
|
||
|
||
/// <summary>
|
||
/// Used to extract mission from log file - both sent or received
|
||
/// </summary>
|
||
/// <param name="buffer">packet</param>
|
||
void getWPsfromstream(ref byte[] buffer)
|
||
{
|
||
if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT)
|
||
{
|
||
// clear old
|
||
MAV.wps.Clear();
|
||
}
|
||
|
||
if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
|
||
{
|
||
MAVLink.mavlink_mission_item_t wp = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_item_t>(6);
|
||
|
||
if (wp.current == 2)
|
||
{
|
||
// guide mode wp
|
||
MAV.GuidedMode = wp;
|
||
}
|
||
else
|
||
{
|
||
MAV.wps[wp.seq] = wp;
|
||
}
|
||
|
||
//Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
|
||
}
|
||
|
||
if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_RALLY_POINT)
|
||
{
|
||
MAVLink.mavlink_rally_point_t rallypt = buffer.ByteArrayToStructure<MAVLink.mavlink_rally_point_t>(6);
|
||
|
||
MAV.rallypoints[rallypt.idx] = rallypt;
|
||
|
||
//Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt);
|
||
}
|
||
|
||
if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_FENCE_POINT)
|
||
{
|
||
MAVLink.mavlink_fence_point_t fencept = buffer.ByteArrayToStructure<MAVLink.mavlink_fence_point_t>(6);
|
||
|
||
MAV.fencepoints[fencept.idx] = fencept;
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// used for outbound packet sending
|
||
/// </summary>
|
||
public int packetcount = 0;
|
||
private bool SendReq;
|
||
#pragma warning disable CS0414 // The field is assigned but its value is never used
|
||
private bool giveComport;
|
||
#pragma warning restore CS0414 // The field is assigned but its value is never used
|
||
|
||
public async Task GeneratePacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
|
||
{
|
||
byte[] data;
|
||
|
||
if (mavlinkversion == 3)
|
||
{
|
||
data = MavlinkUtil.StructureToByteArray(indata);
|
||
}
|
||
else
|
||
{
|
||
data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
|
||
}
|
||
// data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
|
||
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
|
||
byte[] packet = new byte[data.Length + 6 + 2];
|
||
|
||
if (mavlinkversion == 3 || mavlinkversion == 0)
|
||
{
|
||
packet[0] = MAVLink.MAVLINK_STX;
|
||
}
|
||
else if (mavlinkversion == 2)
|
||
{
|
||
packet[0] = (byte)'U';
|
||
}
|
||
packet[1] = (byte)(data.Length);
|
||
packet[2] = (byte)packetcount;
|
||
//packet[2] = 35;
|
||
packetcount++;
|
||
|
||
packet[3] = 255; // this is always 255 - MYGCS
|
||
|
||
packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
|
||
|
||
packet[5] = messageType;
|
||
|
||
int i = 6;
|
||
foreach (byte b in data)
|
||
{
|
||
packet[i] = b;
|
||
i++;
|
||
}
|
||
|
||
ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
|
||
|
||
if (mavlinkversion == 3 || mavlinkversion == 0)
|
||
{
|
||
checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
|
||
}
|
||
|
||
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
|
||
byte ck_b = (byte)(checksum >> 8); ///< Low byte
|
||
|
||
packet[i] = ck_a;
|
||
i += 1;
|
||
packet[i] = ck_b;
|
||
i += 1;
|
||
|
||
if (IsConnected)
|
||
{
|
||
try
|
||
{
|
||
await Connection.WriteAsync(packet, 0, i);
|
||
// Console.WriteLine("sendout:" + i);
|
||
_sendnumber += i; //统计发送数量
|
||
}
|
||
catch
|
||
{
|
||
// 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
public async Task GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata,int rtklength)
|
||
{
|
||
byte[] data;
|
||
int rtkdataleng= rtklength + 3;
|
||
if (mavlinkversion == 3)
|
||
{
|
||
data = MavlinkUtil.StructureToByteArray(indata);
|
||
}
|
||
else
|
||
{
|
||
data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
|
||
}
|
||
// data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
|
||
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
|
||
|
||
byte[] packet = new byte[rtkdataleng + 6 + 2];
|
||
|
||
if (mavlinkversion == 3 || mavlinkversion == 0)
|
||
{
|
||
packet[0] = MAVLink.MAVLINK_STX;
|
||
}
|
||
else if (mavlinkversion == 2)
|
||
{
|
||
packet[0] = (byte)'U';
|
||
}
|
||
packet[1] = (byte)(rtkdataleng);
|
||
packet[2] = (byte)packetcount;
|
||
//packet[2] = 35;
|
||
packetcount++;
|
||
|
||
packet[3] = 255; // this is always 255 - MYGCS
|
||
|
||
packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
|
||
|
||
packet[5] = messageType;
|
||
|
||
int i = 6;
|
||
foreach (byte b in data)
|
||
{
|
||
if (i < rtkdataleng + 6 )
|
||
packet[i] = b;
|
||
else break;
|
||
i++;
|
||
}
|
||
|
||
ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
|
||
|
||
if (mavlinkversion == 3 || mavlinkversion == 0)
|
||
{
|
||
checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
|
||
}
|
||
|
||
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
|
||
byte ck_b = (byte)(checksum >> 8); ///< Low byte
|
||
|
||
packet[i] = ck_a;
|
||
i += 1;
|
||
packet[i] = ck_b;
|
||
i += 1;
|
||
|
||
if (IsConnected)
|
||
{
|
||
try
|
||
{
|
||
await Connection.WriteAsync(packet, 0, i);
|
||
//Console.WriteLine("sendout:" + i);
|
||
_sendnumber += i; //统计发送数量
|
||
}
|
||
catch
|
||
{
|
||
// 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
|
||
}
|
||
}
|
||
}
|
||
|
||
public async Task<bool> DoLEDAsync()
|
||
{
|
||
return await DoLEDCommandAsync();
|
||
}
|
||
|
||
public async Task<bool> DoLEDCommandAsync()
|
||
{
|
||
giveComport = true;
|
||
MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
|
||
led.target_system = sysid;
|
||
led.target_component = compid;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
|
||
led.instance = 0;
|
||
led.pattern = 0;
|
||
led.custom_len = 4;
|
||
led.custom_bytes = new byte[24];
|
||
led.custom_bytes[0] = 255;
|
||
led.custom_bytes[1] = 255;
|
||
led.custom_bytes[2] = 255;
|
||
led.custom_bytes[3] = 2;
|
||
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
|
||
return true;
|
||
}
|
||
|
||
public async Task<bool> DoARMAsync(bool armit)
|
||
{
|
||
return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
|
||
}
|
||
|
||
public async Task<bool> DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
|
||
{
|
||
return await DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
|
||
}
|
||
|
||
public async Task<bool> DoTakeoffAsync()
|
||
{
|
||
///<summary> Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| </summary>
|
||
return await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, -1, 0, 0, 15);
|
||
}
|
||
public async Task<bool> DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
|
||
{
|
||
///<summary> Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| </summary>
|
||
return await DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0);
|
||
}
|
||
|
||
|
||
public async Task<bool> DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
|
||
{
|
||
giveComport = true;
|
||
//byte[] buffer;
|
||
|
||
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
|
||
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
|
||
if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
|
||
{
|
||
req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
|
||
}
|
||
|
||
req.command = (ushort)actionid;
|
||
|
||
|
||
Console.WriteLine("P1 = " + p1);
|
||
req.param1 = p1;
|
||
req.param2 = p2;
|
||
req.param3 = p3;
|
||
req.param4 = p4;
|
||
req.param5 = p5;
|
||
req.param6 = p6;
|
||
req.param7 = p7;
|
||
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
|
||
return true;
|
||
|
||
//DateTime start = DateTime.Now;
|
||
//int retrys = 3;
|
||
|
||
//int timeout = 2000;
|
||
|
||
////// imu calib take a little while
|
||
//if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1)
|
||
//{
|
||
// // this is for advanced accel offsets, and blocks execution
|
||
// return true;
|
||
//}
|
||
//else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION)
|
||
//{
|
||
// retrys = 1;
|
||
// timeout = 25000;
|
||
//}
|
||
//else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN)
|
||
//{
|
||
// await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
|
||
// giveComport = false;
|
||
// return true;
|
||
//}
|
||
//else if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
|
||
//{
|
||
// // 10 seconds as may need an imu calib
|
||
// timeout = 10000;
|
||
//}
|
||
|
||
//while (true)
|
||
//{
|
||
// if (!(start.AddMilliseconds(timeout) > DateTime.Now))
|
||
// {
|
||
// if (retrys > 0)
|
||
// {
|
||
// //log.Info("doAction Retry " + retrys);
|
||
// await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req);
|
||
// start = DateTime.Now;
|
||
// retrys--;
|
||
// continue;
|
||
// }
|
||
// giveComport = false;
|
||
// throw new Exception("Timeout on read - doAction");
|
||
// }
|
||
|
||
// buffer = readPacket();
|
||
// if (buffer.Length > 5)
|
||
// {
|
||
// if (buffer[5] == MAVLink.MAVLINK_MSG_ID_COMMAND_ACK)
|
||
// {
|
||
// var ack = buffer.ByteArrayToStructure<MAVLink.mavlink_command_ack_t>(6);
|
||
// if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
|
||
// {
|
||
// giveComport = false;
|
||
// return true;
|
||
// }
|
||
// else
|
||
// {
|
||
// giveComport = false;
|
||
// return false;
|
||
// }
|
||
// }
|
||
// }
|
||
//}
|
||
}
|
||
|
||
|
||
public double GetBearing(float lat1,float lng1,float lat2,float lng2)
|
||
{
|
||
var latitude1 = Math.PI / 180*lat1;
|
||
var latitude2 = Math.PI / 180 * lat2;
|
||
var longitudeDifference = Math.PI / 180*(lng2 - lng1);
|
||
|
||
var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2);
|
||
var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference);
|
||
|
||
return (180 / Math.PI*(Math.Atan2(y, x)) + 360) % 360;
|
||
}
|
||
|
||
|
||
public async Task SetGuidedModeWPAsync(Locationwp gotohere)
|
||
{
|
||
//if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
|
||
// return;
|
||
|
||
//giveComport = true;
|
||
|
||
//try
|
||
//{
|
||
//gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
|
||
//老方法无法设置速度
|
||
// await SetGuidedWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
|
||
//}
|
||
//catch { }
|
||
|
||
//giveComport = false;
|
||
|
||
|
||
//老方法无法设置速度
|
||
await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
|
||
|
||
/*
|
||
TaskTools.HIL.Vector3 targetVelocity = new TaskTools.HIL.Vector3();
|
||
float targspeed = (float)2.0; //目标速度
|
||
double targetbearing= GetBearing((float)lat, (float)lng, (float)gotohere.lat, (float)gotohere.lng); //目标机头方向
|
||
targetVelocity.x = Math.Cos(targetbearing * 180 / Math.PI) * targspeed; //分解速度
|
||
targetVelocity.y = Math.Sin(targetbearing * 180 / Math.PI) * targspeed; //分解速度
|
||
// await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT,
|
||
// gotohere.lat, gotohere.lng, gotohere.alt, targetVelocity.x, targetVelocity.y, 2.0);
|
||
|
||
await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT,
|
||
gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0);
|
||
*/
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
public async Task setPositionTargetGlobalInt( bool pos, bool vel, bool acc, MAVLink.MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz)
|
||
{
|
||
MAVLink.mavlink_set_position_target_global_int_t target = new MAVLink.mavlink_set_position_target_global_int_t()
|
||
{
|
||
target_system = sysid,
|
||
target_component = compid,
|
||
alt = (float)alt,
|
||
lat_int = (int)(lat * 1e7),
|
||
lon_int = (int)(lng * 1e7),
|
||
coordinate_frame = (byte)frame,
|
||
vx = (float)vx,
|
||
vy = (float)vy,
|
||
vz = (float)vz
|
||
};
|
||
|
||
target.type_mask = 7 + 56 + 448;
|
||
|
||
if (pos)
|
||
target.type_mask -= 7;
|
||
if (vel)
|
||
target.type_mask -= 56;
|
||
if (acc)
|
||
target.type_mask -= 448;
|
||
|
||
await GeneratePacketAsync(MAVLink.SET_POSITION_TARGET_GLOBAL_INT, target);
|
||
|
||
}
|
||
|
||
|
||
public async Task SetNewWPAltAsync(Locationwp gotohere)
|
||
{
|
||
//givecomport = true;
|
||
|
||
//try
|
||
//{
|
||
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
|
||
|
||
//MAVLink.MAV_MISSION_RESULT ans =
|
||
await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3);
|
||
|
||
// if (ans != mavlink.mav_mission_result.mav_mission_accepted)
|
||
// throw new exception("alt change failed");
|
||
//}
|
||
//catch { givecomport = false; /*log.error(ex);*/ throw; }
|
||
|
||
//givecomport = false;
|
||
}
|
||
|
||
public async Task SetWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0)
|
||
{
|
||
giveComport = true;
|
||
MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();
|
||
|
||
req.target_system = sysid;
|
||
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
|
||
|
||
req.command = loc.id;
|
||
|
||
|
||
req.current = current;
|
||
|
||
req.frame = (byte)frame;
|
||
|
||
req.x = (float)(loc.lat);
|
||
req.y = (float)(loc.lng);
|
||
req.z = (float)(loc.alt);
|
||
|
||
req.param1 = loc.p1;
|
||
req.param2 = loc.p2;
|
||
req.param3 = loc.p3;
|
||
req.param4 = loc.p4;
|
||
|
||
req.seq = index;
|
||
|
||
//log.InfoFormat("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
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req);
|
||
}
|
||
|
||
public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
|
||
{
|
||
//某些情况下ch4设置成0会导致自旋
|
||
ushort checkyaw = ch4 ?? ch4in;
|
||
if ((checkyaw < 1200)|| (checkyaw > 1800))
|
||
checkyaw = 1500;
|
||
////
|
||
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
|
||
rc.target_component = compid;
|
||
rc.target_system = sysid;
|
||
rc.chan1_raw = ch1 ?? ch1in;
|
||
rc.chan2_raw = ch2 ?? ch2in;
|
||
rc.chan3_raw = ch3 ?? ch3in;
|
||
rc.chan4_raw = checkyaw;
|
||
rc.chan5_raw = ch5 ?? ch5in;
|
||
rc.chan6_raw = ch6 ?? ch6in;
|
||
rc.chan7_raw = ch7 ?? ch7in;
|
||
rc.chan8_raw = ch8 ?? ch8in;
|
||
await SendPacketAsync(rc);
|
||
}
|
||
|
||
public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null, float? alt = null)
|
||
{
|
||
/* MAVLink.mavlink_rc_mobile_control_t rc = new MAVLink.mavlink_rc_mobile_control_t();
|
||
rc.target_component = compid;
|
||
rc.target_system = sysid;
|
||
rc.chan1_raw = ch1 ?? ch1in;
|
||
rc.chan2_raw = ch2 ?? ch2in;
|
||
rc.chan3_raw = ch3 ?? ch3in;
|
||
rc.chan4_raw = ch4 ?? ch4in;
|
||
rc.chan5_raw = ch5 ?? ch5in;
|
||
rc.chan6_raw = ch6 ?? ch6in;
|
||
rc.chan7_raw = ch7 ?? ch7in;
|
||
rc.chan8_raw = ch8 ?? ch8in;
|
||
rc.Yaw = yaw ?? this.yaw;
|
||
rc.alt = alt ?? this.alt;
|
||
await SendPacketAsync(rc);
|
||
*/
|
||
float ch4yaw;
|
||
ch4yaw = yaw ?? this.yaw;
|
||
if (ch4yaw != this.yaw)
|
||
{
|
||
ch4yaw = ch4yaw % 360;
|
||
await DoCommandAsync(MAVLink.MAV_CMD.CONDITION_YAW, ch4yaw, 0, 1, 0, 0, 0, 0);
|
||
}
|
||
|
||
/*
|
||
float ch4yaw;
|
||
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
|
||
rc.target_component = compid;
|
||
rc.target_system = sysid;
|
||
rc.chan1_raw = ch1 ?? ch1in;
|
||
rc.chan2_raw = ch2 ?? ch2in;
|
||
rc.chan3_raw = ch3 ?? ch3in;
|
||
rc.chan4_raw = 1500;
|
||
rc.chan5_raw = ch5 ?? ch5in;
|
||
rc.chan6_raw = ch6 ?? ch6in;
|
||
rc.chan7_raw = ch7 ?? ch7in;
|
||
rc.chan8_raw = ch8 ?? ch8in;
|
||
ch4yaw= yaw ?? this.yaw;
|
||
rc.chan4_raw =(ushort)ch4yaw;
|
||
await SendPacketAsync(rc);
|
||
*/
|
||
|
||
}
|
||
|
||
public async Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
|
||
{
|
||
if (param.ContainsKey(paramName)) param[paramName] = null;
|
||
var paramId = Encoding.ASCII.GetBytes(paramName);
|
||
Array.Resize(ref paramId, 16);
|
||
var req = new MAVLink.mavlink_param_request_read_t
|
||
{
|
||
param_id = paramId,
|
||
param_index = -1,
|
||
target_component = compid,
|
||
target_system = sysid
|
||
};
|
||
var stopwatch = Stopwatch.StartNew();
|
||
while (true)
|
||
{
|
||
for (int ii = 0; ii < 5; ii++)
|
||
{
|
||
await SendPacketAsync(req);
|
||
await Task.Delay(10).ConfigureAwait(false);
|
||
}
|
||
|
||
int i = 0;
|
||
for (; i < 5 && (!param.ContainsKey(paramName) || param[paramName] == null); i++)
|
||
{
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
if (i < 5)
|
||
{
|
||
stopwatch.Stop();
|
||
break;
|
||
}
|
||
else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout)
|
||
{
|
||
stopwatch.Stop();
|
||
throw new TimeoutException("The GetParamAsync operation has timed out.");
|
||
}
|
||
}
|
||
return param[paramName].Value;
|
||
}
|
||
|
||
public async Task SendPacketAsync<TMavlinkPacket>(TMavlinkPacket indata)
|
||
{
|
||
bool validPacket = false;
|
||
byte a = 0;
|
||
var packetType = indata.GetType();
|
||
foreach (Type ty in MAVLink.MAVLINK_MESSAGE_INFO)
|
||
{
|
||
if (ty == packetType)
|
||
{
|
||
validPacket = true;
|
||
await GeneratePacketAsync(a, indata);
|
||
return;
|
||
}
|
||
a++;
|
||
}
|
||
if (!validPacket)
|
||
{
|
||
RaiseMessageCreated("The packet type is not valid.");
|
||
//log.Info("Mavlink : NOT VALID PACKET await SendPacketAsync() " + indata.GetType().ToString());
|
||
}
|
||
}
|
||
|
||
public double[] packetspersecond { get; set; }
|
||
DateTime[] packetspersecondbuild = new DateTime[256];
|
||
|
||
byte ratesensors = 1;
|
||
public byte RateSensors { get { return ratesensors; } set { ratesensors = value; } }
|
||
|
||
private DateTime heatbeatSend;
|
||
private bool bInitChannel = false;
|
||
private bool bGPSBadHealth;
|
||
private bool bGyroBadHealth;
|
||
private bool bAccel;
|
||
private bool bCompass;
|
||
private bool bBarometer;
|
||
|
||
public short gx;
|
||
public short gy;
|
||
public short gz;
|
||
public short ax;
|
||
public short ay;
|
||
public short az;
|
||
public short mx;
|
||
public short my;
|
||
public short mz;
|
||
|
||
public short gx2;
|
||
public short gy2;
|
||
public short gz2;
|
||
public short ax2;
|
||
public short ay2;
|
||
public short az2;
|
||
public short mx2;
|
||
public short my2;
|
||
public short mz2;
|
||
|
||
public float mag_ofs_x;
|
||
public float mag_ofs_y;
|
||
public float mag_ofs_z;
|
||
public float mag_declination;
|
||
public int raw_press;
|
||
public int raw_temp;
|
||
public float gyro_cal_x;
|
||
public float gyro_cal_y;
|
||
public float gyro_cal_z;
|
||
public float accel_cal_x;
|
||
public float accel_cal_y;
|
||
public float accel_cal_z;
|
||
|
||
public ushort param_total;
|
||
public ushort WpCount;
|
||
|
||
public async Task RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
|
||
{
|
||
|
||
double pps = 0;
|
||
|
||
switch (id)
|
||
{
|
||
case MAVLink.MAV_DATA_STREAM.ALL:
|
||
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA1:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA2:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA3:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.POSITION:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
}
|
||
|
||
//packetspersecond[temp[5]];
|
||
|
||
if (pps == 0 && hzrate == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
|
||
await GetDatastreamAsync(id, hzrate);
|
||
}
|
||
|
||
public async Task StopDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
|
||
{
|
||
|
||
double pps = 0;
|
||
|
||
switch (id)
|
||
{
|
||
case MAVLink.MAV_DATA_STREAM.ALL:
|
||
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA1:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA2:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.EXTRA3:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.POSITION:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
//case MAVLink.MAV_DATA_STREAM.SCALED_IMU2:
|
||
|
||
// break;
|
||
case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
|
||
if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
|
||
break;
|
||
pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
|
||
if (hzratecheck(pps, hzrate))
|
||
{
|
||
return;
|
||
}
|
||
break;
|
||
}
|
||
//log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
|
||
await StopGetDatastreamAsync(id, hzrate);
|
||
}
|
||
|
||
bool hzratecheck(double pps, int hzrate)
|
||
{
|
||
|
||
if (hzrate == 0 && pps == 0)
|
||
{
|
||
return true;
|
||
}
|
||
else if (hzrate == 1 && pps >= 0.5 && pps <= 2)
|
||
{
|
||
return true;
|
||
}
|
||
else if (hzrate == 3 && pps >= 2 && hzrate < 5)
|
||
{
|
||
return true;
|
||
}
|
||
else if (hzrate == 10 && pps > 5 && hzrate < 15)
|
||
{
|
||
return true;
|
||
}
|
||
else if (hzrate > 15 && pps > 15)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
|
||
}
|
||
|
||
async Task GetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
|
||
{
|
||
MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
|
||
req.req_message_rate = hzrate;
|
||
req.start_stop = 1; // start
|
||
req.req_stream_id = (byte)id; // id
|
||
|
||
|
||
// send each one twice.
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
||
}
|
||
public void GetCommunicationNumber(out int recnumber, out int sendnumber)
|
||
{
|
||
recnumber=_recnumber;
|
||
sendnumber = _sendnumber;
|
||
}
|
||
|
||
|
||
//重设数据量
|
||
public void ResetCommunicationNumber()
|
||
{
|
||
_recnumber = 0;
|
||
_sendnumber = 0;
|
||
}
|
||
async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
|
||
{
|
||
MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
|
||
req.req_message_rate = 0;
|
||
req.start_stop = 0; // start
|
||
req.req_stream_id = (byte)id; // id
|
||
|
||
// send each one twice.
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
||
}
|
||
|
||
public async Task SetParamAsync(string paramname, float value)
|
||
{
|
||
// param type is set here, however it is always sent over the air as a float 100int = 100f.
|
||
var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = (byte)param_types[paramname] };
|
||
|
||
byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);
|
||
|
||
Array.Resize(ref temp, 16);
|
||
req.param_id = temp;
|
||
req.param_value = (value);
|
||
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
|
||
}
|
||
|
||
public async Task SetParam2Async(string paramname, float value)
|
||
{
|
||
// param type is set here, however it is always sent over the air as a float 100int = 100f.
|
||
var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = 4 };
|
||
|
||
byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);
|
||
|
||
Array.Resize(ref temp, 16);
|
||
req.param_id = temp;
|
||
req.param_value = (value);
|
||
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
|
||
}
|
||
|
||
public async Task SetModeAsync(ac2modes modein)
|
||
{
|
||
try
|
||
{
|
||
MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
|
||
|
||
if (TranslateMode(modein, ref mode))
|
||
{
|
||
await SetModeAsync(mode);
|
||
}
|
||
}
|
||
catch { RaiseMessageCreated("Failed to change Modes"); }
|
||
}
|
||
|
||
public async Task SetModeAsync(string modein)
|
||
{
|
||
try
|
||
{
|
||
MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
|
||
|
||
if (TranslateMode(modein, ref mode))
|
||
{
|
||
await SetModeAsync(mode);
|
||
}
|
||
}
|
||
catch { RaiseMessageCreated("Failed to change Modes"); }
|
||
}
|
||
|
||
public async Task SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
|
||
{
|
||
mode.base_mode |= (byte)base_mode;
|
||
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
|
||
await Task.Delay(10);
|
||
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
|
||
}
|
||
|
||
|
||
|
||
|
||
public async Task InjectGpsDataAsync(byte[] data, ushort length)
|
||
{
|
||
MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
|
||
var msglen = 110;
|
||
int datalen = 0;
|
||
var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
|
||
for (int a = 0; a < len; a++)
|
||
{
|
||
datalen = Math.Min(length - a * msglen, msglen);
|
||
gps.data = new byte[msglen];
|
||
Array.Copy(data, a * msglen, gps.data, 0, datalen);
|
||
gps.len = (byte)datalen;
|
||
gps.target_component = compid;
|
||
gps.target_system = sysid;
|
||
await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
|
||
// Console.WriteLine("send:" + (ushort)gps.len);
|
||
}
|
||
}
|
||
|
||
public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
|
||
{
|
||
//MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();
|
||
mode.target_system = sysid;
|
||
|
||
switch (modein)
|
||
{
|
||
case ac2modes.STABILIZE:
|
||
case ac2modes.AUTO:
|
||
case ac2modes.RTL:
|
||
case ac2modes.LOITER:
|
||
case ac2modes.ACRO:
|
||
case ac2modes.ALT_HOLD:
|
||
case ac2modes.CIRCLE:
|
||
case ac2modes.POSITION:
|
||
case ac2modes.LAND:
|
||
case ac2modes.OF_LOITER:
|
||
case ac2modes.GUIDED:
|
||
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
|
||
mode.custom_mode = (uint)modein;
|
||
break;
|
||
default:
|
||
RaiseMessageCreated("错误模式: " + modein);
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
public bool TranslateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
|
||
{
|
||
//MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();
|
||
mode.target_system = sysid;
|
||
|
||
var modeValue = EnumTranslator.GetValue<ac2modes>(modein.ToUpper());
|
||
switch (modeValue)
|
||
{
|
||
case (int)ac2modes.STABILIZE:
|
||
case (int)ac2modes.AUTO:
|
||
case (int)ac2modes.RTL:
|
||
case (int)ac2modes.LOITER:
|
||
case (int)ac2modes.ACRO:
|
||
case (int)ac2modes.ALT_HOLD:
|
||
case (int)ac2modes.CIRCLE:
|
||
case (int)ac2modes.POSITION:
|
||
case (int)ac2modes.LAND:
|
||
case (int)ac2modes.OF_LOITER:
|
||
case (int)ac2modes.GUIDED:
|
||
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
|
||
mode.custom_mode = (uint)modeValue;
|
||
break;
|
||
default:
|
||
RaiseMessageCreated("错误模式: " + modein);
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
public async Task GetWPsAsync()
|
||
{
|
||
MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t();
|
||
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Gets specfied WP
|
||
/// </summary>
|
||
/// <param name="index"></param>
|
||
/// <returns>WP</returns>
|
||
public async Task GetWPAsync(ushort index)
|
||
{
|
||
giveComport = true;
|
||
MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t();
|
||
req.target_system = sysid;
|
||
req.target_component = compid;
|
||
req.seq = index;
|
||
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
|
||
// request
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST, req);
|
||
}
|
||
|
||
public async Task SetWPTotalAsync(ushort wp_total)
|
||
{
|
||
giveComport = true;
|
||
MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t();
|
||
|
||
req.target_system = sysid;
|
||
req.target_component = compid; // MSG_NAMES.MISSION_COUNT
|
||
|
||
req.count = wp_total;
|
||
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT, req);
|
||
}
|
||
|
||
/// <param name="loc">任务信息。</param>
|
||
/// <param name="index">任务编号。</param>
|
||
/// <param name="frame">坐标系。</param>
|
||
/// <param name="current">0：在 Auto 模式下飞往指定位置；2：切到 GUIDED 模式，立即飞往指定位置；3：仅改变高度，立即执行。</param>
|
||
/// <param name="autocontinue">指示是否自动继续执行下一个任务。</param>
|
||
public async Task WriteWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
|
||
{
|
||
MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();
|
||
|
||
req.target_system = sysid;
|
||
req.target_component = compid; // MSG_NAMES.MISSION_ITEM
|
||
|
||
req.command = loc.id;
|
||
|
||
req.current = current;
|
||
req.autocontinue = autocontinue;
|
||
|
||
req.frame = (byte)frame;
|
||
req.y = (float)(loc.lng);
|
||
req.x = (float)(loc.lat);
|
||
req.z = (float)(loc.alt);
|
||
|
||
req.param1 = loc.p1;
|
||
req.param2 = loc.p2;
|
||
req.param3 = loc.p3;
|
||
req.param4 = loc.p4;
|
||
|
||
req.seq = index;
|
||
|
||
//log.InfoFormat("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
|
||
await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req);
|
||
}
|
||
|
||
const float rad2deg = (float)(180 / Math.PI);
|
||
const float deg2rad = (float)(1.0 / rad2deg);
|
||
|
||
public async Task<bool> SetSensorOffsetsAsync(sensoroffsetsenum sensor, float x, float y, float z)
|
||
{
|
||
return await DoCommandAsync(Plane.Protocols.MAVLink.MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int)sensor, x, y, z, 0, 0, 0);
|
||
}
|
||
|
||
public byte rssi;
|
||
|
||
|
||
private void RaiseMessageCreated(string message)
|
||
{
|
||
var handler = MessageCreated;
|
||
if (handler != null)
|
||
{
|
||
var e = new MessageCreatedEventArgs { Message = message };
|
||
RunOnUIThread(() =>
|
||
{
|
||
handler(this, e);
|
||
});
|
||
}
|
||
}
|
||
|
||
private void RunOnUIThread(Action action)
|
||
{
|
||
if (SynchronizationContext.Current == _uiSyncContext)
|
||
{
|
||
action();
|
||
}
|
||
else
|
||
{
|
||
_uiSyncContext.Post(delegate { action(); }, null);
|
||
}
|
||
}
|
||
|
||
private void RaiseEventOnUIThread(EventHandler ev)
|
||
{
|
||
if (ev != null)
|
||
{
|
||
RunOnUIThread(() => ev(this, EventArgs.Empty));
|
||
}
|
||
}
|
||
|
||
private void RaiseEventOnUIThread<TEventArgs>(EventHandler<TEventArgs> ev, TEventArgs e)
|
||
{
|
||
if (ev != null)
|
||
{
|
||
RunOnUIThread(() => ev(this, e));
|
||
}
|
||
}
|
||
}
|
||
}
|