310 lines
11 KiB
C#
310 lines
11 KiB
C#
using System;
|
|
using static Plane.Protocols.MAVLink;
|
|
|
|
namespace Plane.Copters
|
|
{
|
|
public partial class PLCopter : CopterImplSharedPart
|
|
{
|
|
private bool _fetchingFirmwareVersion;
|
|
|
|
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
|
|
{
|
|
IsConnected = _internalCopter.IsConnected;
|
|
}
|
|
|
|
private void _internalCopter_GetLogDataEvent(string log)
|
|
{
|
|
//飞机消息日志，后面需要改为记录方式
|
|
StatusText =Name+":"+log;
|
|
var e = new MessageCreatedEventArgs { Message = StatusText };
|
|
RaiseTextReceived(e);
|
|
}
|
|
|
|
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
|
|
{
|
|
switch (StreamType)
|
|
{
|
|
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
|
|
{
|
|
SatCount = _internalCopter.satcount;
|
|
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
|
|
GpsHdop = _internalCopter.gpshdop;
|
|
Altitude = _internalCopter.gpsalt;
|
|
Retain = BitConverter.GetBytes(_internalCopter.retain);
|
|
|
|
Voltage = _internalCopter.battery_voltage;
|
|
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
|
|
CommModuleVersion = _internalCopter.commModuleVersion;
|
|
IsUnlocked = _internalCopter.isUnlocked;
|
|
//Yaw = _internalCopter.yaw;
|
|
Heading = _internalCopter.heading;
|
|
HeartbeatCount++;
|
|
|
|
if (SatCount > 8)
|
|
{
|
|
IsGpsAccurate = true;
|
|
RecordLat = _internalCopter.lat;
|
|
RecordLng = _internalCopter.lng;
|
|
}
|
|
|
|
Latitude = RecordLat;
|
|
Longitude = RecordLng;
|
|
UpdateFlightDataIfNeeded();
|
|
RaiseLocationChangedIfNeeded();
|
|
|
|
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
|
|
|
break;
|
|
}
|
|
case MAVLINK_MSG_ID_ATTITUDE: //无用
|
|
{
|
|
Roll = _internalCopter.roll;
|
|
Pitch = _internalCopter.pitch;
|
|
Yaw = _internalCopter.yaw;
|
|
TimebootMs = _internalCopter.timebootms;
|
|
RaiseAttitudeChanged();
|
|
|
|
break;
|
|
}
|
|
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
|
|
{
|
|
Channel1 = _internalCopter.ch1in;
|
|
Channel2 = _internalCopter.ch2in;
|
|
Channel3 = _internalCopter.ch3in;
|
|
Channel4 = _internalCopter.ch4in;
|
|
Channel5 = _internalCopter.ch5in;
|
|
Channel6 = _internalCopter.ch6in;
|
|
Channel7 = _internalCopter.ch7in;
|
|
Channel8 = _internalCopter.ch8in;
|
|
|
|
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
|
|
|
|
break;
|
|
}
|
|
case MAVLINK_MSG_ID_VFR_HUD:
|
|
{
|
|
Heading = _internalCopter.heading; //有用
|
|
Altitude = _internalCopter.alt; //有用
|
|
AirSpeed = _internalCopter.airspeed; //没用
|
|
GroundSpeed = _internalCopter.groundspeed; //没用
|
|
|
|
RaiseAltitudeChangedIfNeeded();
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
|
|
{
|
|
IsUnlocked = _internalCopter.armed;
|
|
Mode = (FlightMode)_internalCopter.mode;
|
|
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
|
|
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
|
|
++HeartbeatCount;
|
|
|
|
IsCheckingConnection = false;
|
|
/*
|
|
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
|
|
{
|
|
try
|
|
{
|
|
_fetchingFirmwareVersion = true;
|
|
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
|
|
}
|
|
catch (TimeoutException)
|
|
{
|
|
// 吞掉。
|
|
}
|
|
finally
|
|
{
|
|
_fetchingFirmwareVersion = false;
|
|
}
|
|
}
|
|
*/
|
|
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
|
|
}
|
|
|
|
private void _internalCopter_ReceiveSensorEvent(object sender)
|
|
{
|
|
RaiseSensorDataReceived(EventArgs.Empty);
|
|
}
|
|
|
|
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
|
|
{
|
|
Voltage = _internalCopter.battery_voltage / 1000;
|
|
|
|
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
|
|
{
|
|
CalcBatteryPer();
|
|
}
|
|
else
|
|
{
|
|
BatteryPer = _internalCopter.battery_remaining;
|
|
}
|
|
|
|
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
|
|
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
|
|
if (FirmwareVersion != null)
|
|
{
|
|
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
|
|
{
|
|
// 飞控提供了 canTakeOff。
|
|
IsGpsAccurate = canTakeOff;
|
|
}
|
|
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
|
|
{
|
|
// 2.0 飞行器，飞控未提供 canTakeOff。
|
|
IsGpsAccurate = SatCount >= 12;
|
|
}
|
|
else
|
|
{
|
|
// 1.0 飞行器，飞控不提供 canTakeOff。
|
|
IsGpsAccurate = SatCount >= 6;
|
|
}
|
|
}
|
|
else
|
|
IsGpsAccurate = SatCount >= 8;
|
|
|
|
FlightDistance2D = _internalCopter.FlightDistance2D;
|
|
|
|
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
|
|
}
|
|
|
|
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
|
{
|
|
switch (e.Packet[5])
|
|
{
|
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
|
MissionCount = _internalCopter.WpCount;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
|
{
|
|
switch (e.Packet[5])
|
|
{
|
|
case MAVLINK_MSG_ID_MISSION_ACK:
|
|
AnalyzeMissionAckPacket(e.Packet);
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
|
AnalyzeMissionCountPacket(e.Packet);
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_MISSION_ITEM:
|
|
AnalyzeMissionItemPacket(e.Packet);
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
|
AnalyzeMissionRequestPacket(e.Packet);
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_SET_PAIR:
|
|
AnalyzeSetPairPacket(e.Packet);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
#if PRIVATE
|
|
protected virtual void RegisterInternalCopterEventHandlers()
|
|
#else
|
|
|
|
private void RegisterInternalCopterEventHandlers()
|
|
#endif
|
|
{
|
|
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
|
|
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
|
|
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
|
|
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
|
|
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
|
|
_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 计算剩余电量
|
|
|
|
private int bPerTimes;
|
|
private int outBatteryPer;
|
|
private int[] tPerTimes = new int[20];
|
|
private int v_num;
|
|
|
|
/// <summary>
|
|
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
|
|
/// </summary>
|
|
private void CalcBatteryPer()
|
|
{
|
|
float volmax = 0f;
|
|
float volmin = 0f;
|
|
if (Voltage > 5 && Voltage < 9)
|
|
{
|
|
volmax = 8.2f;
|
|
volmin = 7f;
|
|
}
|
|
else if (Voltage >= 9
|
|
&& Voltage < 13.6)
|
|
{
|
|
volmax = 11.6f;
|
|
volmin = 10.2f;
|
|
}
|
|
else if (Voltage >= 13.6
|
|
&& Voltage < 17.2)
|
|
{
|
|
volmax = 16.3f;
|
|
volmin = 14.2f;
|
|
}
|
|
else if (Voltage >= 17.2
|
|
&& Voltage < 26.2)
|
|
{
|
|
volmax = 24.8f;
|
|
volmin = 21.2f;
|
|
}
|
|
|
|
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
|
|
|
|
if (batteryPer == -1 || volmax == 0 || volmin == 0)
|
|
return;
|
|
|
|
if (bPerTimes < 20)
|
|
{
|
|
tPerTimes[bPerTimes] = batteryPer;
|
|
bPerTimes += 1;
|
|
}
|
|
else
|
|
{
|
|
tPerTimes[v_num] = batteryPer;
|
|
v_num++;
|
|
if (v_num == 20)
|
|
v_num = 0;
|
|
}
|
|
for (int i = 0; i < bPerTimes; i++)
|
|
{
|
|
outBatteryPer += tPerTimes[i];
|
|
}
|
|
|
|
outBatteryPer = outBatteryPer / bPerTimes;
|
|
if (outBatteryPer < BatteryPer && bPerTimes > 18)
|
|
{
|
|
BatteryPer = (byte)outBatteryPer;
|
|
}
|
|
}
|
|
|
|
#endregion 计算剩余电量
|
|
}
|
|
}
|