Plane.Sdk3/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs

//#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]);
        }

        /// <summary>
        /// 处理通信模块发过来的数据
        /// </summary>
        /// <param name="buffer"></param>
        public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
        {
            var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
            lat = info.lat * 1.0e-7; 
            lng = info.lng * 1.0e-7;
            commModuleMode = (uint)info.control_mode;
            gpsstatus = (byte)info.gps_status;
            gpsalt = ((float)info.alt) / 1000;
            satcount = info.gps_num_sats;
            isUnlocked = info.lock_status == 1;

            battery_voltage = ((float)info.battery_voltage) / 1000;
            yaw = ((float)info.yaw / 100) % 360;

            commModuleVersion = version;

            retain = info.retain;
            RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
        }
    }
}