Exemple #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)
        {
            #if MAVLINK10
            ArdupilotMega.MAVLink.__mavlink_hil_state_t hilstate = new ArdupilotMega.MAVLink.__mavlink_hil_state_t();

            ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
            #else
            ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
            #endif
            ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t();

            ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();

            ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t();

            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
                }

                att.pitch = (DATA[18][0] * deg2rad);
                att.roll = (DATA[18][1] * deg2rad);
                att.yaw = (DATA[18][2] * deg2rad);
                att.pitchspeed = (DATA[17][0]);
                att.rollspeed = (DATA[17][1]);
                att.yawspeed = (DATA[17][2]);

                TimeSpan timediff = DateTime.Now - oldtime;

                float pdiff = (float)((att.pitch - oldatt.pitch) / timediff.TotalSeconds);
                float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds);
                float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds);

                //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);

                oldatt = att;

                rdiff = DATA[17][1];
                pdiff = DATA[17][0];
                ydiff = DATA[17][2];

                Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
                Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
                Int16 zgyro = Constrain(ydiff * 1000.0, Int16.MinValue, Int16.MaxValue);

                oldtime = DateTime.Now;

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

                float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(att.roll))) * ft2m;

                float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;

                //Console.WriteLine("old {0} {1} {2}",accel3D.X,accel3D.Y,accel3D.Z);

                YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel);

                accel3D -= cent3D;

                //Console.WriteLine("new {0} {1} {2}", accel3D.X, accel3D.Y, accel3D.Z);

                oldax = DATA[4][5];
                olday = DATA[4][6];
                oldaz = DATA[4][4];

                double head = DATA[18][2] - 90;
            #if MAVLINK10
                imu.time_usec = ((ulong)DateTime.Now.ToBinary());
                #else
                imu.usec = ((ulong)DateTime.Now.ToBinary());
                #endif
                imu.xgyro = xgyro; // roll - yes
                imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                imu.ygyro = ygyro; // pitch - yes
                imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
                imu.zgyro = zgyro;
                imu.zmag = 0;

                imu.xacc = (Int16)(accel3D.X * 1000); // pitch
                imu.yacc = (Int16)(accel3D.Y * 1000); // roll
                imu.zacc = (Int16)(accel3D.Z * 1000);

                //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
            #if MAVLINK10
                gps.alt = (int)(DATA[20][2] * ft2m * 1000);
                gps.fix_type = 3;
                gps.cog = (ushort)(DATA[19][2] * 100);
                gps.lat = (int)(DATA[20][0] * 1.0e7);
                gps.lon = (int)(DATA[20][1] * 1.0e7);
                gps.time_usec = ((ulong)0);
                gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100);
            #else
                gps.alt = ((float)(DATA[20][2] * ft2m));
                gps.fix_type = 3;
                gps.hdg = ((float)DATA[19][2]);
                gps.lat = ((float)DATA[20][0]);
                gps.lon = ((float)DATA[20][1]);
                gps.usec = ((ulong)0);
                gps.v = ((float)(DATA[3][7] * 0.44704));
            #endif

                asp.airspeed = ((float)(DATA[3][6] * 0.44704));

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

                object imudata = new fgIMUData();

                MAVLink.ByteArrayToStructureEndian(data, ref imudata, 0);

                imudata = (fgIMUData)(imudata);

                fgIMUData imudata2 = (fgIMUData)imudata;

                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 == 582)
            {
                aeroin = new TDataFromAeroSimRC();

                object temp = aeroin;

                MAVLink.ByteArrayToStructure(data, ref temp, 0);

                aeroin = (TDataFromAeroSimRC)(temp);

                att.pitch = (aeroin.Model_fPitch);
                att.roll = (aeroin.Model_fRoll * -1);
                att.yaw = (float)((aeroin.Model_fHeading));
                att.pitchspeed = (aeroin.Model_fAngVelX);
                att.rollspeed = (aeroin.Model_fAngVelY);
                att.yawspeed = (aeroin.Model_fAngVelZ);

            #if MAVLINK10
                imu.time_usec = ((ulong)DateTime.Now.ToBinary());
                #else
                imu.usec = ((ulong)DateTime.Now.ToBinary());
                #endif
                imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes
                //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes
                //imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
                imu.zgyro = (short)(aeroin.Model_fAngVelZ * 1000);
                //imu.zmag = 0;

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

                imu.xacc = (Int16)((accel3D.X + aeroin.Model_fAccelX) * 1000); // pitch
                imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll
                imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000);

                Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc);

            #if MAVLINK10
                gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
                gps.fix_type = 3;
                gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100);
                gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7);
                gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7);
                gps.time_usec = ((ulong)DateTime.Now.Ticks);
                gps.vel = (ushort)(Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)) * 100);
            #else
                gps.alt = ((float)(aeroin.Model_fPosZ));
                gps.fix_type = 3;
                gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg);
                gps.lat = ((float)aeroin.Model_fLatitude);
                gps.lon = ((float)aeroin.Model_fLongitude);
                gps.usec = ((ulong)DateTime.Now.Ticks);
                gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)));

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

                asp.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));

            }
            else if (receviedbytes > 0x100)
            {

                FGNetFDM fdm = new FGNetFDM();

                object temp = fdm;

                MAVLink.ByteArrayToStructureEndian(data, ref temp, 0);

                fdm = (FGNetFDM)(temp);

                lastfdmdata = fdm;

                att.roll = fdm.phi;
                att.pitch = fdm.theta;
                att.yaw = fdm.psi;

            #if MAVLINK10
                imu.time_usec = ((ulong)DateTime.Now.ToBinary());
                #else
                imu.usec = ((ulong)DateTime.Now.ToBinary());
                #endif
                imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes
                //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
                imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes
                //imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
                imu.zgyro = (short)(fdm.psidot * 1150);
                imu.zmag = 0;

                imu.xacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_X_pilot * 9808 / 32.2))); // pitch
                imu.yacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Y_pilot * 9808 / 32.2))); // roll
                imu.zacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Z_pilot / 32.2 * 9808)));

                //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
            #if MAVLINK10
                gps.alt = ((int)(fdm.altitude * ft2m * 1000));
                gps.fix_type = 3;
                gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100);
                gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7);
                gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7);
                gps.time_usec = ((ulong)DateTime.Now.Ticks);
                gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100);
            #else
                gps.alt = ((float)(fdm.altitude * ft2m));
                gps.fix_type = 3;
                gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360);
                //Console.WriteLine(gps.hdg);
                gps.lat = ((float)fdm.latitude * rad2deg);
                gps.lon = ((float)fdm.longitude * rad2deg);
                gps.usec = ((ulong)DateTime.Now.Ticks);
                gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m);

            #endif
                asp.airspeed = fdm.vcas * kts2fps * ft2m;
            }
            else
            {
                //FlightGear - old style udp

                DATA[20] = new float[8];

                DATA[18] = new float[8];

                DATA[19] = new float[8];

                DATA[3] = new float[8];

                // this text line is defined from ardupilot.xml
                string telem = Encoding.ASCII.GetString(data, 0, data.Length);

                try
                {
                    // should convert this to regex.... or just leave it.
                    int oldpos = 0;
                    int pos = telem.IndexOf(",");
                    DATA[20][0] = float.Parse(telem.Substring(oldpos, pos - 1), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf(",", pos + 1);
                    DATA[20][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf(",", pos + 1);
                    DATA[20][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf(",", pos + 1);
                    DATA[18][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf(",", pos + 1);
                    DATA[18][0] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf(",", pos + 1);
                    DATA[19][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));

                    oldpos = pos;
                    pos = telem.IndexOf("\n", pos + 1);
                    DATA[3][6] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
                    DATA[3][7] = DATA[3][6];
                }
                catch (Exception) { }

                chkSensor.Checked = false;

                att.pitch = (DATA[18][0]);
                att.roll = (DATA[18][1]);
                att.yaw = (DATA[19][2]);
            #if MAVLINK10
                gps.alt = ((int)(DATA[20][2] * ft2m * 1000));
                gps.fix_type = 3;
                gps.cog = (ushort)(DATA[18][2] * 100);
                gps.lat = (int)(DATA[20][0] * 1.0e7);
                gps.lon = (int)(DATA[20][1] * 1.0e7);
                gps.time_usec = ((ulong)0);
                gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100));
            #else
                gps.alt = ((float)(DATA[20][2] * ft2m));
                gps.fix_type = 3;
                gps.hdg = ((float)DATA[18][2]);
                gps.lat = ((float)DATA[20][0]);
                gps.lon = ((float)DATA[20][1]);
                gps.usec = ((ulong)0);
                gps.v = ((float)(DATA[3][7] * 0.44704));
            #endif

                asp.airspeed = ((float)(DATA[3][6] * 0.44704));
            }

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

            #if MAVLINK10

            TimeSpan gpsspan = DateTime.Now - lastgpsupdate;

            if (gpsspan.TotalMilliseconds >= GPS_rate)
            {
                lastgpsupdate = DateTime.Now;
                oldgps = gps;
                //comPort.sendPacket(gps);
            }

            hilstate.alt = oldgps.alt;
            hilstate.lat = oldgps.lat;
            hilstate.lon = oldgps.lon;
            hilstate.pitch = att.pitch;
            hilstate.pitchspeed = att.pitchspeed;
            hilstate.roll = att.roll;
            hilstate.rollspeed = att.rollspeed;
            hilstate.time_usec = gps.time_usec;
            hilstate.vx = (short)(gps.vel * Math.Sin(oldgps.cog / 100.0 * deg2rad));
            hilstate.vy = (short)(gps.vel * Math.Cos(oldgps.cog / 100.0 * deg2rad));
            hilstate.vz = 0;
            hilstate.xacc = imu.xacc;
            hilstate.yacc = imu.yacc;
            hilstate.yaw = att.yaw;
            hilstate.yawspeed = att.yawspeed;
            hilstate.zacc = imu.zacc;

            comPort.sendPacket(hilstate);

            comPort.sendPacket(asp);

            #else

            if (chkSensor.Checked == false) // attitude
            {
                comPort.sendPacket( att);

                comPort.sendPacket( asp);
            }
            else // raw imu
            {
                // imudata

                comPort.sendPacket( imu);

            #endif

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

            comPort.sendPacket(pres);
            #if !MAVLINK10
                comPort.sendPacket(asp);
            }

            TimeSpan gpsspan = DateTime.Now - lastgpsupdate;

            if (gpsspan.TotalMilliseconds >= GPS_rate)
            {
                lastgpsupdate = DateTime.Now;

                comPort.sendPacket(gps);
            }
            #endif
        }
        /*
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
        {
            UpdateCurrentSettings(bs, false, MainV2.comPort);
        }
        */
        public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
        {
            if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz
            {
                lastupdate = DateTime.Now;

                if (DateTime.Now.Second != lastwindcalc.Second)
                {
                    lastwindcalc = DateTime.Now;
                    dowindcalc();
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
                {

                    string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);

                    int ind = logdata.IndexOf('\0');
                    if (ind != -1)
                        logdata = logdata.Substring(0, ind);
                    if (messages.Count > 5)
                    {
                        messages.RemoveAt(0);
                    }
                    messages.Add(logdata + "\n");

                    mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil
                {
                    var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t();

                    object temp = hil;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6);

                    hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp);

                    hilch1 = hil.chan1_scaled;
                    hilch2 = hil.chan2_scaled;
                    hilch3 = hil.chan3_scaled;
                    hilch4 = hil.chan4_scaled;
                    hilch5 = hil.chan5_scaled;
                    hilch6 = hil.chan6_scaled;
                    hilch7 = hil.chan7_scaled;
                    hilch8 = hil.chan8_scaled;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
                {
                    MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t();

                    object temp = nav;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6);

                    nav = (MAVLink.__mavlink_nav_controller_output_t)(temp);

                    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;
                    xtrack_error = nav.xtrack_error;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
                }
            #if MAVLINK10
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();

                    object temp = hb;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);

                    hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);

                    string oldmode = mode;

                    mode = "Unknown";

                    if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0)
                    {
                        if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING)
                        {
                            switch (hb.custom_mode)
                            {
                                case (byte)(Common.apmmodes.MANUAL):
                                    mode = "Manual";
                                    break;
                                case (byte)(Common.apmmodes.GUIDED):
                                    mode = "Guided";
                                    break;
                                case (byte)(Common.apmmodes.STABILIZE):
                                    mode = "Stabilize";
                                    break;
                                case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
                                    mode = "FBW A";
                                    break;
                                case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
                                    mode = "FBW B";
                                    break;
                                case (byte)(Common.apmmodes.AUTO):
                                    mode = "Auto";
                                    break;
                                case (byte)(Common.apmmodes.RTL):
                                    mode = "RTL";
                                    break;
                                case (byte)(Common.apmmodes.LOITER):
                                    mode = "Loiter";
                                    break;
                                case (byte)(Common.apmmodes.CIRCLE):
                                    mode = "Circle";
                                    break;
                                default:
                                    mode = "Unknown";
                                    break;
                            }
                        }
                        else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR)
                        {
                            switch (hb.custom_mode)
                            {
                                case (byte)(Common.ac2modes.STABILIZE):
                                    mode = "Stabilize";
                                    break;
                                case (byte)(Common.ac2modes.ACRO):
                                    mode = "Acro";
                                    break;
                                case (byte)(Common.ac2modes.ALT_HOLD):
                                    mode = "Alt Hold";
                                    break;
                                case (byte)(Common.ac2modes.AUTO):
                                    mode = "Auto";
                                    break;
                                case (byte)(Common.ac2modes.GUIDED):
                                    mode = "Guided";
                                    break;
                                case (byte)(Common.ac2modes.LOITER):
                                    mode = "Loiter";
                                    break;
                                case (byte)(Common.ac2modes.RTL):
                                    mode = "RTL";
                                    break;
                                case (byte)(Common.ac2modes.CIRCLE):
                                    mode = "Circle";
                                    break;
                                default:
                                    mode = "Unknown";
                                    break;
                            }
                        }
                    }

                    if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                    }
                }

                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();

                    object temp = sysstatus;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);

                    sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);

                    battery_voltage = sysstatus.voltage_battery;
                    battery_remaining = sysstatus.battery_remaining;

                    packetdropremote = sysstatus.drop_rate_comm;

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                }
                #else

                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();

                    object temp = sysstatus;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);

                    sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);

                    string oldmode = mode;

                    switch (sysstatus.mode)
                    {
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
                                    mode = "Initialising";
                                    break;
                            }
                            break;
                        case (byte)(100 + Common.ac2modes.STABILIZE):
                            mode = "Stabilize";
                            break;
                        case (byte)(100 + Common.ac2modes.ACRO):
                            mode = "Acro";
                            break;
                        case (byte)(100 + Common.ac2modes.ALT_HOLD):
                            mode = "Alt Hold";
                            break;
                        case (byte)(100 + Common.ac2modes.AUTO):
                            mode = "Auto";
                            break;
                        case (byte)(100 + Common.ac2modes.GUIDED):
                            mode = "Guided";
                            break;
                        case (byte)(100 + Common.ac2modes.LOITER):
                            mode = "Loiter";
                            break;
                        case (byte)(100 + Common.ac2modes.RTL):
                            mode = "RTL";
                            break;
                        case (byte)(100 + Common.ac2modes.CIRCLE):
                            mode = "Circle";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
                            mode = "Manual";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
                            mode = "Guided";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
                            mode = "Stabilize";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
                            mode = "FBW A"; // fall though  old
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)1:
                                    mode = "FBW A";
                                    break;
                                case (byte)2:
                                    mode = "FBW B";
                                    break;
                                case (byte)3:
                                    mode = "FBW C";
                                    break;
                            }
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
                            mode = "FBW B";
                            break;
                        case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
                            switch (sysstatus.nav_mode)
                            {
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
                                    mode = "Auto";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
                                    mode = "RTL";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
                                    mode = "Loiter";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
                                    mode = "Takeoff";
                                    break;
                                case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
                                    mode = "Land";
                                    break;
                            }

                            break;
                        default:
                            mode = "Unknown";
                            break;
                    }

                    battery_voltage = sysstatus.vbat;
                    battery_remaining = sysstatus.battery_remaining;

                    packetdropremote = sysstatus.packet_drop;

                    if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
                }
            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
                {
                    var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();

                    object temp = att;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6);

                    att = (MAVLink.__mavlink_attitude_t)(temp);

                    roll = att.roll * rad2deg;
                    pitch = att.pitch * rad2deg;
                    yaw = att.yaw * rad2deg;

                    //                    Console.WriteLine(roll + " " + pitch + " " + yaw);

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
                }
            #if MAVLINK10
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_raw_int_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);

                    lat = gps.lat * 1.0e-7f;
                    lng = gps.lon * 1.0e-7f;
                    //                alt = gps.alt; // using vfr as includes baro calc

                    gpsstatus = gps.fix_type;
                    //                    Console.WriteLine("gpsfix {0}",gpsstatus);

                    gpshdop = gps.eph;

                    groundspeed = gps.vel * 1.0e-2f;
                    groundcourse = gps.cog * 1.0e-2f;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
                }
                #else

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_raw_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_raw_t)(temp);

                    lat = gps.lat;
                    lng = gps.lon;
                    //                alt = gps.alt; // using vfr as includes baro calc

                    gpsstatus = gps.fix_type;
                    //                    Console.WriteLine("gpsfix {0}",gpsstatus);

                    gpshdop = gps.eph;

                    groundspeed = gps.v;
                    groundcourse = gps.hdg;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
                }
            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
                {
                    var gps = new MAVLink.__mavlink_gps_status_t();

                    object temp = gps;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6);

                    gps = (MAVLink.__mavlink_gps_status_t)(temp);

                    satcount = gps.satellites_visible;
                }
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
                {
                    var loc = new MAVLink.__mavlink_global_position_int_t();

                    object temp = loc;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6);

                    loc = (MAVLink.__mavlink_global_position_int_t)(temp);

                    //alt = loc.alt / 1000.0f;
                    lat = loc.lat / 10000000.0f;
                    lng = loc.lon / 10000000.0f;

                }
            #if MAVLINK10
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();

                    object temp = wpcur;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);

                    wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);

                    int oldwp = (int)wpno;

                    wpno = wpcur.seq;

                    if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
                }
                #else
                   if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
                {
                    var loc = new MAVLink.__mavlink_global_position_t();

                    object temp = loc;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6);

                    loc = (MAVLink.__mavlink_global_position_t)(temp);

                    alt = loc.alt;
                    lat = loc.lat;
                    lng = loc.lon;

                }
                if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null)
                {
                    ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t();

                    object temp = wpcur;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6);

                    wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);

                    int oldwp = (int)wpno;

                    wpno = wpcur.seq;

                    if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
                    {
                        MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
                    }

                    //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
                }

            #endif
                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null)
                {
                    var rcin = new MAVLink.__mavlink_rc_channels_raw_t();

                    object temp = rcin;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6);

                    rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp);

                    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;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null)
                {
                    var servoout = new MAVLink.__mavlink_servo_output_raw_t();

                    object temp = servoout;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6);

                    servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp);

                    ch1out = servoout.servo1_raw;
                    ch2out = servoout.servo2_raw;
                    ch3out = servoout.servo3_raw;
                    ch4out = servoout.servo4_raw;
                    ch5out = servoout.servo5_raw;
                    ch6out = servoout.servo6_raw;
                    ch7out = servoout.servo7_raw;
                    ch8out = servoout.servo8_raw;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null)
                {
                    var imu = new MAVLink.__mavlink_raw_imu_t();

                    object temp = imu;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6);

                    imu = (MAVLink.__mavlink_raw_imu_t)(temp);

                    gx = imu.xgyro;
                    gy = imu.ygyro;
                    gz = imu.zgyro;

                    ax = imu.xacc;
                    ay = imu.yacc;
                    az = imu.zacc;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null)
                {
                    var imu = new MAVLink.__mavlink_scaled_imu_t();

                    object temp = imu;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6);

                    imu = (MAVLink.__mavlink_scaled_imu_t)(temp);

                    gx = imu.xgyro;
                    gy = imu.ygyro;
                    gz = imu.zgyro;

                    ax = imu.xacc;
                    ay = imu.yacc;
                    az = imu.zacc;

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
                {
                    MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();

                    object temp = vfr;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6);

                    vfr = (MAVLink.__mavlink_vfr_hud_t)(temp);

                    groundspeed = vfr.groundspeed;
                    airspeed = vfr.airspeed;

                    alt = vfr.alt; // this might include baro

                    //climbrate = vfr.climb;

                    if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt)
                    {
                        climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                        verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
                        if (float.IsInfinity(_verticalspeed))
                            _verticalspeed = 0;
                        lastalt = DateTime.Now;
                        oldalt = alt;
                    }

                    //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
                }

                if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil
                {
                    var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t();

                    object temp = mem;

                    MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6);

                    mem = (MAVLink.__mavlink_meminfo_t)(temp);

                    freemem = mem.freemem;
                    brklevel = mem.brkval;

                }

            }
            //Console.WriteLine(DateTime.Now.Millisecond + " start ");
            // update form
            try
            {
                if (bs != null)
                {
                    //Console.WriteLine(DateTime.Now.Millisecond);
                    bs.DataSource = this;
                    //Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow);
                    bs.ResetBindings(false);
                    //Console.WriteLine(DateTime.Now.Millisecond + " done ");
                }
            }
            catch { Console.WriteLine("CurrentState Binding error"); }
        }
