378 lines
12 KiB
C#
378 lines
12 KiB
C#
//#define LOG_PACKETS
|
|
|
|
using Plane.Protocols;
|
|
using System;
|
|
using System.Diagnostics;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using TaskTools.Utilities;
|
|
|
|
#if LOG_PACKETS
|
|
|
|
using System.Diagnostics;
|
|
|
|
#endif
|
|
|
|
namespace Plane.Copters
|
|
{
|
|
partial class PlaneCopter
|
|
{
|
|
private void AnalyzeAttitudePacket(byte[] buffer)
|
|
{
|
|
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
|
|
roll = att.roll * rad2deg;
|
|
pitch = att.pitch * rad2deg;
|
|
yaw = att.yaw * rad2deg;
|
|
timebootms = att.time_boot_ms;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
|
|
{
|
|
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
|
|
useLocation = true;
|
|
if (loc.lat == 0 && loc.lon == 0)
|
|
{
|
|
useLocation = false;
|
|
}
|
|
else
|
|
{
|
|
gpsalt = loc.alt / 1000.0f;
|
|
lat = loc.lat / 10000000.0;
|
|
lng = loc.lon / 10000000.0;
|
|
}
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
#if LOG_GPS_RAW_INT
|
|
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
|
|
private TimeSpan _lastGpsRawIntPacketTimeSpan;
|
|
#endif
|
|
|
|
private void AnalyzeGpsRawIntPacket(byte[] buffer)
|
|
{
|
|
#if LOG_GPS_RAW_INT
|
|
if (!_lastGpsRawIntPacketTime.IsRunning)
|
|
{
|
|
_lastGpsRawIntPacketTime.Start();
|
|
}
|
|
else
|
|
{
|
|
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
|
|
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
|
|
_lastGpsRawIntPacketTimeSpan = elapsed;
|
|
}
|
|
#endif
|
|
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
|
|
long mylat = BitConverter.ToInt32(buffer, 14);
|
|
long mylong = BitConverter.ToInt32(buffer, 18);
|
|
//if (!useLocation)
|
|
//{
|
|
lat = gps.lat * 1.0e-7;
|
|
lng = gps.lon * 1.0e-7;
|
|
//}
|
|
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
|
|
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
|
|
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
|
|
satcount = gps.satellites_visible;
|
|
groundspeed = gps.vel * 1.0e-2f;
|
|
groundcourse = gps.cog * 1.0e-2f;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
#if LOG_PACKETS
|
|
private DateTime _lastHeartbeatTime = DateTime.MinValue;
|
|
#endif
|
|
|
|
private void AnalyzeHeartbeatPacket(byte[] buffer)
|
|
{
|
|
#if LOG_PACKETS
|
|
var now = DateTime.Now;
|
|
if (_lastHeartbeatTime != DateTime.MinValue)
|
|
{
|
|
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
|
|
}
|
|
_lastHeartbeatTime = now;
|
|
#endif
|
|
DataTimeOut = 0;
|
|
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
|
|
mavlinkversion = hb.mavlink_version;
|
|
aptype = (MAVLink.MAV_TYPE)hb.type;
|
|
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
|
|
sysid = buffer[3];
|
|
compid = buffer[4];
|
|
recvpacketcount = buffer[2];
|
|
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
|
|
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
|
|
mode = hb.custom_mode;
|
|
|
|
var handler = ReceiveHeartBearEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() =>
|
|
{
|
|
handler(this, iErrorData, iDataCount);
|
|
});
|
|
}
|
|
|
|
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
|
|
{
|
|
SendReq = true;
|
|
Task.Run(GetCopterDataAsync);
|
|
}
|
|
}
|
|
|
|
private void AnalyzeMemInfoPacket(byte[] buffer)
|
|
{
|
|
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
|
|
freemem = mem.freemem;
|
|
brklevel = mem.brkval;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeMissionCurrentPacket(byte[] buffer)
|
|
{
|
|
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
|
|
{
|
|
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
|
|
nav_roll = nav.nav_roll;
|
|
nav_pitch = nav.nav_pitch;
|
|
nav_bearing = nav.nav_bearing;
|
|
target_bearing = nav.target_bearing;
|
|
wp_dist = nav.wp_dist;
|
|
alt_error = nav.alt_error;
|
|
aspd_error = nav.aspd_error / 100.0f;
|
|
xtrack_error = nav.xtrack_error;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeParamValuePacket(byte[] buffer)
|
|
{
|
|
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
|
|
param_total = (par.param_count);
|
|
string paramID = Encoding.ASCII.GetString(par.param_id);
|
|
int pos = paramID.IndexOf('\0');
|
|
if (pos != -1)
|
|
{
|
|
paramID = paramID.Substring(0, pos);
|
|
}
|
|
try
|
|
{
|
|
param[paramID] = (par.param_value);
|
|
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
|
|
}
|
|
catch (IndexOutOfRangeException ex)
|
|
{
|
|
Debug.WriteLine(ex);
|
|
}
|
|
|
|
var handler = ReceiveParamEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() =>
|
|
{
|
|
handler(this, paramID, par.param_index, par.param_count);
|
|
});
|
|
}
|
|
}
|
|
|
|
private void AnalyzeRadioPacket(byte[] buffer)
|
|
{
|
|
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
|
|
//Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
|
|
remrssi = radio.remrssi;
|
|
rssi = radio.rssi;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeRawImuPacket(byte[] buffer)
|
|
{
|
|
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
|
|
|
|
gx = imu.xgyro;
|
|
gy = imu.ygyro;
|
|
gz = imu.zgyro;
|
|
|
|
ax = imu.xacc;
|
|
ay = imu.yacc;
|
|
az = imu.zacc;
|
|
|
|
mx = imu.xmag;
|
|
my = imu.ymag;
|
|
mz = imu.zmag;
|
|
|
|
var handler = ReceiveSensorEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() =>
|
|
{
|
|
handler(this);
|
|
});
|
|
}
|
|
}
|
|
|
|
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
|
|
{
|
|
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
|
|
|
|
ch1in = rcin.chan1_raw;
|
|
ch2in = rcin.chan2_raw;
|
|
ch3in = rcin.chan3_raw;
|
|
ch4in = rcin.chan4_raw;
|
|
ch5in = rcin.chan5_raw;
|
|
ch6in = rcin.chan6_raw;
|
|
ch7in = rcin.chan7_raw;
|
|
ch8in = rcin.chan8_raw;
|
|
if (bInitChannel == false)
|
|
{
|
|
bInitChannel = true;
|
|
}
|
|
|
|
//percent
|
|
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
|
|
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
|
|
private void AnalyzeScaledImu2Packet(byte[] buffer)
|
|
{
|
|
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
|
|
gx2 = imu2.xgyro;
|
|
gy2 = imu2.ygyro;
|
|
gz2 = imu2.zgyro;
|
|
|
|
ax2 = imu2.xacc;
|
|
ay2 = imu2.yacc;
|
|
az2 = imu2.zacc;
|
|
|
|
mx2 = imu2.xmag;
|
|
my2 = imu2.ymag;
|
|
mz2 = imu2.zmag;
|
|
|
|
var handler = ReceiveScaleImu2Event;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() => handler(this));
|
|
}
|
|
}
|
|
|
|
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
|
|
{
|
|
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
|
|
|
|
mag_ofs_x = sensofs.mag_ofs_x;
|
|
mag_ofs_y = sensofs.mag_ofs_y;
|
|
mag_ofs_z = sensofs.mag_ofs_z;
|
|
mag_declination = sensofs.mag_declination;
|
|
|
|
raw_press = sensofs.raw_press;
|
|
raw_temp = sensofs.raw_temp;
|
|
|
|
gyro_cal_x = sensofs.gyro_cal_x;
|
|
gyro_cal_y = sensofs.gyro_cal_y;
|
|
gyro_cal_z = sensofs.gyro_cal_z;
|
|
|
|
accel_cal_x = sensofs.accel_cal_x;
|
|
accel_cal_y = sensofs.accel_cal_y;
|
|
accel_cal_z = sensofs.accel_cal_z;
|
|
|
|
var handler = ReceiveSensorEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() =>
|
|
{
|
|
handler(this);
|
|
});
|
|
}
|
|
}
|
|
|
|
private void AnalyzeStatusTextPacket(byte[] buffer)
|
|
{
|
|
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
|
|
var length = Array.IndexOf<byte>(stext.text, 0);
|
|
var log = Encoding.ASCII.GetString(stext.text, 0, length);
|
|
var handler = GetLogDataEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() => handler(log));
|
|
}
|
|
}
|
|
|
|
private void AnalyzeSysStatusPacket(byte[] buffer)
|
|
{
|
|
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
|
|
battery_voltage = sysstatus.voltage_battery;
|
|
battery_remaining = sysstatus.battery_remaining;
|
|
current_battery = sysstatus.current_battery / 100.0f;
|
|
|
|
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
|
|
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
|
|
|
|
if (sensors_health.gps != sensors_enabled.gps)
|
|
{
|
|
bGPSBadHealth = true;
|
|
}
|
|
else
|
|
{
|
|
bGPSBadHealth = false;
|
|
}
|
|
if (sensors_health.gyro != sensors_enabled.gyro)
|
|
{
|
|
bGyroBadHealth = true;
|
|
}
|
|
else
|
|
{
|
|
bGyroBadHealth = false;
|
|
}
|
|
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
|
|
{
|
|
bAccel = true;
|
|
}
|
|
else
|
|
{
|
|
bAccel = false;
|
|
}
|
|
if (sensors_health.compass != sensors_enabled.compass)
|
|
{
|
|
bCompass = true;
|
|
}
|
|
else
|
|
{
|
|
bCompass = false;
|
|
}
|
|
if (sensors_health.barometer != sensors_enabled.barometer)
|
|
{
|
|
bBarometer = true;
|
|
}
|
|
else
|
|
{
|
|
bBarometer = false;
|
|
}
|
|
FlightDistance2D = sysstatus.errors_count2;
|
|
var handler = ReceiveSysStatusEvent;
|
|
if (handler != null)
|
|
{
|
|
RunOnUIThread(() =>
|
|
{
|
|
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
|
|
});
|
|
}
|
|
}
|
|
|
|
private void AnalyzeVfrHudPacket(byte[] buffer)
|
|
{
|
|
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
|
|
groundspeed = vfr.groundspeed;
|
|
airspeed = vfr.airspeed;
|
|
altorigin = vfr.alt; // this might include baro
|
|
ch3percent = vfr.throttle;
|
|
heading = vfr.heading;
|
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
|
}
|
|
}
|
|
}
|