添加空中升级
修改断线消息提醒机制
This commit is contained in:
parent
4ffa68c99a
commit
cb64fd625a
@ -69,11 +69,13 @@ namespace Plane.CommunicationManagement
|
|||||||
|
|
||||||
if (Connection.IsOnline)
|
if (Connection.IsOnline)
|
||||||
{
|
{
|
||||||
|
Message.Connect(true);
|
||||||
SendQuery();
|
SendQuery();
|
||||||
await StartReadingPacketsAsync();
|
await StartReadingPacketsAsync();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
Message.Connect(false);
|
||||||
Reconnect();
|
Reconnect();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -112,11 +114,12 @@ namespace Plane.CommunicationManagement
|
|||||||
int times = 1;
|
int times = 1;
|
||||||
private void Reconnect()
|
private void Reconnect()
|
||||||
{
|
{
|
||||||
|
//Message.Show($"正在重新连接...");
|
||||||
Task.Run(async () =>
|
Task.Run(async () =>
|
||||||
{
|
{
|
||||||
CloseConnection();
|
CloseConnection();
|
||||||
await Task.Delay(250).ConfigureAwait(false);
|
await Task.Delay(250).ConfigureAwait(false);
|
||||||
Message.Show($"正在重连:次数{times++}");
|
//Message.Show($"正在重连:次数{times++}");
|
||||||
await Task.Delay(250).ConfigureAwait(false);
|
await Task.Delay(250).ConfigureAwait(false);
|
||||||
await ConnectAsync();
|
await ConnectAsync();
|
||||||
|
|
||||||
@ -254,6 +257,14 @@ namespace Plane.CommunicationManagement
|
|||||||
await WriteCommPacketAsync(copterId, messageType, data);
|
await WriteCommPacketAsync(copterId, messageType, data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public async Task UpdateCommModule()
|
||||||
|
{
|
||||||
|
MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
|
||||||
|
commUpdate.mav_count = (short)CommModuleCopterCount;
|
||||||
|
commUpdate.update_code = 0x7713;
|
||||||
|
await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
|
||||||
|
}
|
||||||
|
|
||||||
bool temp = false;
|
bool temp = false;
|
||||||
//测试用 灯光间隔1S闪烁
|
//测试用 灯光间隔1S闪烁
|
||||||
public async Task TestLED(short id)
|
public async Task TestLED(short id)
|
||||||
@ -265,9 +276,9 @@ namespace Plane.CommunicationManagement
|
|||||||
led.pattern = 0;
|
led.pattern = 0;
|
||||||
led.custom_len = 4;
|
led.custom_len = 4;
|
||||||
led.custom_bytes = new byte[24];
|
led.custom_bytes = new byte[24];
|
||||||
led.custom_bytes[0] = 80;
|
led.custom_bytes[0] = 175;
|
||||||
led.custom_bytes[1] = 0;
|
led.custom_bytes[1] = 30;
|
||||||
led.custom_bytes[2] = 0;
|
led.custom_bytes[2] = 50;
|
||||||
led.custom_bytes[3] = 3;
|
led.custom_bytes[3] = 3;
|
||||||
byte[] data;
|
byte[] data;
|
||||||
data = MavlinkUtil.StructureToByteArray(led);
|
data = MavlinkUtil.StructureToByteArray(led);
|
||||||
|
@ -118,7 +118,7 @@ namespace Plane.CommunicationManagement
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
|
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, 0);
|
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);
|
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -43,7 +43,8 @@ namespace Plane.CommunicationManagement
|
|||||||
|
|
||||||
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
||||||
{
|
{
|
||||||
string msg = System.Text.Encoding.Default.GetString(buffer).Replace("tcpserver", "flicube");
|
int count = Array.IndexOf(buffer, (byte)0);
|
||||||
|
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
|
||||||
Message.Show(msg);
|
Message.Show(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -102,6 +103,13 @@ namespace Plane.CommunicationManagement
|
|||||||
case 0x03:
|
case 0x03:
|
||||||
SaveMissionWriteStat(copterId, buffer[1]);
|
SaveMissionWriteStat(copterId, buffer[1]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 0x0e:
|
||||||
|
if (copterId == 0)
|
||||||
|
Message.Show("----------全部更新完成----------");
|
||||||
|
else
|
||||||
|
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -79,6 +79,11 @@ namespace Plane.Protocols
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_NOTIFICATION = 0x1234;
|
public const short COMM_NOTIFICATION = 0x1234;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 空中升级(更新通信模块飞机端)
|
||||||
|
/// </summary>
|
||||||
|
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
||||||
|
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
public enum CommMode
|
public enum CommMode
|
||||||
@ -119,6 +124,13 @@ namespace Plane.Protocols
|
|||||||
public Int16 mav_count; //飞机总数
|
public Int16 mav_count; //飞机总数
|
||||||
};
|
};
|
||||||
|
|
||||||
|
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
|
||||||
|
public struct comm_update_copter_module
|
||||||
|
{
|
||||||
|
public Int16 mav_count; //飞机总数
|
||||||
|
public Int16 update_code;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
|
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
|
||||||
public struct comm_asyn_time
|
public struct comm_asyn_time
|
||||||
|
Loading…
Reference in New Issue
Block a user