Ejemplo n.º 1
0
        /// <summary>
        /// Recevied UDP packet, process and send required data to serial port.
        /// </summary>
        /// <param name="data">Packet</param>
        /// <param name="receviedbytes">Length</param>
        /// <param name="comPort">Com Port</param>
        private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
        {
            sitl_fdm sitldata = new sitl_fdm();

            if (data[0] == 'D' && data[1] == 'A')
            {
                // Xplanes sends
                // 5 byte header
                // 1 int for the index - numbers on left of output
                // 8 floats - might be useful. or 0 if not
                int count = 5;
                while (count < receviedbytes)
                {
                    int index = BitConverter.ToInt32(data, count);

                    DATA[index] = new float[8];

                    DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
                    DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
                    DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
                    DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
                    DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
                    DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
                    DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
                    DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;

                    count += 36; // 8 * float
                }

                bool xplane9 = !CHK_xplane10.Checked;

                if (xplane9)
                {
                    sitldata.pitchDeg = (DATA[18][0]);
                    sitldata.rollDeg = (DATA[18][1]);
                    sitldata.yawDeg = (DATA[18][2]);
                    sitldata.pitchRate = (DATA[17][0] * rad2deg);
                    sitldata.rollRate = (DATA[17][1] * rad2deg);
                    sitldata.yawRate = (DATA[17][2] * rad2deg);

                    sitldata.heading = ((float)DATA[19][2]);
                }
                else
                {
                    sitldata.pitchDeg = (DATA[17][0]);
                    sitldata.rollDeg = (DATA[17][1]);
                    sitldata.yawDeg = (DATA[17][2]);
                    sitldata.pitchRate = (DATA[16][0] * rad2deg);
                    sitldata.rollRate = (DATA[16][1] * rad2deg);
                    sitldata.yawRate = (DATA[16][2] * rad2deg);

                    sitldata.heading = (DATA[18][2]); // 18-2
                }

                sitldata.airspeed = ((DATA[3][5] * .44704));

                sitldata.latitude = (DATA[20][0]);
                sitldata.longitude = (DATA[20][1]);
                sitldata.altitude = (DATA[20][2] * ft2m);

                sitldata.speedN = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
                sitldata.speedE = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
                sitldata.speedD = -DATA[21][4];

                Matrix3 dcm = new Matrix3();
                dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.heading * deg2rad);

                // rad = tas^2 / (tan(angle) * G)
                float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.808f * Math.Tan(sitldata.rollDeg * deg2rad)));

                float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs

                // a = v^2/r
                float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;

                Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.808));

                Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));

              //  accel_body += centrip_accel;

                Vector3 velocitydelta = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN), (sitldata_old.speedE - sitldata.speedE), (sitldata_old.speedD - sitldata.speedD)));

                Vector3 velocity = dcm.transposed() * (new Vector3((sitldata.speedN), (sitldata.speedE), (sitldata.speedD)));

                //Console.WriteLine("vel " + velocity.ToString());
                //Console.WriteLine("ved " + velocitydelta.ToString());

                // a = dv / dt

                // 50 hz = 0.02sec
                Vector3 accel_mine_body = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN) / 0.02, (sitldata_old.speedE - sitldata.speedE) / 0.02, (sitldata_old.speedD - sitldata.speedD) / 0.02));

             //   Console.WriteLine("G"+accel_body.ToString());
                //Console.WriteLine("M"+accel_mine_body.ToString());

             //   sitldata.pitchRate = 0;
              //  sitldata.rollRate = 0;
              //  sitldata.yawRate = 0;

                //accel_mine_body.x =0;// *= -1;
                //accel_mine_body.y =0;//*= -1;

               // accel_body -= accel_mine_body;

                sitldata.xAccel = accel_body.x;// DATA[4][5] * 1;
                sitldata.yAccel = accel_body.y;//  DATA[4][6] * 1;
                sitldata.zAccel = accel_body.z;//  (0 - DATA[4][4]) * 9.808;

                sitldata.xAccel = DATA[4][5] *9.808;
                sitldata.yAccel = DATA[4][6] *9.808;
                sitldata.zAccel = -DATA[4][4] *9.808;

             //   Console.WriteLine(accel_body.ToString());
              //  Console.WriteLine("        {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);

            }
            else if (receviedbytes == 0x64) // FG binary udp
            {
                //FlightGear
                /*

                fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);

                if (imudata2.magic != 0x4c56414d)
                    return;

                if (imudata2.latitude == 0)
                    return;

                chkSensor.Checked = true;

                imu.time_usec = ((ulong)DateTime.Now.ToBinary());

                imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
                imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
                imu.xmag = 0;
                imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
                imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
                imu.ymag = 0;
                imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
                imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
                imu.zmag = 0;

                gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
                gps.fix_type = 3;
                gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
                gps.lat = (int)(imudata2.latitude * 1.0e7);
                gps.lon = (int)(imudata2.longitude * 1.0e7);
                gps.time_usec = ((ulong)DateTime.Now.Ticks);
                gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);

                //FileStream stream = File.OpenWrite("fgdata.txt");
                //stream.Write(data, 0, receviedbytes);
                //stream.Close();
                 */
            }
            else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83   662 = 3.91
            {
                /*
            // Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
            public float Model_fVel_Body_X; public float Model_fVel_Body_Y; public float Model_fVel_Body_Z;    // m/s    Model velocity in body coordinates
            public float Model_fAngVel_Body_X; public float Model_fAngVel_Body_Y; public float Model_fAngVel_Body_Z; // rad/s  Model angular velocity in body coordinates
            public float Model_fAccel_Body_X; public float Model_fAccel_Body_Y; public float Model_fAccel_Body_Z;  // m/s/s  Model acceleration in body coordinates
                 */
                TDataFromAeroSimRC aeroin_last = aeroin;

                aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);

                sitldata.pitchDeg = (aeroin.Model_fPitch * rad2deg);
                sitldata.rollDeg = (aeroin.Model_fRoll * -1 * rad2deg);
                sitldata.yawDeg = ((aeroin.Model_fHeading * rad2deg));

                sitldata.pitchRate =aeroin.Model_fAngVel_Body_X * rad2deg;
                sitldata.rollRate = aeroin.Model_fAngVel_Body_Y * rad2deg;
                sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z * rad2deg;

                // calc gravity
                Matrix3 dcm = new Matrix3();
                dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);

                Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8)); // -9.8

                sitldata.xAccel = aeroin.Model_fAccel_Body_Y / 9.808 + accel_body.x;// pitch - back forward-
                sitldata.yAccel = aeroin.Model_fAccel_Body_X / 9.808 + accel_body.y; // roll - left right-
                sitldata.zAccel = -aeroin.Model_fAccel_Body_Z /9.808 + accel_body.z;

              //  Console.WriteLine("2 {0,20} {1,20} {2,20}", aeroin.Model_fAccel_Body_X.ToString("0.000"), aeroin.Model_fAccel_Body_Y.ToString("0.000"), aeroin.Model_fAccel_Body_Z.ToString("0.000"));

                sitldata.altitude = aeroin.Model_fPosZ;
                sitldata.latitude = aeroin.Model_fLatitude;
                sitldata.longitude = aeroin.Model_fLongitude;

                sitldata.speedN = aeroin.Model_fVelY;
                sitldata.speedE = aeroin.Model_fVelX;
                sitldata.speedD = aeroin.Model_fVelZ;

                float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
                float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;

                sitldata.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
            }
            else if (receviedbytes == 408)
            {

                FGNetFDM fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);

                lastfdmdata = fdm;

                sitldata.altitude = (fdm.altitude);
                sitldata.latitude = (fdm.latitude * rad2deg);
                sitldata.longitude = (fdm.longitude * rad2deg);

                sitldata.rollDeg = fdm.phi * rad2deg;
                sitldata.pitchDeg = fdm.theta * rad2deg;
                sitldata.yawDeg = fdm.psi * rad2deg;

                sitldata.rollRate = fdm.phidot * rad2deg;
                sitldata.pitchRate = fdm.thetadot * rad2deg;
                sitldata.yawRate = fdm.psidot * rad2deg;

                sitldata.speedN = fdm.v_north * ft2m;
                sitldata.speedE = fdm.v_east * ft2m;
                sitldata.speedD = fdm.v_down * ft2m;

                sitldata.xAccel = (fdm.A_X_pilot * 9.808 / 32.2); // pitch
                sitldata.yAccel = (fdm.A_Y_pilot * 9.808 / 32.2); // roll
                sitldata.zAccel = (fdm.A_Z_pilot / 32.2 * 9.808);

                sitldata.airspeed = fdm.vcas * 0.5144444f;//  knots to m/s

               // Console.WriteLine("1 {0} {1} {2} {3}",(float)sitldata.rollDeg,MainV2.comPort.MAV.cs.roll,sitldata.pitchDeg,MainV2.comPort.MAV.cs.pitch);

                if (RAD_JSBSim.Checked)
                    sitldata.airspeed = fdm.vcas * ft2m;//  fps to m/s

            }
            else
            {
                log.Info("Bad Udp Packet " + receviedbytes);
                return;
            }

            if (sitldata.altitude < 0)
                sitldata.altitude = 0.00001;

            sitldata_old = sitldata;

            // write arduimu to ardupilot
            if (CHK_quad.Checked && !RAD_aerosimrc.Checked) // quad does its own
            {
                return;
            }

            if (RAD_JSBSim.Checked && chkSITL.Checked)
            {
                byte[] buffer = new byte[1500];
                while (JSBSimSEND.Client.Available > 5)
                {
                    int read = JSBSimSEND.Client.Receive(buffer);
                   // Console.WriteLine(ASCIIEncoding.ASCII.GetString(buffer,0,read));
                }

                sitldata.magic = (int)0x4c56414f;

                byte[] sendme = StructureToByteArray(sitldata);

                SITLSEND.Send(sendme, sendme.Length);

                return;
            }

            if (chkSITL.Checked)
            {
                sitldata.magic = (int)0x4c56414f;

                byte[] sendme = StructureToByteArray(sitldata);

                SITLSEND.Send(sendme, sendme.Length);

                return;
            }

            TimeSpan gpsspan = DateTime.Now - lastgpsupdate;

            // add gps delay
            if (gpsspan.TotalMilliseconds >= GPS_rate)
            {
                lastgpsupdate = DateTime.Now;

                // save current fix = 3
                sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;

                //                Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));

                // return buffer index + 5 = (3 + 5) = 8 % 6 = 2
                oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];

                //comPort.sendPacket(oldgps);

                gpsbufferindex++;
            }

            MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();

            hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec

            hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
            hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
            hilstate.alt = (int)(oldgps.altitude * 1000); // mm

             //   Console.WriteLine(hilstate.alt);

               // Console.WriteLine("{0} {1} {2}", sitldata.rollDeg.ToString("0.0"), sitldata.pitchDeg.ToString("0.0"), sitldata.yawDeg.ToString("0.0"));

            hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
            hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
            hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
            hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
            hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
            hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)

            hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100 - lat
            hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100 - long
            hilstate.vz = (short)(sitldata.speedD * 100); // m/s * 100 - + speed down

            hilstate.xacc = (short)(sitldata.xAccel * 101.957); // (mg)
            hilstate.yacc = (short)(sitldata.yAccel * 101.957); // (mg)
            hilstate.zacc = (short)(sitldata.zAccel * 101.957); // (mg)

            packetcount++;

            if (comPort.BaseStream.BytesToWrite > 100)
                return;

              //  if (packetcount % 2 == 0)
              //      return;

            comPort.sendPacket(hilstate);

            //            comPort.sendPacket(oldgps);

            //comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );

            MAVLink.mavlink_raw_pressure_t pres = new MAVLink.mavlink_raw_pressure_t();
            double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * sitldata.altitude, 5.25588)); // updated from valid gps
            pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa

               // comPort.sendPacket(pres);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Recevied UDP packet, process and send required data to serial port.
        /// </summary>
        /// <param name="data">Packet</param>
        /// <param name="receviedbytes">Length</param>
        /// <param name="comPort">Com Port</param>
        private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
        {
            sitl_fdm sitldata = new sitl_fdm();

            if (data[0] == 'D' && data[1] == 'A')
            {
                // Xplanes sends
                // 5 byte header
                // 1 int for the index - numbers on left of output
                // 8 floats - might be useful. or 0 if not
                int count = 5;
                while (count < receviedbytes)
                {
                    int index = BitConverter.ToInt32(data, count);

                    DATA[index] = new float[8];

                    DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
                    DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
                    DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
                    DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
                    DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
                    DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
                    DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
                    DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;

                    count += 36; // 8 * float
                }

                bool xplane9 = !CHK_xplane10.Checked;

                if (xplane9)
                {
                    sitldata.pitchDeg = (DATA[18][0]);
                    sitldata.rollDeg = (DATA[18][1]);
                    sitldata.yawDeg = (DATA[18][2]);
                    sitldata.pitchRate = (DATA[17][0] * rad2deg);
                    sitldata.rollRate = (DATA[17][1] * rad2deg);
                    sitldata.yawRate = (DATA[17][2] * rad2deg);

                    sitldata.heading = ((float)DATA[19][2]);
                }
                else
                {
                    sitldata.pitchDeg = (DATA[17][0]);
                    sitldata.rollDeg = (DATA[17][1]);
                    sitldata.yawDeg = (DATA[17][2]);
                    sitldata.pitchRate = (DATA[16][0] * rad2deg);
                    sitldata.rollRate = (DATA[16][1] * rad2deg);
                    sitldata.yawRate = (DATA[16][2] * rad2deg);

                    sitldata.heading = (DATA[18][2]);
                }

                sitldata.airspeed = ((DATA[3][5] * .44704));

                sitldata.latitude = (DATA[20][0]);
                sitldata.longitude = (DATA[20][1]);
                sitldata.altitude = (DATA[20][2] * ft2m);

                sitldata.speedN = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
                sitldata.speedE = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));

                Matrix3 dcm = new Matrix3();
                dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);

                // rad = tas^2 / (tan(angle) * G)
                float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.8f * Math.Tan(sitldata.rollDeg * deg2rad)));

                float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs

                // a = v^2/r
                float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;

                Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8));

                Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));

                accel_body -= centrip_accel;

                sitldata.xAccel = DATA[4][5] * 9.8;
                sitldata.yAccel = DATA[4][6] * 9.8;
                sitldata.zAccel = (0 - DATA[4][4]) * 9.8;

              //      Console.WriteLine(accel_body.ToString());
              //      Console.WriteLine("        {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);

            }
            else if (receviedbytes == 0x64) // FG binary udp
            {
                //FlightGear
                /*

                fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);

                if (imudata2.magic != 0x4c56414d)
                    return;

                if (imudata2.latitude == 0)
                    return;

                chkSensor.Checked = true;

            #if MAVLINK10
                imu.time_usec = ((ulong)DateTime.Now.ToBinary());
            #else
                imu.usec = ((ulong)DateTime.Now.ToBinary());
            #endif

                imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
                imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
                imu.xmag = 0;
                imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
                imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
                imu.ymag = 0;
                imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
                imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
                imu.zmag = 0;

            #if MAVLINK10
                gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
                gps.fix_type = 3;
                gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
                gps.lat = (int)(imudata2.latitude * 1.0e7);
                gps.lon = (int)(imudata2.longitude * 1.0e7);
                gps.time_usec = ((ulong)DateTime.Now.Ticks);
                gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
            #else
                gps.alt = ((float)(imudata2.altitude * ft2m));
                gps.fix_type = 3;
                gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg);
                gps.lat = ((float)imudata2.latitude);
                gps.lon = ((float)imudata2.longitude);
                gps.usec = ((ulong)DateTime.Now.Ticks);
                gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m);

            #endif
                //FileStream stream = File.OpenWrite("fgdata.txt");
                //stream.Write(data, 0, receviedbytes);
                //stream.Close();
                 */
            }
            else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83   662 = 3.91
            {

                aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);

                sitldata.pitchDeg = (aeroin.Model_fPitch * rad2deg);
                sitldata.rollDeg = (aeroin.Model_fRoll * -1 * rad2deg);
                sitldata.yawDeg = ((aeroin.Model_fHeading * rad2deg));

                sitldata.pitchRate = aeroin.Model_fAngVel_Body_X * rad2deg;
                sitldata.rollRate = aeroin.Model_fAngVel_Body_Y * rad2deg;
                sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z * rad2deg;

                sitldata.xAccel = aeroin.Model_fAccel_Body_X; // pitch
                sitldata.yAccel = aeroin.Model_fAccel_Body_Y; // roll
                sitldata.zAccel = aeroin.Model_fAccel_Body_Z;

               //     YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2]

                sitldata.altitude = aeroin.Model_fPosZ;
                sitldata.latitude = aeroin.Model_fLatitude;
                sitldata.longitude = aeroin.Model_fLongitude;

                sitldata.speedN = aeroin.Model_fVelX;
                sitldata.speedE = aeroin.Model_fVelY;

                float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
                float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;

                sitldata.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
            }
            else if (receviedbytes == 408)
            {

                FGNetFDM fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);

                lastfdmdata = fdm;

                sitldata.rollDeg = fdm.phi * rad2deg;
                sitldata.pitchDeg = fdm.theta * rad2deg;
                sitldata.yawDeg = fdm.psi * rad2deg;

                sitldata.rollRate = fdm.phidot * rad2deg;
                sitldata.pitchRate = fdm.thetadot * rad2deg;
                sitldata.yawRate = fdm.psidot * rad2deg;

                sitldata.xAccel = (fdm.A_X_pilot * 9.808 / 32.2); // pitch
                sitldata.yAccel =  (fdm.A_Y_pilot * 9.808 / 32.2); // roll
                sitldata.zAccel =  (fdm.A_Z_pilot / 32.2 * 9.808);

                sitldata.altitude = (fdm.altitude);
                sitldata.latitude = (fdm.latitude * rad2deg);
                sitldata.longitude = (fdm.longitude * rad2deg);

                sitldata.speedN = fdm.v_east * ft2m;
                sitldata.speedE = fdm.v_north * ft2m;

                sitldata.airspeed = fdm.vcas * 0.5144444f;//  knots to m/s

                if (RAD_JSBSim.Checked)
                    sitldata.airspeed = fdm.vcas * 0.3048f;//  fps to m/s

            }
            else
            {
                log.Info("Bad Udp Packet " + receviedbytes);
                return;
            }

            // write arduimu to ardupilot
            if (CHK_quad.Checked && !RAD_aerosimrc.Checked) // quad does its own
            {
                return;
            }

            if (RAD_JSBSim.Checked && chkSensor.Checked)
            {
                byte[] buffer = new byte[1500];
                while (JSBSimSEND.Client.Available > 5)
                {
                    int read = JSBSimSEND.Client.Receive(buffer);
                }

                byte[] sitlout = new byte[16 * 8 + 1 * 4]; // 16 * double + 1 * int
                int a = 0;

                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.latitude * rad2deg), a, sitlout, a, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.longitude * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.altitude), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psi * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.v_north * ft2m), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.v_east * ft2m), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_X_pilot * ft2m), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_Y_pilot * ft2m), 0, sitlout, a += 8, 8);

                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_Z_pilot * ft2m), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.phidot * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.thetadot * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psidot * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.phi * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.theta * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psi * rad2deg), 0, sitlout, a += 8, 8);
                Array.Copy(BitConverter.GetBytes((double)lastfdmdata.vcas * ft2m), 0, sitlout, a += 8, 8);

                //                Console.WriteLine(lastfdmdata.theta);

                Array.Copy(BitConverter.GetBytes((int)0x4c56414e), 0, sitlout, a += 8, 4);

                SITLSEND.Send(sitlout, sitlout.Length);

                return;
            }

            if (RAD_softXplanes.Checked && chkSensor.Checked)
            {
                sitldata.magic = (int)0x4c56414e;

                byte[] sendme = StructureToByteArray(sitldata);

                SITLSEND.Send(sendme, sendme.Length);

                return;
            }

            TimeSpan gpsspan = DateTime.Now - lastgpsupdate;

            // add gps delay
            if (gpsspan.TotalMilliseconds >= GPS_rate)
            {
                lastgpsupdate = DateTime.Now;

                // save current fix = 3
                sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;

                //                Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));

                // return buffer index + 5 = (3 + 5) = 8 % 6 = 2
                oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];

                //comPort.sendPacket(oldgps);

                gpsbufferindex++;
            }

            MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();

            hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec

            hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
            hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
            hilstate.alt = (int)(oldgps.altitude * 1000); // mm

             //   Console.WriteLine(hilstate.alt);

            hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
            hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
            hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
            hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
            hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
            hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)

            hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100
            hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100
            hilstate.vz = 0; // m/s * 100

            hilstate.xacc = (short)(sitldata.xAccel * 1000); // (mg)
            hilstate.yacc = (short)(sitldata.yAccel * 1000); // (mg)
            hilstate.zacc = (short)(sitldata.zAccel * 1000); // (mg)

            comPort.sendPacket(hilstate);

            //            comPort.sendPacket(oldgps);

            comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );

            MAVLink.mavlink_raw_pressure_t pres = new MAVLink.mavlink_raw_pressure_t();
            double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * sitldata.altitude, 5.25588)); // updated from valid gps
            pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa

               // comPort.sendPacket(pres);
        }