private void ConnectComPort_Click(object sender, EventArgs e) { if (threadrun == 0) { OutputLog.Clear(); if (MainV2.comPort.BaseStream.IsOpen == false) { // CustomMessageBox.Show("Please connect first"); // return; } try { // reset/create lastfdmdata = new FGNetFDM(); if (RAD_JSBSim.Checked) { simPort = 5124; recvPort = 5123; } SetupUDPRecv(); if (RAD_softXplanes.Checked) { SetupUDPXplanes(); SetupUDPMavLink(); } else { SetupUDPXplanes(); // fg udp style SetupUDPMavLink(); // pass traffic - raw } OutputLog.AppendText("Sim Link Started\n"); } catch (Exception ex) { OutputLog.AppendText("Socket setup problem. Do you have this open already? " + ex); } // set to highest to try prevent any timer issues var t11 = new Thread(mainloop) { Name = "Main simu Serial/UDP listener", IsBackground = true, Priority = ThreadPriority.Lowest }; t11.Start(); timer_servo_graph.Start(); } else { timer_servo_graph.Stop(); threadrun = 0; if (JSBSimSEND != null && JSBSimSEND.Connected) { try { JSBSimSEND.Client.Send(Encoding.ASCII.GetBytes("\n\nexit\n")); } catch { } } if (SimulatorRECV != null) SimulatorRECV.Close(); if (SimulatorRECV != null && SimulatorRECV.Connected) SimulatorRECV.Disconnect(true); if (SITLRCRECV != null) SITLRCRECV.Close(); if (MavLink != null) MavLink.Close(); position.Clear(); if (XplanesSEND != null) XplanesSEND.Close(); // if (comPort.BaseStream.IsOpen) // comPort.stopall(true); OutputLog.AppendText("Sim Link Stopped\n"); Thread.Sleep(1000); Application.DoEvents(); } }
/// <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, MAVLinkInterface 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]); 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]; } 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.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]; } sitldata.airspeed = ((DATA[3][5] * .44704)); sitldata.latitude = (DATA[20][0]); sitldata.longitude = (DATA[20][1]); sitldata.altitude = (DATA[20][2] * ft2m); 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(); DateTime epochBegin = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc); hilstate.time_usec = (UInt64)((DateTime.Now.Ticks - epochBegin.Ticks) / 10); // 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.IsOpen) return; 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); }
/// <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); }
public void update(ref double[] servos, FGNetFDM fdm) { for (int i = 0; i < servos.Length; i++) { var servo = servos[(int) self.motors[i].servo]; if (servo <= 0.0) { motor_speed[i] = 0; } else { motor_speed[i] = scale_rc(i, (float) servo, 0.0f, 1.0f); //servos[i] = motor_speed[i]; } } double[] m = motor_speed; //# how much time has passed? //DateTime t = DateTime.Now; //TimeSpan delta_time = t - last_time; // 0.02 //last_time = t; // run at 1000hz lockstep TimeSpan delta_time = new TimeSpan(0, 0, 0, 0, 1); if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug { delta_time = new TimeSpan(0, 0, 0, 0, 20); } // rotational acceleration, in degrees/s/s, in body frame Vector3 rot_accel = new Vector3(0, 0, 0); double thrust = 0.0; foreach (var i in range((self.motors.Length))) { rot_accel.x += -radians(5000.0)*sin(radians(self.motors[i].angle))*m[i]; rot_accel.y += radians(5000.0)*cos(radians(self.motors[i].angle))*m[i]; if (!self.motors[i].clockwise) { rot_accel.z -= m[i]*radians(400.0); } else { rot_accel.z += m[i]*radians(400.0); } thrust += m[i]*self.thrust_scale; // newtons } //Console.WriteLine("rot_accel " + rot_accel.ToString()); // rotational air resistance rot_accel.x -= self.gyro.x*radians(5000.0)/self.terminal_rotation_rate; rot_accel.y -= self.gyro.y*radians(5000.0)/self.terminal_rotation_rate; rot_accel.z -= self.gyro.z*radians(400.0)/self.terminal_rotation_rate; // Console.WriteLine("rot_accel " + rot_accel.ToString()); // update rotational rates in body frame self.gyro += rot_accel*delta_time.TotalSeconds; // Console.WriteLine("gyro " + gyro.ToString()); // update attitude self.dcm.rotate(self.gyro*delta_time.TotalSeconds); self.dcm.normalize(); // air resistance Vector3 air_resistance = -self.velocity*(self.gravity/self.terminal_velocity); accel_body = new Vector3(0, 0, -thrust/self.mass); Vector3 accel_earth = self.dcm*accel_body; accel_earth += new Vector3(0, 0, self.gravity); accel_earth += air_resistance; // add in some wind (turn force into accel by dividing by mass). // accel_earth += self.wind.drag(self.velocity) / self.mass; // if we're on the ground, then our vertical acceleration is limited // to zero. This effectively adds the force of the ground on the aircraft if (self.on_ground() && accel_earth.z > 0) accel_earth.z = 0; // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity self.accel_body = self.dcm.transposed()*(accel_earth + new Vector3(0, 0, -self.gravity)); // new velocity vector self.velocity += accel_earth*delta_time.TotalSeconds; if (double.IsNaN(velocity.x) || double.IsNaN(velocity.y) || double.IsNaN(velocity.z)) velocity = new Vector3(); // new position vector old_position = self.position.copy(); self.position += self.velocity*delta_time.TotalSeconds; if (home_latitude == 0) { home_latitude = fdm.latitude*rad2deg; home_longitude = fdm.longitude*rad2deg; home_altitude = fdm.altitude; ground_level = home_altitude; } // constrain height to the ground if (self.on_ground()) { if (!self.on_ground(old_position)) Console.WriteLine("Hit ground at {0} m/s", (self.velocity.z)); self.velocity = new Vector3(0, 0, 0); // zero roll/pitch, but keep yaw double r = 0; double p = 0; double y = 0; self.dcm.to_euler(ref r, ref p, ref y); self.dcm.from_euler(0, 0, y); self.position = new Vector3(self.position.x, self.position.y, -(self.ground_level + self.frame_height - self.home_altitude)); } // update lat/lon/altitude self.update_position(); }
public void update(ref double[] servos, FGNetFDM fdm) { for (int i = 0; i < servos.Length; i++) { var servo = servos[(int)self.motors[i].servo]; if (servo <= 0.0) { motor_speed[i] = 0; } else { motor_speed[i] = scale_rc(i, (float)servo, 0.0f, 1.0f); //servos[i] = motor_speed[i]; } } double[] m = motor_speed; //# how much time has passed? //DateTime t = DateTime.Now; //TimeSpan delta_time = t - last_time; // 0.02 //last_time = t; // run at 1000hz lockstep TimeSpan delta_time = new TimeSpan(0, 0, 0, 0, 1); if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug { delta_time = new TimeSpan(0, 0, 0, 0, 20); } // rotational acceleration, in degrees/s/s, in body frame Vector3 rot_accel = new Vector3(0, 0, 0); double thrust = 0.0; foreach (var i in range((self.motors.Length))) { rot_accel.x += -radians(5000.0) * sin(radians(self.motors[i].angle)) * m[i]; rot_accel.y += radians(5000.0) * cos(radians(self.motors[i].angle)) * m[i]; if (!self.motors[i].clockwise) { rot_accel.z -= m[i] * radians(400.0); } else { rot_accel.z += m[i] * radians(400.0); } thrust += m[i] * self.thrust_scale; // newtons } //Console.WriteLine("rot_accel " + rot_accel.ToString()); // rotational air resistance rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate; rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate; rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate; // Console.WriteLine("rot_accel " + rot_accel.ToString()); // update rotational rates in body frame self.gyro += rot_accel * delta_time.TotalSeconds; // Console.WriteLine("gyro " + gyro.ToString()); // update attitude self.dcm.rotate(self.gyro * delta_time.TotalSeconds); self.dcm.normalize(); // air resistance Vector3 air_resistance = -self.velocity * (self.gravity / self.terminal_velocity); accel_body = new Vector3(0, 0, -thrust / self.mass); Vector3 accel_earth = self.dcm * accel_body; accel_earth += new Vector3(0, 0, self.gravity); accel_earth += air_resistance; // add in some wind (turn force into accel by dividing by mass). // accel_earth += self.wind.drag(self.velocity) / self.mass; // if we're on the ground, then our vertical acceleration is limited // to zero. This effectively adds the force of the ground on the aircraft if (self.on_ground() && accel_earth.z > 0) { accel_earth.z = 0; } // work out acceleration as seen by the accelerometers. It sees the kinematic // acceleration (ie. real movement), plus gravity self.accel_body = self.dcm.transposed() * (accel_earth + new Vector3(0, 0, -self.gravity)); // new velocity vector self.velocity += accel_earth * delta_time.TotalSeconds; if (double.IsNaN(velocity.x) || double.IsNaN(velocity.y) || double.IsNaN(velocity.z)) { velocity = new Vector3(); } // new position vector old_position = self.position.copy(); self.position += self.velocity * delta_time.TotalSeconds; if (home_latitude == 0) { home_latitude = fdm.latitude * rad2deg; home_longitude = fdm.longitude * rad2deg; home_altitude = fdm.altitude; ground_level = home_altitude; } // constrain height to the ground if (self.on_ground()) { if (!self.on_ground(old_position)) { Console.WriteLine("Hit ground at {0} m/s", (self.velocity.z)); } self.velocity = new Vector3(0, 0, 0); // zero roll/pitch, but keep yaw double r = 0; double p = 0; double y = 0; self.dcm.to_euler(ref r, ref p, ref y); self.dcm.from_euler(0, 0, y); self.position = new Vector3(self.position.x, self.position.y, -(self.ground_level + self.frame_height - self.home_altitude)); } // update lat/lon/altitude self.update_position(); }
private void ConnectComPort_Click(object sender, EventArgs e) { if (threadrun == 0) { OutputLog.Clear(); if (MainV2.comPort.BaseStream.IsOpen == false) { // CustomMessageBox.Show("Please connect first"); // return; } try { // reset/create lastfdmdata = new FGNetFDM(); quad = new HIL.MultiCopter(); if (RAD_JSBSim.Checked) { simPort = 5124; recvPort = 5123; } SetupUDPRecv(); if (chkSITL.Checked) { SITLRCRECV = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp); SITLRCRECV.Bind(new IPEndPoint(IPAddress.Any, 5502)); OutputLog.AppendText("SITL rc from " + SITLIP + " port " + 5502 + " \n"); SITLSEND = new UdpClient(SITLIP, 5501); OutputLog.AppendText("SITL to " + SITLIP + " port " + 5501 + " \n"); } if (RAD_softXplanes.Checked) { SetupUDPXplanes(); SetupUDPMavLink(); } else { if (RAD_JSBSim.Checked) { System.Diagnostics.ProcessStartInfo _procstartinfo = new System.Diagnostics.ProcessStartInfo(); _procstartinfo.WorkingDirectory = Path.GetDirectoryName(Application.ExecutablePath); _procstartinfo.Arguments = "--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=jsbsim/fgout.xml --script=jsbsim/rascal_test.xml"; _procstartinfo.FileName = "JSBSim.exe"; // Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + _procstartinfo.UseShellExecute = true; //_procstartinfo.RedirectStandardOutput = true; System.Diagnostics.Process.Start(_procstartinfo); System.Threading.Thread.Sleep(2000); SetupTcpJSBSim(); } SetupUDPXplanes(); // fg udp style SetupUDPMavLink(); // pass traffic - raw } OutputLog.AppendText("Sim Link Started\n"); } catch (Exception ex) { OutputLog.AppendText("Socket setup problem. Do you have this open already? " + ex.ToString()); } // set to highest to try prevent any timer issues System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) { Name = "Main simu Serial/UDP listener", IsBackground = true, Priority = System.Threading.ThreadPriority.Lowest }; t11.Start(); timer_servo_graph.Start(); } else { timer_servo_graph.Stop(); threadrun = 0; if (JSBSimSEND != null && JSBSimSEND.Connected) { try { JSBSimSEND.Client.Send(ASCIIEncoding.ASCII.GetBytes("\n\nexit\n")); } catch { } } if (SimulatorRECV != null) SimulatorRECV.Close(); if (SimulatorRECV != null && SimulatorRECV.Connected) SimulatorRECV.Disconnect(true); if (SITLRCRECV != null) SITLRCRECV.Close(); if (MavLink != null) MavLink.Close(); position.Clear(); if (XplanesSEND != null) XplanesSEND.Close(); // if (comPort.BaseStream.IsOpen) // comPort.stopall(true); OutputLog.AppendText("Sim Link Stopped\n"); System.Threading.Thread.Sleep(1000); Application.DoEvents(); } }
/// <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 }