From 5748fe0e906612b07dce8b8f0116351118733fdb Mon Sep 17 00:00:00 2001 From: edouard Date: Mon, 20 Dec 2010 22:30:42 +0000 Subject: [PATCH] Add rotation rates into FG simulator to test Stabilization. Does not work well yet... git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2262 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../plugins/gcscontrol/gcscontrolgadget.cpp | 16 ++++---- ground/src/plugins/hitlnew/fgsimulator.cpp | 38 +++++++++++++------ ground/src/plugins/hitlnew/fgsimulator.h | 1 + 3 files changed, 36 insertions(+), 19 deletions(-) diff --git a/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp index 602dadb01..b93e7cbce 100644 --- a/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -96,19 +96,19 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { switch (controlsMode) { case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle - emit sticksChangedRemotely(yaw,-pitch,roll,throttle); + emit sticksChangedRemotely(yaw,pitch,roll,throttle); break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch - emit sticksChangedRemotely(yaw,throttle,roll,-pitch); + emit sticksChangedRemotely(yaw,throttle,roll,pitch); break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle - emit sticksChangedRemotely(roll,-pitch,yaw,throttle); + emit sticksChangedRemotely(roll,pitch,yaw,throttle); break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; - emit sticksChangedRemotely(roll,throttle,yaw,-pitch); + emit sticksChangedRemotely(roll,throttle,yaw,pitch); break; } } @@ -133,28 +133,28 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r case 1: // Mode 1: LeftX = Yaw, LeftY = Pitch, RightX = Roll, RightY = Throttle newRoll = rightX; - newPitch = -leftY; + newPitch = leftY; newYaw = leftX; newThrottle = rightY; break; case 2: // Mode 2: LeftX = Yaw, LeftY = Throttle, RightX = Roll, RightY = Pitch newRoll = rightX; - newPitch = -rightY; + newPitch = rightY; newYaw = leftX; newThrottle = leftY; break; case 3: // Mode 3: LeftX = Roll, LeftY = Pitch, RightX = Yaw, RightY = Throttle newRoll = leftX; - newPitch = -leftY; + newPitch = leftY; newYaw = rightX; newThrottle = rightY; break; case 4: // Mode 4: LeftX = Roll, LeftY = Throttle, RightX = Yaw, RightY = Pitch; newRoll = leftX; - newPitch = -rightY; + newPitch = rightY; newYaw = rightX; newThrottle = leftY; break; diff --git a/ground/src/plugins/hitlnew/fgsimulator.cpp b/ground/src/plugins/hitlnew/fgsimulator.cpp index 4eb694c66..e2928dfa9 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/src/plugins/hitlnew/fgsimulator.cpp @@ -38,6 +38,8 @@ const float FGSimulator::FT2M = 0.3048; const float FGSimulator::KT2MPS = 0.514444444; const float FGSimulator::INHG2KPA = 3.386; const float FGSimulator::FPS2CMPS = 30.48; +const float FGSimulator::DEG2RAD = (M_PI/180.0); + //FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) : @@ -171,7 +173,7 @@ void FGSimulator::transmitUpdate() if(manCtrlData.Armed == ManualControlCommand::ARMED_TRUE) { ailerons = manCtrlData.Roll; - elevator = -manCtrlData.Pitch; + elevator = manCtrlData.Pitch; rudder = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } @@ -182,7 +184,7 @@ void FGSimulator::transmitUpdate() actData = actDesired->getData(); ailerons = actData.Roll; - elevator = -actData.Pitch; + elevator = actData.Pitch; rudder = actData.Yaw; throttle = actData.Throttle; } @@ -236,34 +238,35 @@ void FGSimulator::transmitUpdate() void FGSimulator::processUpdate(const QByteArray& inp) { + //TODO: this does not use the FLIGHT_PARAM structure, it should! static char once=0; // Split QString data(inp); QStringList fields = data.split(","); // Get xRate (deg/s) -// float xRate = fields[0].toFloat() * 180.0/M_PI; +// float xRate = fields[0].toFloat() * 180.0/M_PI; // Get yRate (deg/s) -// float yRate = fields[1].toFloat() * 180.0/M_PI; +// float yRate = fields[1].toFloat() * 180.0/M_PI; // Get zRate (deg/s) -// float zRate = fields[2].toFloat() * 180.0/M_PI; +// float zRate = fields[2].toFloat() * 180.0/M_PI; // Get xAccel (m/s^2) -// float xAccel = fields[3].toFloat() * FT2M; +// float xAccel = fields[3].toFloat() * FT2M; // Get yAccel (m/s^2) -// float yAccel = fields[4].toFloat() * FT2M; +// float yAccel = fields[4].toFloat() * FT2M; // Get xAccel (m/s^2) -// float zAccel = fields[5].toFloat() * FT2M; +// float zAccel = fields[5].toFloat() * FT2M; // Get pitch (deg) float pitch = fields[6].toFloat(); // Get pitchRate (deg/s) -// float pitchRate = fields[7].toFloat(); + float pitchRate = fields[7].toFloat(); // Get roll (deg) float roll = fields[8].toFloat(); // Get rollRate (deg/s) -// float rollRate = fields[9].toFloat(); + float rollRate = fields[9].toFloat(); // Get yaw (deg) float yaw = fields[10].toFloat(); // Get yawRate (deg/s) -// float yawRate = fields[11].toFloat(); + float yawRate = fields[11].toFloat(); // Get latitude (deg) float latitude = fields[12].toFloat(); // Get longitude (deg) @@ -381,5 +384,18 @@ void FGSimulator::processUpdate(const QByteArray& inp) positionActualData.East = NED[1]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance positionActualData.Down = NED[2]*100; //Multiply by 100 because positionActual expects input in Centimeters. posActual->setData(positionActualData); + + // Update AttitudeRaw object (filtered gyros only for now) + AttitudeRaw::DataFields rawData; + memset(&rawData, 0, sizeof(AttitudeRaw::DataFields)); + rawData = attRaw->getData(); + rawData.gyros_filtered[0] = rollRate; + //rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate; + //rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate; + rawData.gyros_filtered[1] = -pitchRate; + rawData.gyros_filtered[2] = yawRate; + attRaw->setData(rawData); + attRaw->updated(); + } diff --git a/ground/src/plugins/hitlnew/fgsimulator.h b/ground/src/plugins/hitlnew/fgsimulator.h index 96e3a959a..f4b8213aa 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.h +++ b/ground/src/plugins/hitlnew/fgsimulator.h @@ -51,6 +51,7 @@ private: static const float KT2MPS; static const float INHG2KPA; static const float FPS2CMPS; + static const float DEG2RAD; int udpCounterGCSsend; //keeps track of udp packets sent to FG int udpCounterFGrecv; //keeps track of udp packets received by FG