805 lines
30 KiB
C#
805 lines
30 KiB
C#
using Plane.Copters;
|
||
using Plane.Protocols;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using static Plane.Copters.PlaneCopter;
|
||
|
||
namespace Plane.CommunicationManagement
|
||
{
|
||
public partial class CommModuleManager
|
||
{
|
||
public int packetcount = 0;
|
||
|
||
|
||
/// <summary>
|
||
/// 设置通道
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
/// <param name="ch1"></param>
|
||
/// <param name="ch2"></param>
|
||
/// <param name="ch3"></param>
|
||
/// <param name="ch4"></param>
|
||
/// <returns></returns>
|
||
public async Task SetChannelsAsync(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = SetChannels(ch1, ch2, ch3, ch4);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
|
||
{
|
||
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
|
||
rc.target_component = 1;
|
||
rc.target_system = 1;
|
||
rc.chan1_raw = ch1 ?? 1500;
|
||
rc.chan2_raw = ch2 ?? 1500;
|
||
rc.chan3_raw = ch3 ?? 1500;
|
||
rc.chan4_raw = ch4 ?? 1500;
|
||
rc.chan5_raw = 1500;
|
||
rc.chan6_raw = 1500;
|
||
rc.chan7_raw = 1500;
|
||
rc.chan8_raw = 1500;
|
||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置飞机上通信模块的回传功率
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
public async Task SetCopterCommBackPower(short id, byte power)
|
||
{
|
||
byte[] data = new byte[12];
|
||
short num = (0x5 << 12) ^ 1;
|
||
byte frequencyPoint = 16;
|
||
int frequencyValue = 0x50;
|
||
|
||
Array.Copy(BitConverter.GetBytes(num), 0, data, 0, 2); //广播个数
|
||
Array.Copy(BitConverter.GetBytes(id), 0, data, 2, 2); //小批量 当前只对1个采用小批量模式广播
|
||
data[4] = 0x00;
|
||
data[5] = 0xFC;
|
||
data[6] = power;
|
||
data[7] = frequencyPoint;
|
||
Array.Copy(BitConverter.GetBytes(frequencyValue), 0, data, 8, 4);
|
||
|
||
await WriteCommPacketAsync(0, MavComm.COMM_SET_BACK_POWER, data).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task GetCurr_Sys_Parm()
|
||
{
|
||
configurationData = null;
|
||
configData1 = null;
|
||
configData2 = null;
|
||
|
||
byte[] data = new byte[2];
|
||
data = BitConverter.GetBytes((short)0x0000);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_GET_CURR_SYS_PARM, data).ConfigureAwait(false);
|
||
|
||
await Task.Delay(100);
|
||
|
||
data = BitConverter.GetBytes((short)0x0001);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_GET_CURR_SYS_PARM, data).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task SetCurr_Sys_Parm()
|
||
{
|
||
if (configurationData == null) return;
|
||
|
||
byte[] data = new byte[configurationData.Length + 2];
|
||
|
||
Array.Copy(BitConverter.GetBytes((short)configurationData.Length), data, 2);
|
||
Array.Copy(configurationData,0, data, 2, configurationData.Length);
|
||
|
||
await WriteCommPacketAsync(0, MavComm.COMM_SET_CURR_SYS_PARM, data).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task SetCommCount(short count, short startNum)
|
||
{
|
||
byte[] data = new byte[4];
|
||
Array.Copy(BitConverter.GetBytes(count), 0, data, 0, 2);
|
||
Array.Copy(BitConverter.GetBytes(startNum), 0, data, 2, 2);
|
||
|
||
await WriteCommPacketAsync(0, MavComm.COMM_SET_MAV_COUNT, data).ConfigureAwait(false);
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 写入航点
|
||
/// </summary>
|
||
/// <param name="id">飞机ID</param>
|
||
/// <param name="missions">航点列表</param>
|
||
/// <returns></returns>
|
||
public async Task<bool> WriteMissionListAsync(short id, List<IMission> missions)
|
||
{
|
||
missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION);
|
||
List<MAVLink.mavlink_mission_item_t> mission_list = new List<MAVLink.mavlink_mission_item_t>();
|
||
foreach (IMission mission in missions)
|
||
{
|
||
var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
|
||
|
||
var req = new MAVLink.mavlink_mission_item_t();
|
||
|
||
req.target_system = 1;
|
||
req.target_component = 1;
|
||
|
||
req.command = (byte)mission.Command;
|
||
|
||
req.current = 0;
|
||
req.autocontinue = 1;
|
||
|
||
req.frame = (byte)frame;
|
||
if (mission.Command == FlightCommand.LEDControl)
|
||
{
|
||
req.x = mission.R;
|
||
req.y = mission.G;
|
||
req.z = mission.B;
|
||
}
|
||
else
|
||
{
|
||
req.x = (float)mission.Latitude;
|
||
req.y = (float)mission.Longitude;
|
||
req.z = mission.Altitude;
|
||
}
|
||
|
||
req.param1 = mission.Param1;
|
||
req.param2 = mission.Param2;
|
||
req.param3 = mission.Param3;
|
||
req.param4 = mission.Param4;
|
||
|
||
req.seq = mission.Sequence;
|
||
mission_list.Add(req);
|
||
}
|
||
|
||
bool result = await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray());
|
||
|
||
lock(MissinLocker)
|
||
{
|
||
if (missionWriteState.ContainsKey(id))
|
||
{
|
||
missionWriteState[id].StateReturn = MissionErrorId;
|
||
missionWriteState[id].ErrorCode = ErrorCode;
|
||
missionWriteState[id].SendAchieved = result;
|
||
missionWriteState[id].WriteSucceed = false;
|
||
}
|
||
else
|
||
{
|
||
CommWriteMissinState state = new CommWriteMissinState(result);
|
||
state.StateReturn = MissionErrorId;
|
||
state.ErrorCode = ErrorCode;
|
||
missionWriteState.Add(id, state);
|
||
}
|
||
return result;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 开始任务
|
||
/// </summary>
|
||
public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
|
||
{
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 暂停任务
|
||
/// </summary>
|
||
public async Task DoMissionPauseAsync()
|
||
{
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 恢复任务
|
||
/// </summary>
|
||
public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc)
|
||
{
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0 , 2, 0);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解锁
|
||
/// </summary>
|
||
/// <param name="armit"></param>
|
||
/// <returns></returns>
|
||
public async Task UnlockAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters,out copterId, out batchPacket);
|
||
|
||
byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
|
||
|
||
byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD);
|
||
|
||
byte[] packet3 = DoARMAsync(true);
|
||
|
||
byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 返航偏移
|
||
/// </summary>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task RLTOffsetAsync(float lat, float lng, IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] data = DoCommandAsync(MAVLink.MAV_CMD.DO_SET_HOME, 2, 0, 0, lat, lng, 0, 0);
|
||
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// LED闪烁白灯
|
||
/// </summary>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public void LED_FlickerAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
Task.Run(async () =>
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = DoLEDCommandAsync();
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
});
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 设置参数
|
||
/// </summary>
|
||
/// <param name="paramname"></param>
|
||
/// <param name="value"></param>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task<int> SetParamAsync(string paramname, float value, IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = SetParam2Async(paramname, value);
|
||
int packetNum = packet[2];
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
return packetNum;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 读取参数
|
||
/// </summary>
|
||
/// <param name="paramname"></param>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task ReadParamAsnyc(string paramname, IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = GetParam2Async(paramname);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 起飞测试
|
||
/// </summary>
|
||
/// <param name="alt"></param>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task TakeOffAsync(float alt, IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet1 = SetModeAsync(FlightMode.GUIDED);
|
||
|
||
byte[] packet2 = SetChannels(1500, 1500, 1500, 1500);
|
||
|
||
byte[] packet3 = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt);
|
||
|
||
byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 返航
|
||
/// </summary>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task ReturnToLaunchAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = SetModeAsync(FlightMode.RTL);
|
||
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 获取版本
|
||
/// </summary>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task GetVersionsAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
|
||
req.version = 0;
|
||
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
|
||
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
/// <summary>
|
||
/// 降落
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
public async Task LandAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = SetModeAsync(FlightMode.LAND);
|
||
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 加锁
|
||
/// </summary>
|
||
/// <param name="armit"></param>
|
||
/// <returns></returns>
|
||
public async Task LockAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
|
||
|
||
byte[] packet2 = DoARMAsync(false);
|
||
|
||
byte[] data = packet1.Concat(packet2).ToArray();
|
||
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 测试电机
|
||
/// </summary>
|
||
/// <param name="motor"></param>
|
||
/// <returns></returns>
|
||
public async Task MotorTestAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
//1-4代表1-4电机
|
||
byte[] data1 = DoMotorTestAsync(1, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
|
||
byte[] data2 = DoMotorTestAsync(2, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
|
||
byte[] data3 = DoMotorTestAsync(3, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
|
||
byte[] data4 = DoMotorTestAsync(4, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
|
||
|
||
byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray();
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 悬停
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
public async Task HoverAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet1 = SetChannels(1500, 1500, 1500, 1500);
|
||
|
||
byte[] packet2 = SetModeAsync(FlightMode.LOITER);
|
||
|
||
byte[] data = packet1.Concat(packet2).ToArray();
|
||
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 手动
|
||
/// </summary>
|
||
/// <param name="copters"></param>
|
||
/// <returns></returns>
|
||
public async Task FloatAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = SetModeAsync(FlightMode.ALT_HOLD);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校准加速计
|
||
/// </summary>
|
||
/// <param name="copterId"></param>
|
||
/// <returns></returns>
|
||
public async Task DoStartPreflightCompassAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校准加速计下一步
|
||
/// </summary>
|
||
/// <param name="copterId"></param>
|
||
/// <returns></returns>
|
||
public async Task DoNextPreflightCompassAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t();
|
||
req.command = 1;
|
||
req.result = 1;
|
||
|
||
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校准指南针
|
||
/// </summary>
|
||
/// <param name="copterId"></param>
|
||
/// <returns></returns>
|
||
public async Task DoCalibrationCompassAsync(IEnumerable<ICopter> copters = null)
|
||
{
|
||
short copterId = 0;
|
||
byte[] batchPacket = null;
|
||
GetCopterIds(copters, out copterId, out batchPacket);
|
||
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 放弃校准指南针
|
||
/// </summary>
|
||
/// <param name="copterId"></param>
|
||
/// <returns></returns>
|
||
public async Task DoCancelCalibrationCompassAsync(short copterId)
|
||
{
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task DoAcceptCalibrationCompassAsync(short copterId)
|
||
{
|
||
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
|
||
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
|
||
private byte[] SetModeAsync(FlightMode mode)
|
||
{
|
||
return SetModeAsync(mode.ToAC2Mode());
|
||
}
|
||
|
||
private byte[] DoARMAsync(bool armit)
|
||
{
|
||
return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
|
||
}
|
||
|
||
private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
|
||
{
|
||
return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
|
||
}
|
||
|
||
private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
|
||
{
|
||
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
|
||
|
||
req.target_system = 1;
|
||
req.target_component = 1;
|
||
if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
|
||
{
|
||
req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
|
||
}
|
||
req.command = (ushort)actionid;
|
||
req.param1 = p1;
|
||
req.param2 = p2;
|
||
req.param3 = p3;
|
||
req.param4 = p4;
|
||
req.param5 = p5;
|
||
req.param6 = p6;
|
||
req.param7 = p7;
|
||
|
||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ;
|
||
}
|
||
|
||
private byte[] GeneratePacket<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
|
||
{
|
||
byte[] data;
|
||
data = MavlinkUtil.StructureToByteArray(indata);
|
||
byte[] packet = new byte[data.Length + 6 + 2];
|
||
packet[0] = MAVLink.MAVLINK_STX;
|
||
packet[1] = (byte)(data.Length);
|
||
packet[2] = (byte)packetcount;
|
||
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);
|
||
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;
|
||
|
||
return packet;
|
||
}
|
||
|
||
|
||
private byte[] SetModeAsync(ac2modes modein)
|
||
{
|
||
try
|
||
{
|
||
MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
|
||
|
||
if (TranslateMode(modein, ref mode))
|
||
{
|
||
return SetModeAsync(mode);
|
||
}
|
||
}
|
||
catch { }
|
||
return null;
|
||
}
|
||
|
||
private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
|
||
{
|
||
mode.base_mode |= (byte)base_mode;
|
||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
|
||
}
|
||
|
||
private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
|
||
{
|
||
mode.target_system = 1;
|
||
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:
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
///广播Rtk
|
||
/// </summary>
|
||
/// <param name="data"></param>
|
||
/// <param name="length"></param>
|
||
/// <returns></returns>
|
||
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 = 1;
|
||
gps.target_system = 1;
|
||
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
public byte[] GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata, int rtklength)
|
||
{
|
||
byte[] data;
|
||
int rtkdataleng = rtklength + 3;
|
||
|
||
data = MavlinkUtil.StructureToByteArray(indata);
|
||
|
||
byte[] packet = new byte[rtkdataleng + 6 + 2];
|
||
|
||
packet[0] = MAVLink.MAVLINK_STX;
|
||
packet[1] = (byte)(rtkdataleng);
|
||
packet[2] = (byte)packetcount;
|
||
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);
|
||
|
||
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;
|
||
|
||
return packet;
|
||
}
|
||
|
||
|
||
private byte[] 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 = 1, target_component = 1, param_type = 4 };
|
||
|
||
byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);
|
||
|
||
Array.Resize(ref temp, 16);
|
||
req.param_id = temp;
|
||
req.param_value = (value);
|
||
|
||
return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
|
||
}
|
||
|
||
private byte[] GetParam2Async(string paramname)
|
||
{
|
||
var paramId = Encoding.ASCII.GetBytes(paramname);
|
||
Array.Resize(ref paramId, 16);
|
||
|
||
var req = new MAVLink.mavlink_param_request_read_t
|
||
{
|
||
param_index = -1,
|
||
target_system = 1,
|
||
target_component = 1,
|
||
param_id = paramId
|
||
};
|
||
|
||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
|
||
}
|
||
|
||
private byte[] DoLEDCommandAsync()
|
||
{
|
||
MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
|
||
led.target_system = 1;
|
||
led.target_component = 1;//(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] = 3;
|
||
|
||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
|
||
}
|
||
|
||
private byte[] BatchCopterIdData(IEnumerable<ICopter> copters)
|
||
{
|
||
byte[] BatchByte = null;
|
||
List<ICopter> copterList = copters.OrderBy(c=>int.Parse(c.Id)).ToList();
|
||
for (int i = 0; i < copterList.Count; i++)
|
||
{
|
||
byte[] tempByte;
|
||
int id = int.Parse(copterList[i].Id);
|
||
int count = 1;
|
||
while(i+1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count)
|
||
{
|
||
count++;
|
||
i++;
|
||
}
|
||
|
||
if (count == 1)
|
||
{
|
||
short curId = (short)(0 << 12 ^ id);
|
||
tempByte = BitConverter.GetBytes(curId);
|
||
}
|
||
else
|
||
{
|
||
short beginId = (short)(2 << 12 ^ id);
|
||
tempByte = BitConverter.GetBytes(beginId);
|
||
|
||
short numCount = (short)(4 << 12 ^ count);
|
||
tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray();
|
||
}
|
||
|
||
if (BatchByte == null)
|
||
BatchByte = tempByte;
|
||
else
|
||
BatchByte = BatchByte.Concat(tempByte).ToArray();
|
||
}
|
||
short byteNum = (short)(BatchByte.Length / 2);
|
||
short length = (short)((0x5 << 12) ^ byteNum);
|
||
byte[] ret = new byte[2+ BatchByte.Length];
|
||
|
||
Array.Copy(BitConverter.GetBytes(length), ret, 2);
|
||
|
||
Array.Copy(BatchByte, 0, ret, 2, BatchByte.Length);
|
||
|
||
return ret;
|
||
}
|
||
|
||
private void GetCopterIds(IEnumerable<ICopter> copters, out short copterId, out byte[] batchPacket)
|
||
{
|
||
copterId = 0;
|
||
batchPacket = null;
|
||
|
||
if (copters != null)
|
||
{
|
||
//为提高通信稳定性，无论1架还是多架全部改为小批量广播
|
||
//if (copters.Count() == 1)
|
||
// copterId = short.Parse(copters.FirstOrDefault().Id);
|
||
//else
|
||
{
|
||
batchPacket = BatchCopterIdData(copters);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|