修改灯光频率为小数,传输转换为间隔

修改没有刷新编号的bug
This commit is contained in:
xu 2020-05-03 12:15:51 +08:00
parent 17d2fbf499
commit dc07d2d560
5 changed files with 59 additions and 6 deletions

View File

@ -13,5 +13,6 @@ namespace Plane.Copters
/// 飞行器摆放时地面的相对高度 /// 飞行器摆放时地面的相对高度
/// </summary> /// </summary>
float GroundAlt { get; set; } float GroundAlt { get; set; }
float RetainInt{ get; }
} }
} }

View File

@ -6,6 +6,7 @@ using System.Collections.Generic;
using System.Linq; using System.Linq;
using System.Text; using System.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
using System.Windows.Media;
using static Plane.Copters.PlaneCopter; using static Plane.Copters.PlaneCopter;
namespace Plane.CommunicationManagement namespace Plane.CommunicationManagement
@ -201,6 +202,30 @@ namespace Plane.CommunicationManagement
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
} }
private byte[] DoTaskLEDCommandAsync(int ledmode, float ledrate, int ledtimes, byte cR,byte cG,Byte cB)
{
//频率转为间隔单位100ms
byte LEDInterval = (byte)(10 / ledrate);
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 = 6;
led.custom_bytes = new byte[24];
led.custom_bytes[0] = cR;
led.custom_bytes[1] = cG;
led.custom_bytes[2] = cB;
led.custom_bytes[3] =(byte)ledmode;
led.custom_bytes[4] = LEDInterval;
led.custom_bytes[5] = (byte)ledtimes;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
}
private byte[] BatchCopterIdData(IEnumerable<ICopter> copters) private byte[] BatchCopterIdData(IEnumerable<ICopter> copters)
{ {
byte[] BatchByte = null; byte[] BatchByte = null;
@ -536,6 +561,31 @@ namespace Plane.CommunicationManagement
} }
/*
private int ledmode = 0; //灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
private float ledrate = 0; //频率 最小0.1
private int ledtimes = 0; //次数 (预留、暂取消其意义)
private string ledRGB = "FFFFFF"; //灯光颜色
*/
public void LED_TaskAsync( int ledmode, float ledrate, int ledtimes, string ledRGB, ICopter copter )
{
if (UseTransModule)
{
Task.Run(async () =>
{
Color rightColor = (Color)ColorConverter.ConvertFromString("#" + ledRGB);
byte[] packet = DoTaskLEDCommandAsync(ledmode, ledrate, ledtimes, (byte)rightColor.R, (byte)rightColor.G, (byte)rightColor.B);
await WriteCommPacketAsync(short.Parse(copter.Id), MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
});
}
}
/// <summary> /// <summary>
/// 设置参数 /// 设置参数

View File

@ -119,17 +119,19 @@ namespace Plane.CommunicationManagement
{ {
lock (MissinLocker) lock (MissinLocker)
{ {
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
if (wirteMissRet == 0x20) if (wirteMissRet == 0x20)
{ {
if (missionWriteState.ContainsKey(copterId)) if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = true; missionWriteState[copterId].WriteSucceed = true;
Message.Show($"{copterId}:航点写入成功"); Message.Show($"ID:{copterId}:航点写入成功");
} }
if (wirteMissRet > 0x20 && wirteMissRet < 0x30) if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
{ {
if (missionWriteState.ContainsKey(copterId)) if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = false; missionWriteState[copterId].WriteSucceed = false;
Message.Show($"{copterId}:航点写入失败"); Message.Show($"ID:{copterId}:航点写入失败");
} }
} }
await Task.Delay(10).ConfigureAwait(false); await Task.Delay(10).ConfigureAwait(false);

View File

@ -407,19 +407,19 @@ namespace Plane.Copters
public int VirtualId public int VirtualId
{ {
get { return _VirtualId; } get { return _VirtualId; }
set { Set(nameof(VirtualId), ref _VirtualId, value); } set { Set(nameof(VirtualId), ref _VirtualId, value); RefreashLoc(); }
} }
public bool DisplayVirtualId public bool DisplayVirtualId
{ {
get { return _DisplayVirtualId; } get { return _DisplayVirtualId; }
set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); } set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); RefreashLoc(); }
} }
public bool DisplayID public bool DisplayID
{ {
get { return _DisplayID; } get { return _DisplayID; }
set { Set(nameof(DisplayID), ref _DisplayID, value); } set { Set(nameof(DisplayID), ref _DisplayID, value); RefreashLoc(); }
} }

View File

@ -187,7 +187,7 @@ namespace Plane.Copters
Altitude = alt Altitude = alt
}; };
public static IMission CreateLEDControlMission(int delay, int ledmode, int ledrate, int ledtimes, byte red, byte green, byte blue) => new Mission public static IMission CreateLEDControlMission(int delay, int ledmode, float ledrate, int ledtimes, byte red, byte green, byte blue) => new Mission
{ {
Command = FlightCommand.LEDControl, Command = FlightCommand.LEDControl,
Param1 = delay, //灯光延时 单位:100ms Param1 = delay, //灯光延时 单位:100ms