public override void SendToAP(sitl_fdm sitldata) { 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(); Simudataform Simudata = new Simudataform(); hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec hilstate.lat = (int)(Simudataform.test_lat * 1e7); // * 1E7 修改为Home经纬度、高度 hilstate.lon = (int)(Simudataform.test_lon * 1e7); // * 1E7 hilstate.alt = (int)(Simudataform.test_alt * 1000); // mm // Console.WriteLine(hilstate.alt); hilstate.rollspeed = Simudataform.test_wind_speed; // (rad/s) 修改为风速 hilstate.pitchspeed = Simudataform.test_wind_direction; // (rad/s) 修改为风向 hilstate.yawspeed = Simudataform.test_wind_turbulance; // (rad/s) 修改为风的扰动 hilstate.roll = Simudataform.test_frame_mass; // (rad) 修改为机身质量 hilstate.pitch = Simudataform.test_frame_height; // (rad) 修改为机身高度 hilstate.yaw = (float)(sitldata.yawDeg * MathHelper.deg2rad); // (rad) 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) MainV2.comPort.sendPacket(hilstate, MainV2.comPort.sysidcurrent, MainV2.comPort.compidcurrent); MainV2.comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed }, MainV2.comPort.sysidcurrent, MainV2.comPort.compidcurrent); }
public override void SendToAP(sitl_fdm sitldata) { 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) MainV2.comPort.sendPacket(hilstate); MainV2.comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed }); }
/// <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); }
private void processArduPilot() { bool heli = CHK_heli.Checked; if (CHK_quad.Checked && !RAD_aerosimrc.Checked) { double[] m = new double[4]; // 11 for (int a = 0; a < m.Length; a++) m[a] = 0; m[0] = (ushort)MainV2.comPort.MAV.cs.ch1out; m[1] = (ushort)MainV2.comPort.MAV.cs.ch2out; m[2] = (ushort)MainV2.comPort.MAV.cs.ch3out; m[3] = (ushort)MainV2.comPort.MAV.cs.ch4out; if (!RAD_softFlightGear.Checked) { lastfdmdata.latitude = DATA[20][0] * deg2rad; lastfdmdata.longitude = DATA[20][1] * deg2rad; lastfdmdata.altitude = (DATA[20][2]); lastfdmdata.version = 999; } try { if (lastfdmdata.version == 0) return; quad.update(ref m, lastfdmdata); Vector3 earth_rates = Utils.BodyRatesToEarthRates(quad.dcm, quad.gyro); quad.dcm.to_euler(ref quad.roll, ref quad.pitch, ref quad.yaw); //quad.dcm.to_euler(ref roll, ref pitch, ref yaw); if (chkSITL.Checked) { sitl_fdm sitldata = new sitl_fdm(); sitldata.latitude = quad.latitude; sitldata.longitude = quad.longitude; sitldata.altitude = quad.altitude; sitldata.heading = quad.yaw; sitldata.speedN = quad.velocity.x; sitldata.speedE = quad.velocity.y; sitldata.speedD = quad.velocity.z; sitldata.xAccel = quad.accelerometer.x; sitldata.yAccel = quad.accelerometer.y; sitldata.zAccel = quad.accelerometer.z; sitldata.rollDeg = quad.roll * rad2deg; sitldata.pitchDeg = quad.pitch * rad2deg; sitldata.yawDeg = quad.yaw * rad2deg; sitldata.rollRate = earth_rates.x * rad2deg; sitldata.pitchRate = earth_rates.y * rad2deg; sitldata.yawRate = earth_rates.z * rad2deg; sitldata.airspeed = ((float)Math.Sqrt((sitldata.speedN * sitldata.speedN) + (sitldata.speedE * sitldata.speedE))); ; sitldata.magic = (int)0x4c56414f; byte[] sendme = StructureToByteArray(sitldata); SITLSEND.Send(sendme, sendme.Length); byte[] rcreceiver = new byte[2 * 8]; Array.ConstrainedCopy(BitConverter.GetBytes((ushort)MainV2.comPort.MAV.cs.rcoverridech1), 0, rcreceiver, 0, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)MainV2.comPort.MAV.cs.rcoverridech2), 0, rcreceiver, 2, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)MainV2.comPort.MAV.cs.rcoverridech3), 0, rcreceiver, 4, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)MainV2.comPort.MAV.cs.rcoverridech4), 0, rcreceiver, 6, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)1505), 0, rcreceiver, 8, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)1506), 0, rcreceiver, 10, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)1507), 0, rcreceiver, 12, 2); Array.ConstrainedCopy(BitConverter.GetBytes((ushort)1508), 0, rcreceiver, 14, 2); SITLSEND.Send(rcreceiver, rcreceiver.Length); } else { // send to apm MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t(); hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec hilstate.lat = (int)(quad.latitude * 1e7); // * 1E7 hilstate.lon = (int)(quad.longitude * 1e7); // * 1E7 hilstate.alt = (int)(quad.altitude * 1000); // mm quad.dcm.to_euler(ref quad.roll, ref quad.pitch, ref quad.yaw); if (double.IsNaN(quad.roll)) { quad.dcm.identity(); } hilstate.roll = (float)quad.roll; hilstate.pitch = (float)quad.pitch; hilstate.yaw = (float)quad.yaw; //Vector3 earth_rates2 = Utils.BodyRatesToEarthRates(quad.dcm, quad.gyro); hilstate.rollspeed = (float)quad.gyro.x; hilstate.pitchspeed = (float)quad.gyro.y; hilstate.yawspeed = (float)quad.gyro.z; //hilstate.rollspeed = (float)earth_rates2.x; //hilstate.pitchspeed = (float)earth_rates2.y; //hilstate.yawspeed = (float)earth_rates2.z; hilstate.vx = (short)(quad.velocity.y * 100); // m/s * 100 hilstate.vy = (short)(quad.velocity.x * 100); // m/s * 100 hilstate.vz = (short)(quad.velocity.z * 100); // m/s * 100 hilstate.xacc = (short)(quad.accelerometer.x * 100); // (mg) hilstate.yacc = (short)(quad.accelerometer.y * 100); // (mg) hilstate.zacc = (short)(quad.accelerometer.z * 100); // (mg) MainV2.comPort.sendPacket(hilstate); } } catch (Exception e) { log.Info("Quad hil error " + e.ToString()); } byte[] FlightGear = new byte[8 * 11];// StructureToByteArray(fg); Array.Copy(BitConverter.GetBytes((double)(quad.motor_speed[0])), 0, FlightGear, 0, 8); Array.Copy(BitConverter.GetBytes((double)(quad.motor_speed[1])), 0, FlightGear, 8, 8); Array.Copy(BitConverter.GetBytes((double)(quad.motor_speed[2])), 0, FlightGear, 16, 8); Array.Copy(BitConverter.GetBytes((double)(quad.motor_speed[3])), 0, FlightGear, 24, 8); Array.Copy(BitConverter.GetBytes((double)(quad.latitude)), 0, FlightGear, 32, 8); Array.Copy(BitConverter.GetBytes((double)(quad.longitude)), 0, FlightGear, 40, 8); Array.Copy(BitConverter.GetBytes((double)(quad.altitude * 1 / ft2m)), 0, FlightGear, 48, 8); Array.Copy(BitConverter.GetBytes((double)((quad.altitude - quad.ground_level) * 1 / ft2m)), 0, FlightGear, 56, 8); Array.Copy(BitConverter.GetBytes((double)(quad.roll * rad2deg)), 0, FlightGear, 64, 8); Array.Copy(BitConverter.GetBytes((double)(quad.pitch * rad2deg)), 0, FlightGear, 72, 8); Array.Copy(BitConverter.GetBytes((double)(quad.yaw * rad2deg)), 0, FlightGear, 80, 8); if (RAD_softFlightGear.Checked || RAD_softXplanes.Checked) { Array.Reverse(FlightGear, 0, 8); Array.Reverse(FlightGear, 8, 8); Array.Reverse(FlightGear, 16, 8); Array.Reverse(FlightGear, 24, 8); Array.Reverse(FlightGear, 32, 8); Array.Reverse(FlightGear, 40, 8); Array.Reverse(FlightGear, 48, 8); Array.Reverse(FlightGear, 56, 8); Array.Reverse(FlightGear, 64, 8); Array.Reverse(FlightGear, 72, 8); Array.Reverse(FlightGear, 80, 8); } try { XplanesSEND.Send(FlightGear, FlightGear.Length); } catch (Exception) { log.Info("Socket Write failed, FG closed?"); } updateScreenDisplay(lastfdmdata.latitude, lastfdmdata.longitude, lastfdmdata.altitude * .3048, lastfdmdata.phi, lastfdmdata.theta, lastfdmdata.psi, lastfdmdata.psi, m[0], m[1], m[2], m[3]); return; } float roll_out, pitch_out, throttle_out, rudder_out, collective_out; collective_out = 0; if (heli) { roll_out = (float)MainV2.comPort.MAV.cs.hilch1 / rollgain; pitch_out = (float)MainV2.comPort.MAV.cs.hilch2 / pitchgain; throttle_out = 1; rudder_out = (float)MainV2.comPort.MAV.cs.hilch4 / -ruddergain; collective_out = (float)(MainV2.comPort.MAV.cs.hilch3 - 1500) / throttlegain; } else { roll_out = (float)MainV2.comPort.MAV.cs.hilch1 / rollgain; pitch_out = (float)MainV2.comPort.MAV.cs.hilch2 / pitchgain; throttle_out = ((float)MainV2.comPort.MAV.cs.hilch3) / throttlegain; rudder_out = (float)MainV2.comPort.MAV.cs.hilch4 / ruddergain; if (RAD_aerosimrc.Checked && CHK_quad.Checked) { throttle_out = ((float)MainV2.comPort.MAV.cs.hilch7 / 2 + 5000) / throttlegain; throttle_out = (float)(MainV2.comPort.MAV.cs.hilch3 - 1100) / throttlegain; } } // Limit min and max roll_out = Constrain(roll_out, -1, 1); pitch_out = Constrain(pitch_out, -1, 1); rudder_out = Constrain(rudder_out, -1, 1); throttle_out = Constrain(throttle_out, 0, 1); try { if (displayfull) { // This updates the servo graphs double time = (Environment.TickCount - tickStart) / 1000.0; if (CHKgraphroll.Checked) { list.Add(time, roll_out); } else { list.Clear(); } if (CHKgraphpitch.Checked) { list2.Add(time, pitch_out); } else { list2.Clear(); } if (CHKgraphrudder.Checked) { list3.Add(time, rudder_out); } else { list3.Clear(); } if (CHKgraphthrottle.Checked) { if (heli) { list4.Add(time, collective_out); } else { list4.Add(time, throttle_out); } } else { list4.Clear(); } } if (packetssent % 10 == 0) // reduce cpu usage { if (RAD_softXplanes.Checked) { bool xplane9 = !CHK_xplane10.Checked; if (xplane9) { updateScreenDisplay(DATA[20][0] * deg2rad, DATA[20][1] * deg2rad, DATA[20][2] * .3048, DATA[18][1] * deg2rad, DATA[18][0] * deg2rad, DATA[19][2] * deg2rad, DATA[18][2] * deg2rad, roll_out, pitch_out, rudder_out, throttle_out); } else { updateScreenDisplay(DATA[20][0] * deg2rad, DATA[20][1] * deg2rad, DATA[20][2] * .3048, DATA[17][1] * deg2rad, DATA[17][0] * deg2rad, DATA[18][2] * deg2rad, DATA[17][2] * deg2rad, roll_out, pitch_out, rudder_out, throttle_out); } } if (RAD_softFlightGear.Checked || RAD_JSBSim.Checked) { updateScreenDisplay(lastfdmdata.latitude, lastfdmdata.longitude, lastfdmdata.altitude * .3048, lastfdmdata.phi, lastfdmdata.theta, lastfdmdata.psi, lastfdmdata.psi, roll_out, pitch_out, rudder_out, throttle_out); } if (RAD_aerosimrc.Checked) { if (heli) updateScreenDisplay(aeroin.Model_fLatitude * deg2rad, aeroin.Model_fLongitude * deg2rad, aeroin.Model_fPosZ, aeroin.Model_fRoll, aeroin.Model_fPitch, aeroin.Model_fHeading, aeroin.Model_fHeading, roll_out, pitch_out, rudder_out, collective_out); else updateScreenDisplay(aeroin.Model_fLatitude * deg2rad, aeroin.Model_fLongitude * deg2rad, aeroin.Model_fPosZ, aeroin.Model_fRoll, aeroin.Model_fPitch, aeroin.Model_fHeading, aeroin.Model_fHeading, roll_out, pitch_out, rudder_out, throttle_out); } } } catch (Exception e) { log.Info("Error updateing screen stuff " + e.ToString()); } packetssent++; if (RAD_aerosimrc.Checked) { //AeroSimRC byte[] AeroSimRC = new byte[4 * 8];// StructureToByteArray(fg); Array.Copy(BitConverter.GetBytes((double)(roll_out * REV_roll)), 0, AeroSimRC, 0, 8); Array.Copy(BitConverter.GetBytes((double)(pitch_out * REV_pitch * -1)), 0, AeroSimRC, 8, 8); Array.Copy(BitConverter.GetBytes((double)(rudder_out * REV_rudder)), 0, AeroSimRC, 16, 8); Array.Copy(BitConverter.GetBytes((double)((throttle_out * 2) - 1)), 0, AeroSimRC, 24, 8); if (heli) { Array.Copy(BitConverter.GetBytes((double)(collective_out)), 0, AeroSimRC, 24, 8); } if (CHK_quad.Checked) { //MainV2.comPort.MAV.cs.ch1out = 1100; //MainV2.comPort.MAV.cs.ch2out = 1100; //MainV2.comPort.MAV.cs.ch3out = 1100; //MainV2.comPort.MAV.cs.ch4out = 1100; //ac // 3 front // 1 left // 4 back // 2 left // Array.Copy(BitConverter.GetBytes((double)((MainV2.comPort.MAV.cs.ch3out - 1100) / 800 * 2 - 1)), 0, AeroSimRC, 0, 8); // motor 1 = front // Array.Copy(BitConverter.GetBytes((double)((MainV2.comPort.MAV.cs.ch1out - 1100) / 800 * 2 - 1)), 0, AeroSimRC, 8, 8); // motor 2 = right // Array.Copy(BitConverter.GetBytes((double)((MainV2.comPort.MAV.cs.ch4out - 1100) / 800 * 2 - 1)), 0, AeroSimRC, 16, 8);// motor 3 = back // Array.Copy(BitConverter.GetBytes((double)((MainV2.comPort.MAV.cs.ch2out - 1100) / 800 * 2 - 1)), 0, AeroSimRC, 24, 8);// motor 4 = left } else { } try { SimulatorRECV.SendTo(AeroSimRC, Remote); } catch { } } //JSBSim if (RAD_JSBSim.Checked) { roll_out = Constrain(roll_out * REV_roll, -1f, 1f); pitch_out = Constrain(-pitch_out * REV_pitch, -1f, 1f); rudder_out = Constrain(rudder_out * REV_rudder, -1f, 1f); throttle_out = Constrain(throttle_out, -0.0f, 1f); string cmd = string.Format("set fcs/aileron-cmd-norm {0}\r\nset fcs/elevator-cmd-norm {1}\r\nset fcs/rudder-cmd-norm {2}\r\nset fcs/throttle-cmd-norm {3}\r\n", roll_out, pitch_out, rudder_out, throttle_out); //Console.Write(cmd); byte[] data = System.Text.Encoding.ASCII.GetBytes(cmd); JSBSimSEND.Client.Send(data); } // Flightgear if (RAD_softFlightGear.Checked) { //if (packetssent % 2 == 0) { return; } // short supply buffer.. seems to reduce lag byte[] FlightGear = new byte[4 * 8];// StructureToByteArray(fg); Array.Copy(BitConverter.GetBytes((double)(roll_out * REV_roll)), 0, FlightGear, 0, 8); Array.Copy(BitConverter.GetBytes((double)(pitch_out * REV_pitch * -1)), 0, FlightGear, 8, 8); Array.Copy(BitConverter.GetBytes((double)(rudder_out * REV_rudder)), 0, FlightGear, 16, 8); Array.Copy(BitConverter.GetBytes((double)(throttle_out)), 0, FlightGear, 24, 8); Array.Reverse(FlightGear, 0, 8); Array.Reverse(FlightGear, 8, 8); Array.Reverse(FlightGear, 16, 8); Array.Reverse(FlightGear, 24, 8); try { XplanesSEND.Send(FlightGear, FlightGear.Length); } catch (Exception) { log.Info("Socket Write failed, FG closed?"); } } // Xplanes if (RAD_softXplanes.Checked) { // sending only 1 packet instead of many. byte[] Xplane = new byte[5 + 36 + 36]; if (heli) { Xplane = new byte[5 + 36 + 36 + 36]; } Xplane[0] = (byte)'D'; Xplane[1] = (byte)'A'; Xplane[2] = (byte)'T'; Xplane[3] = (byte)'A'; Xplane[4] = 0; Array.Copy(BitConverter.GetBytes((int)25), 0, Xplane, 5, 4); // packet index Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 9, 4); // start data Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 13, 4); Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 17, 4); Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 21, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 25, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 29, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 33, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 37, 4); // NEXT ONE - control surfaces Array.Copy(BitConverter.GetBytes((int)11), 0, Xplane, 41, 4); // packet index Array.Copy(BitConverter.GetBytes((float)(pitch_out * REV_pitch)), 0, Xplane, 45, 4); // start data Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll)), 0, Xplane, 49, 4); Array.Copy(BitConverter.GetBytes((float)(rudder_out * REV_rudder)), 0, Xplane, 53, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 57, 4); Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll * 0.5)), 0, Xplane, 61, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 65, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 69, 4); Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 73, 4); if (heli) { Array.Copy(BitConverter.GetBytes((float)(0)), 0, Xplane, 53, 4); int a = 73 + 4; Array.Copy(BitConverter.GetBytes((int)39), 0, Xplane, a, 4); // packet index a += 4; Array.Copy(BitConverter.GetBytes((float)(12 * collective_out)), 0, Xplane, a, 4); // main rotor 0 - 12 a += 4; Array.Copy(BitConverter.GetBytes((float)(12 * rudder_out)), 0, Xplane, a, 4); // tail rotor -12 - 12 a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); a += 4; Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4); } try { XplanesSEND.Send(Xplane, Xplane.Length); } catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); } } }
public override void GetFromSim(ref sitl_fdm sitldata) { if (SimulatorRECV.Available > 0) { byte[] udpdata = new byte[1500]; int receviedbytes = 0; try { while (SimulatorRECV.Available > 0) { receviedbytes = SimulatorRECV.ReceiveFrom(udpdata, ref Remote); } } catch { } if (udpdata[0] == 'D' && udpdata[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(udpdata, count); DATA[index] = new float[8]; DATA[index][0] = BitConverter.ToSingle(udpdata, count + 1 * 4); ; DATA[index][1] = BitConverter.ToSingle(udpdata, count + 2 * 4); ; DATA[index][2] = BitConverter.ToSingle(udpdata, count + 3 * 4); ; DATA[index][3] = BitConverter.ToSingle(udpdata, count + 4 * 4); ; DATA[index][4] = BitConverter.ToSingle(udpdata, count + 5 * 4); ; DATA[index][5] = BitConverter.ToSingle(udpdata, count + 6 * 4); ; DATA[index][6] = BitConverter.ToSingle(udpdata, count + 7 * 4); ; DATA[index][7] = BitConverter.ToSingle(udpdata, count + 8 * 4); ; count += 36; // 8 * float } bool xplane9 = !xplane10; 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); } } }
public override void SendToAP(sitl_fdm sitldata) { 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) MainV2.comPort.sendPacket(hilstate); MainV2.comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed }); }
public abstract void SendToAP(sitl_fdm data);
public abstract void GetFromSim(ref sitl_fdm data);
public override void GetFromSim(ref sitl_fdm sitldata) { if (SimulatorRECV.Available > 0) { byte[] udpdata = new byte[1500]; int receviedbytes = 0; try { while (SimulatorRECV.Available > 0) { receviedbytes = SimulatorRECV.ReceiveFrom(udpdata, ref Remote); } } catch { } if (udpdata[0] == 'D' && udpdata[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(udpdata, count); DATA[index] = new float[8]; DATA[index][0] = BitConverter.ToSingle(udpdata, count + 1 * 4);; DATA[index][1] = BitConverter.ToSingle(udpdata, count + 2 * 4);; DATA[index][2] = BitConverter.ToSingle(udpdata, count + 3 * 4);; DATA[index][3] = BitConverter.ToSingle(udpdata, count + 4 * 4);; DATA[index][4] = BitConverter.ToSingle(udpdata, count + 5 * 4);; DATA[index][5] = BitConverter.ToSingle(udpdata, count + 6 * 4);; DATA[index][6] = BitConverter.ToSingle(udpdata, count + 7 * 4);; DATA[index][7] = BitConverter.ToSingle(udpdata, count + 8 * 4);; count += 36; // 8 * float } bool xplane9 = !xplane10; 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); } } }
/// <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 abstract void SendToAP(sitl_fdm data);
public abstract void GetFromSim(ref sitl_fdm data);