Exemple #3
0
        public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm)
        {
            for (int i = 0; i < servos.Length; i++)
            {
                if (servos[i] <= 0.0)
                {
                    motor_speed[i] = 0;
                }
                else
                {
                    motor_speed[i] = scale_rc(i, (float)servos[i], 0.0f, 1.0f);
                    //servos[i] = motor_speed[i];
                }
            }
            double[] m = motor_speed;

            /*
            roll = 0;
            pitch = 0;
            yaw = 0;
            roll_rate = 0;
            pitch_rate = 0;
            yaw_rate = 0;
            */

            //            Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]);
            //            Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z);
            //Console.WriteLine("in r {0:0.000000} p {1:0.000000} y {2:0.000000}    - r {3:0.000000} p {4:0.000000} y {5:0.000000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate);

            //            m[0] *= 0.5;

            //# how much time has passed?
            DateTime t = DateTime.Now;
            TimeSpan delta_time = t - last_time; // 0.02
            last_time = t;

            if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug
            {
                delta_time = new TimeSpan(0, 0, 0, 0, 20);
            }

            //# rotational acceleration, in degrees/s/s
            double roll_accel = (m[1] - m[0]) * 5000.0;
            double pitch_accel = (m[2] - m[3]) * 5000.0;
            double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0;

             //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);

            //# update rotational rates
            roll_rate += roll_accel * delta_time.TotalSeconds;
            pitch_rate += pitch_accel * delta_time.TotalSeconds;
            yaw_rate += yaw_accel * delta_time.TotalSeconds;

            // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);

            //# update rotation
            roll += roll_rate * delta_time.TotalSeconds;
            pitch += pitch_rate * delta_time.TotalSeconds;
            yaw += yaw_rate * delta_time.TotalSeconds;

            //            Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);

            //Console.WriteLine("r {0:0.0} p {1:0.0} y {2:0.0}    - r {3:0.0} p {4:0.0} y {5:0.0}  ms {6:0.000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate, delta_time.TotalSeconds);

            //# air resistance
            Vector3d air_resistance = -velocity * (gravity / terminal_velocity);

            //# normalise rotations
            normalise();

            double thrust = (m[0] + m[1] + m[2] + m[3]) * thrust_scale;//# Newtons
            double accel = thrust / mass;

            //Console.WriteLine("in {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", roll, pitch, yaw, accel);

            Vector3d accel3D = RPY_to_XYZ(roll, pitch, yaw, accel);
            //Console.WriteLine("accel3D " + accel3D.X + " " + accel3D.Y + " " + accel3D.Z);
            accel3D += new Vector3d(0, 0, -gravity);
            accel3D += air_resistance;

            Random rand = new Random();
            int fixme;

            //velocity.X += .02 + rand.NextDouble() * .03;
            //velocity.Y += .02 + rand.NextDouble() * .03;

            //# new velocity vector
            velocity += accel3D * delta_time.TotalSeconds;
            this.accel = accel3D;

            //# new position vector
            old_position = new Vector3d(position);
            position += velocity * delta_time.TotalSeconds;

            //            Console.WriteLine(fdm.agl + " "+ fdm.altitude);

            //Console.WriteLine("Z {0} halt {1}  < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height);

            if (home_latitude == 0 || home_latitude > 90 || home_latitude < -90 || home_longitude == 0)
            {
                this.home_latitude = fdm.latitude * rad2deg;
                this.home_longitude = fdm.longitude * rad2deg;
                this.home_altitude = fdm.altitude * ft2m;
                this.ground_level = this.home_altitude;

                this.altitude = fdm.altitude * ft2m;
                this.latitude = fdm.latitude * rad2deg;
                this.longitude = fdm.longitude * rad2deg;
            }

            //# constrain height to the ground
            if (position.Z + home_altitude < ground_level + frame_height)
            {
                if (old_position.Z + home_altitude > ground_level + frame_height)
                {
                    //                    Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z));
                }
                velocity = new Vector3d(0, 0, 0);
                roll_rate = 0;
                pitch_rate = 0;
                yaw_rate = 0;
                roll = 0;
                pitch = 0;
                position = new Vector3d(position.X, position.Y,
                                               ground_level + frame_height - home_altitude + 0);
                // Console.WriteLine("here " + position.Z);
            }

            //# update lat/lon/altitude
            update_position();

            // send to apm
            #if MAVLINK10
            ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
            #else
            ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
            #endif

            ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();

            ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t();

            att.roll = (float)roll * deg2rad;
            att.pitch = (float)pitch * deg2rad;
            att.yaw = (float)yaw * deg2rad;
            att.rollspeed = (float)roll_rate * deg2rad;
            att.pitchspeed = (float)pitch_rate * deg2rad;
            att.yawspeed = (float)yaw_rate * deg2rad;

            #if MAVLINK10

            gps.alt = ((int)(altitude * 1000));
            gps.fix_type = 3;
            gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100);
            gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100);
            gps.lat = (int)(latitude* 1.0e7);
            gps.lon = (int)(longitude * 1.0e7);
            gps.time_usec = ((ulong)DateTime.Now.Ticks);

            asp.airspeed = gps.vel;

            #else
            gps.alt = ((float)(altitude));
            gps.fix_type = 3;

            gps.lat = ((float)latitude);
            gps.lon = ((float)longitude);
            gps.usec = ((ulong)DateTime.Now.Ticks);

            //Random rand = new Random();
            //gps.alt += (rand.Next(100) - 50) / 100.0f;
            //gps.lat += (float)((rand.NextDouble() - 0.5) * 0.00005);
            //gps.lon += (float)((rand.NextDouble() - 0.5) * 0.00005);
            //gps.v += (float)(rand.NextDouble() - 0.5) * 1.0f;

            gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
            gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ;

            asp.airspeed = gps.v;
            #endif

            MainV2.comPort.sendPacket(att);

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

            MainV2.comPort.sendPacket(pres);

            MainV2.comPort.sendPacket(asp);

            if (framecount % 12 == 0)
            {// 50 / 10 = 5 hz
                MainV2.comPort.sendPacket(gps);
                //Console.WriteLine(DateTime.Now.Millisecond + " GPS" );
            }

            framecount++;
        }