group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.改变飞行指令在guided模式下不用以前的item命令，不过还有问题，暂时不用

Browse Source 
2.将飞机发回的文本提示加入日志文件，主要用于查找无法解锁的原因
This commit is contained in:
pxzleo 2017-09-07 17:58:05 +08:00
parent b09020f8ff
commit b000e7fa0f
6 changed files with 152 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
PlaneGcsSdk.Contract_Shared/Copters/ICopterEvents.cs
View File

@ -53,5 +53,12 @@ namespace Plane.Copters
/// 在收到系统状态信息时发生。
/// </summary>
event EventHandler<SystemStatusReceivedEventArgs> SystemStatusReceived;
/// <summary>
/// 在收到系统文本日志信息时发生。
/// </summary>
event EventHandler<MessageCreatedEventArgs> TextReceived;
}
}

11
PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
View File

@ -234,6 +234,9 @@ namespace Plane.Copters
public event EventHandler<SystemStatusReceivedEventArgs> SystemStatusReceived;
public event EventHandler<MessageCreatedEventArgs> TextReceived;
protected enum PDataStreamType
{
SYS_STATUS = 1, // MAVLINK_MSG_ID_SYS_STATUS,
@ -1060,6 +1063,14 @@ namespace Plane.Copters
SystemStatusReceived?.Invoke(this, e);
}
protected void RaiseTextReceived(MessageCreatedEventArgs e)
{
TextReceived?.Invoke(this, e);
}
protected void SetTargets(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)
{
if (ch1 != null) DesiredChannel1 = ch1;

9
PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
View File

@ -16,6 +16,8 @@ namespace Plane.Copters
{
//飞机消息日志，后面需要改为记录方式
StatusText =Name+":"+log;
var e = new MessageCreatedEventArgs { Message = StatusText };
RaiseTextReceived(e);
}
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
@ -213,6 +215,13 @@ namespace Plane.Copters
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
}
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
{
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
RaiseTextReceived(txte);
}
#region 计算剩余电量

68
PlaneGcsSdk_Shared/Copters/PlaneCopter.cs
View File

@ -701,6 +701,20 @@ namespace Plane.Copters
//}
}
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)
@ -711,13 +725,65 @@ namespace Plane.Copters
//try
//{
//gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
//老方法无法设置速度
// 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;

57
PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs
View File

@ -518,7 +518,20 @@ namespace Plane.Protocols
///<summary> Local coordinate frame, Z-down (x: east, y: north, z: up) | </summary>
LOCAL_ENU = 4,
///<summary> | </summary>
ENUM_END = 5,
///<summary> Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) | </summary>
GLOBAL_INT = 5,
///<summary> Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. | </summary>
GLOBAL_RELATIVE_ALT_INT = 6,
///<summary> Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | </summary>
LOCAL_OFFSET_NED = 7,
///<summary> Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | </summary>
BODY_NED = 8,
///<summary> Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | </summary>
BODY_OFFSET_NED = 9,
///<summary> Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | </summary>
GLOBAL_TERRAIN_ALT = 10,
///<summary> Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | </summary>
GLOBAL_TERRAIN_ALT_INT = 11,
};
@ -2595,6 +2608,48 @@ namespace Plane.Protocols
};
public const byte SET_POSITION_TARGET_GLOBAL_INT = 86;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)]
///<summary> Sets a desired vehicle position, velocity, and/or acceleration in a global coordinate system (WGS84). Used by an external controller to command the vehicle (manual controller or other system). </summary>
public struct mavlink_set_position_target_global_int_t
{
/// <summary> Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. </summary>
public uint time_boot_ms;
/// <summary> X Position in WGS84 frame in 1e7 * meters </summary>
public int lat_int;
/// <summary> Y Position in WGS84 frame in 1e7 * meters </summary>
public int lon_int;
/// <summary> Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT </summary>
public float alt;
/// <summary> X velocity in NED frame in meter / s </summary>
public float vx;
/// <summary> Y velocity in NED frame in meter / s </summary>
public float vy;
/// <summary> Z velocity in NED frame in meter / s </summary>
public float vz;
/// <summary> X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N </summary>
public float afx;
/// <summary> Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N </summary>
public float afy;
/// <summary> Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N </summary>
public float afz;
/// <summary> yaw setpoint in rad </summary>
public float yaw;
/// <summary> yaw rate setpoint in rad/s </summary>
public float yaw_rate;
/// <summary> Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate </summary>
public ushort type_mask;
/// <summary> System ID </summary>
public byte target_system;
/// <summary> Component ID </summary>
public byte target_component;
/// <summary> Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 MAV_FRAME</summary>
public /*MAV_FRAME*/byte coordinate_frame;
};
public const byte MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET = 89;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
public struct mavlink_local_position_ned_system_global_offset_t

4
PlaneGcsSdk_Shared/TaskTools/HIL/Vector3.cs
View File

@ -1,4 +1,4 @@
#if INTERNAL_TOOL
//#if INTERNAL_TOOL
using System;
using System.Collections.Generic;
@ -151,4 +151,4 @@ namespace TaskTools.HIL
}
}
#endif
//#endif