加入RTK支持

地面站发送接收到的基站数据到飞机,数据不做任何修改直接打包发送到每架飞机
This commit is contained in:
pxzleo 2017-07-31 01:09:32 +08:00
parent f70081bf46
commit 2ae769b5d4
15 changed files with 139 additions and 1 deletions

View File

@ -13,6 +13,9 @@ namespace Plane.Communication
/// </summary> /// </summary>
event EventHandler<ExceptionThrownEventArgs> ExceptionThrown; event EventHandler<ExceptionThrownEventArgs> ExceptionThrown;
int BytesToRead();
/// <summary> /// <summary>
/// 获取一个值,指示通信是否已开启。 /// 获取一个值,指示通信是否已开启。
/// </summary> /// </summary>

View File

@ -4,7 +4,11 @@
{ {
NoFix = 0, NoFix = 0,
Fix2D = 2, Fix2D = 2,
Fix3D = 3 Fix3D = 3,
DGPS3D=4,
RTK3D= 5
} }
public static class ByteExtensions public static class ByteExtensions

View File

@ -126,5 +126,10 @@ namespace Plane.Copters
/// </summary> /// </summary>
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns> /// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
Task UnlockAsync(); Task UnlockAsync();
Task InjectGpsDataAsync(byte[] data, ushort length);
} }
} }

View File

@ -21,6 +21,33 @@ namespace Plane.Communication
public event EventHandler<ExceptionThrownEventArgs> ExceptionThrown; public event EventHandler<ExceptionThrownEventArgs> ExceptionThrown;
public int BytesToRead()
{
int result;
for (int i = 0; i < _candidateConnections.Count; i++)
{
if (_candidateConnections[i].IsOpen)
{
try
{
result = _candidateConnections[i].BytesToRead();
if (result != 0)
{
return result;
}
}
catch (Exception ex)
{
Debug.WriteLine(ex);
ExceptionThrown?.Invoke(this, new ExceptionThrownEventArgs(ex));
}
}
}
return 0;
}
public bool IsOpen public bool IsOpen
{ {
get get

View File

@ -20,6 +20,11 @@ namespace Plane.Communication
IsOpen = false; IsOpen = false;
} }
public int BytesToRead()
{
return 0;
}
public Task OpenAsync() public Task OpenAsync()
{ {
IsOpen = true; IsOpen = true;

View File

@ -55,6 +55,7 @@ namespace Plane.Communication
{ {
_port.Open(); _port.Open();
_internalIsOpen = true; _internalIsOpen = true;
} }
public Task OpenAsync() public Task OpenAsync()
@ -63,6 +64,13 @@ namespace Plane.Communication
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }
public int BytesToRead()
{
return _port.BytesToRead;
}
public int Read(byte[] buffer, int offset, int count) public int Read(byte[] buffer, int offset, int count)
{ {
while (Available < count) while (Available < count)

View File

@ -57,6 +57,13 @@ namespace Plane.Communication
public abstract Task OpenAsync(); public abstract Task OpenAsync();
public int BytesToRead()
{
return 0;
}
public int Read(byte[] buffer, int offset, int count) public int Read(byte[] buffer, int offset, int count)
{ {
try try

View File

@ -20,6 +20,10 @@ namespace Plane.Communication
public abstract void Close(); public abstract void Close();
public abstract Task OpenAsync(); public abstract Task OpenAsync();
public int BytesToRead()
{
return 0;
}
public async Task<int> ReadAsync(byte[] buffer, int offset, int size) public async Task<int> ReadAsync(byte[] buffer, int offset, int size)
{ {

View File

@ -93,6 +93,10 @@ namespace Plane.Communication
return 0; return 0;
} }
} }
public int BytesToRead()
{
return 0;
}
public virtual async Task WriteAsync(byte[] buffer, int offset, int count) public virtual async Task WriteAsync(byte[] buffer, int offset, int count)
{ {

View File

@ -186,6 +186,13 @@ namespace Plane.CopterManagement
{ {
return Copter.TakeOffAsync(); return Copter.TakeOffAsync();
} }
public Task InjectGpsDataAsync(byte[] data, ushort length)
{
return Copter.InjectGpsDataAsync(data, length);
}
public Task TakeOffAsync(float alt) public Task TakeOffAsync(float alt)
{ {

View File

@ -92,6 +92,13 @@ namespace Plane.Copters
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }
public Task InjectGpsDataAsync(byte[] data, ushort length)
{
return TaskUtils.CompletedTask;
}
internal override Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000) internal override Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
{ {
return Task.FromResult(true); return Task.FromResult(true);
@ -101,6 +108,10 @@ namespace Plane.Copters
{ {
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }
} }
#if PRIVATE #if PRIVATE

View File

@ -420,6 +420,12 @@ namespace Plane.Copters
Roll = 0; Roll = 0;
Pitch = 0; Pitch = 0;
} }
public Task InjectGpsDataAsync(byte[] data, ushort length)
{
return TaskUtils.CompletedTask;
}
private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1; private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;

View File

@ -133,6 +133,12 @@ namespace Plane.Copters
} }
public async Task InjectGpsDataAsync(byte[] data, ushort length)
{
await _internalCopter.InjectGpsDataAsync(data, length).ConfigureAwait(false);
}
public async Task SetShowLEDAsync(bool Ledon) public async Task SetShowLEDAsync(bool Ledon)
{ {

View File

@ -1234,6 +1234,26 @@ namespace Plane.Copters
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); 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;
var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
for (int a = 0; a < len; a++)
{
gps.data = new byte[msglen];
int copy = Math.Min(length - a * msglen, msglen);
Array.Copy(data, a * msglen, gps.data, 0, copy);
gps.len = (byte)copy;
gps.target_component = compid;
gps.target_system = sysid;
await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps);
}
}
public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
{ {
//MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();

View File

@ -2913,6 +2913,27 @@ namespace Plane.Protocols
}; };
public const byte MAVLINK_MSG_ID_GPS_INJECT_DATA = 123;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 113)]
///<summary> data for injecting into the onboard GPS (used for DGPS) </summary>
public struct mavlink_gps_inject_data_t
{
/// <summary> System ID </summary>
public byte target_system;
/// <summary> Component ID </summary>
public byte target_component;
/// <summary> data length </summary>
public byte len;
/// <summary> raw data (110 is enough for 12 satellites of RTCMv2) </summary>
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 110)]
public byte[] data;
};
public const byte MAVLINK_MSG_ID_BATTERY_STATUS = 147; public const byte MAVLINK_MSG_ID_BATTERY_STATUS = 147;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)] [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)]
public struct mavlink_battery_status_t public struct mavlink_battery_status_t