Merge branch 'master' of git@pixhawk.ethz.ch:qgroundcontrol into checkRemote

deploy/.gitignore

+SDL.framework

deploy/mac_create_dmg.sh

@@ -8,6 +8,12 @@ make -j4
 # Copy and build the application bundle
 cd deploy
 cp -r ../bin/mac/qgroundcontrol.app mac/.
+
+
 cp -r ../audio mac/qgroundcontrol.app/Contents/MacOs/.
-macdeployqt qgroundcontrol.app --bundle
-echo -e '\n QGroundControl .DMG file is now ready for publishing\n'
+mkdir -p mac/qgroundcontrol.app/Contents/Frameworks/
+cp -r SDL.framework mac/qgroundcontrol.app/Contents/Frameworks/.
+echo -e '\n\nStarting to create disk image. This may take a while..\n'
+macdeployqt mac/qgroundcontrol.app -dmg
+rm -rf mac/qgroundcontrol.app
+echo -e '\n\n QGroundControl .DMG file is now ready for publishing\n'

qgroundcontrol.pri

@@ -37,16 +37,17 @@ message(Qt version $$[QT_VERSION])
 macx { 
 
     HARDWARE_PLATFORM = $$system(uname -a)
-    contains( HARDWARE_PLATFORM, 9.8.0 ) {
+    contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || || contains( HARDWARE_PLATFORM, 9.9.0 )
+    {
         # x86 Mac OS X Leopard 10.5 and earlier
         CONFIG += x86 cocoa phonon
         message(Building for Mac OS X 32bit/Leopard 10.5 and earlier)
 
-		# Enable function-profiling with the OS X saturn tool
-		debug {
+                # Enable function-profiling with the OS X saturn tool
+                debug {
                         #QMAKE_CXXFLAGS += -finstrument-functions
                         #LIBS += -lSaturn
-		}
+                }
     } else {
         # x64 Mac OS X Snow Leopard 10.6 and later
         CONFIG += x86_64 cocoa

parameters_bravo.txt → settings/parameters_bravo.txt

 # Onboard parameters for system MAV 042
 #
 # MAV ID  COMPONENT ID  PARAM NAME  VALUE (FLOAT)
-42	1	POSFILTER	1
-42	1	POSFILTERWEIGHT	0.03
-42	1	PROTOCOLTIMEOUT¿	2e+06
-42	1	SETPOINTDELAY	1e+06
-42	1	YAWTOLERANCE	0.1745
-42	100	42	42
-42	100	SYS_ID	42
-42	120	DOWN_PITCH	-0.026
-42	120	DOWN_ROLL	-0.022
-42	120	DOWN_X	0
-42	120	DOWN_Y	0
-42	120	DOWN_YAW	3.14159
-42	120	DOWN_Z	-0.088
 42	200	ACC_NAV_OFFS_X	0
 42	200	ACC_NAV_OFFS_Y	0
 42	200	ACC_NAV_OFFS_Z	-1000
@@ -26,8 +13,8 @@
 42	200	CAL_ACC_Y	0
 42	200	CAL_ACC_Z	0
 42	200	CAL_FIT_ACTIVE	1
-42	200	CAL_FIT_GYRO_X	31545
-42	200	CAL_FIT_GYRO_Y	29350
+42	200	CAL_FIT_GYRO_X	31500
+42	200	CAL_FIT_GYRO_Y	29370
 42	200	CAL_FIT_GYRO_Z	30230
 42	200	CAL_GYRO_X	29809
 42	200	CAL_GYRO_Y	29959
@@ -64,6 +51,7 @@
 42	200	PID_YAWPOS_AWU	1
 42	200	PID_YAWPOS_D	1
 42	200	PID_YAWPOS_I	0.1
+42	200	PID_YAWPOS_LIM	2
 42	200	PID_YAWPOS_P	5
 42	200	PID_YAWSPEED_D	0
 42	200	PID_YAWSPEED_I	5
@@ -72,9 +60,9 @@
 42	200	PID_YAWSPE_LIM	20
 42	200	POS_SP_ACCEPT	1
 42	200	POS_SP_X	1.66
-42	200	POS_SP_Y	0.57
+42	200	POS_SP_Y	0.55
 42	200	POS_SP_YAW	1.5708
-42	200	POS_SP_Z	0
+42	200	POS_SP_Z	-0.7
 42	200	POS_TIMEOUT	2e+06
 42	200	RC_NICK_CHAN	1
 42	200	RC_ROLL_CHAN	2
@@ -82,6 +70,7 @@
 42	200	RC_THRUST_CHAN	3
 42	200	RC_TRIM_CHAN	0
 42	200	RC_TUNE_CHAN1	7
+42	200	RC_TUNE_CHAN2	5
 42	200	RC_TUNE_CHAN3	6
 42	200	RC_TUNE_CHAN4	8
 42	200	RC_YAW_CHAN	4

settings/parameters_charlie.txt

+# Onboard parameters for system MAV 042
+#
+# MAV ID  COMPONENT ID  PARAM NAME  VALUE (FLOAT)
+42	1	POSFILTER	1
+42	1	PROTOCOLTIMEOUT¿	2e+06
+42	1	SETPOINTDELAY	1e+06
+42	1	YAWTOLERANCE	0.1745
+42	100	SYS_ID	42
+42	120	DOWN_PITCH	0.02
+42	120	DOWN_ROLL	0.18
+42	120	DOWN_X	0
+42	120	DOWN_Y	0
+42	120	DOWN_YAW	3.14159
+42	120	DOWN_Z	0
+42	200	ACC_NAV_OFFS_X	0
+42	200	ACC_NAV_OFFS_Y	0
+42	200	ACC_NAV_OFFS_Z	-1000
+42	200	ATT_KAL_IYAW	1
+42	200	ATT_KAL_KACC	0.0033
+42	200	ATT_OFFSET_X	0
+42	200	ATT_OFFSET_Y	0
+42	200	ATT_OFFSET_Z	-0.08
+42	200	CAL_ACC_X	0
+42	200	CAL_ACC_Y	0
+42	200	CAL_ACC_Z	0
+42	200	CAL_FIT_ACTIVE	1
+42	200	CAL_FIT_GYRO_X	31495
+42	200	CAL_FIT_GYRO_Y	29400
+42	200	CAL_FIT_GYRO_Z	30180
+42	200	CAL_GYRO_X	29809
+42	200	CAL_GYRO_Y	29959
+42	200	CAL_GYRO_Z	29455
+42	200	CAL_TEMP	51.5
+42	200	CAM_EXP	1000
+42	200	CAM_INTERVAL	36000
+42	200	DEBUG_1	0
+42	200	DEBUG_2	1
+42	200	DEBUG_3	0
+42	200	DEBUG_4	0
+42	200	DEBUG_5	1
+42	200	DEBUG_6	0
+42	200	MIX_OFFSET	0
+42	200	MIX_POSITION	0
+42	200	MIX_POS_YAW	0
+42	200	MIX_REMOTE	0
+42	200	MIX_Z_POSITION	0
+42	200	PID_ATT_AWU	0.3
+42	200	PID_ATT_D	15
+42	200	PID_ATT_I	20
+42	200	PID_ATT_LIM	30
+42	200	PID_ATT_P	40
+42	200	PID_POS_AWU	10
+42	200	PID_POS_D	1.8
+42	200	PID_POS_I	0.25
+42	200	PID_POS_LIM	0.15
+42	200	PID_POS_P	1.7
+42	200	PID_POS_Z_AWU	3
+42	200	PID_POS_Z_D	0.15
+42	200	PID_POS_Z_I	0.2
+42	200	PID_POS_Z_LIM	0.2
+42	200	PID_POS_Z_P	0.35
+42	200	PID_YAWPOS_AWU	1
+42	200	PID_YAWPOS_D	1
+42	200	PID_YAWPOS_I	0.1
+42	200	PID_YAWPOS_LIM	2
+42	200	PID_YAWPOS_P	5
+42	200	PID_YAWSPEED_D	0
+42	200	PID_YAWSPEED_I	5
+42	200	PID_YAWSPEED_P	15
+42	200	PID_YAWSPE_AWU	1
+42	200	PID_YAWSPE_LIM	20
+42	200	POS_SP_ACCEPT	1
+42	200	POS_SP_X	1.66
+42	200	POS_SP_Y	0.55
+42	200	POS_SP_YAW	1.5708
+42	200	POS_SP_Z	-0.7
+42	200	POS_TIMEOUT	2e+06
+42	200	RC_NICK_CHAN	1
+42	200	RC_ROLL_CHAN	2
+42	200	RC_SAFETY_CHAN	5
+42	200	RC_THRUST_CHAN	3
+42	200	RC_TRIM_CHAN	0
+42	200	RC_TUNE_CHAN1	7
+42	200	RC_TUNE_CHAN2	5
+42	200	RC_TUNE_CHAN3	6
+42	200	RC_TUNE_CHAN4	8
+42	200	RC_YAW_CHAN	4
+42	200	SEND_DEBUGCHAN	0
+42	200	SLOT_ATTITUDE	1
+42	200	SLOT_CONTROL	0
+42	200	SLOT_RAW_IMU	0
+42	200	SLOT_RC	0
+42	200	SYS_COMP_ID	200
+42	200	SYS_ID	42
+42	200	SYS_IMU_RESET	0
+42	200	SYS_SW_VER	2001
+42	200	SYS_TYPE	2
+42	200	UART_0_BAUD	115200
+42	200	UART_1_BAUD	57600
+42	200	VEL_DAMP	0.95
+42	200	VEL_OFFSET_X	0
+42	200	VEL_OFFSET_Y	0
+42	200	VEL_OFFSET_Z	0
+42	200	VIS_OUTL_TRESH	0.2
+42	200	VIS_YAWCORR	0
+42	200	adaThresBlockS	15
+42	200	adaThresMethod	0
+42	200	adaThresParam1	8
+42	200	artkBorderWidt	0.25
+42	200	artkPatternWid	200
+42	200	artkThreshold	175
+42	200	bDContours	1
+42	200	bDRectangles	1
+42	200	bEqualizeHist	0
+42	200	bRectify	1
+42	200	bRefineCorners	1
+42	200	contApproxMeth	2
+42	200	contDeviThresh	50
+42	200	contImageSize	32
+42	200	contImageType	2
+42	200	contOrFactor	0.8
+42	200	contOrHistBins	64
+42	200	contPerimMulti	0.06
+42	200	contourMinArea	250
+42	200	cropBPBegin	0
+42	200	cropBPEnd	9
+42	200	cropBPStep	3
+42	200	cropBPThres	128
+42	200	cropImageSize	64
+42	200	cropInterpolat	1
+42	200	cropWhiteBalan	1
+42	200	detMatchFactoT	1.5
+42	200	findRectMethod	2
+42	200	fps	0
+42	200	histMinSmoothi	11
+42	200	histMinToleran	0.05
+42	200	kittlerWWeight	1
+42	200	kmeansWWeight	1
+42	200	preprocDivisor	1
+42	200	rSExpectdScore	2
+42	200	rSExpectedDiff	0.5
+42	200	rSLearnFactor	0.5
+42	200	rSMaxNarrowing	0.3
+42	200	rSStandardDevi	30
+42	200	rSUnlearnFacto	0.9
+42	200	rectMaxEdgeRat	1.5
+42	200	rectMinArea	1000
+42	200	refCornRadius	3
+42	200	thresHistBins	64
+42	200	thresMethod	5
+42	200	warpImageSize	128

settings/waypoints_braunschweig1.txt

+	0	3.05	1.6	-0.7	1.5708	1	1	0.2	1500
+	1	3	1	-0.7	1.5708	1	0	0.3	0
+	2	2.1	0.3	-0.7	1.5708	1	0	0.5	0
+	3	1	0.23	-0.7	1.5708	1	0	0.3	0
+	4	0.3	1.1	-0.7	1.5708	1	0	0.3	0
+	5	0.3	2.3	-0.7	1.5708	1	0	0.3	0
+	6	1.03	2.98	-0.7	1.58	1	0	0.3	0
+	7	2.15	3.06	-0.7	1.5708	1	0	0.3	0
+	8	2.93	2.25	-0.7	1.5708	1	0	0.3	0
+	9	3.68	1.85	-0.7	1.5708	1	0	0.3	0
+	10	4.84	1.78	-0.7	1.58	1	0	0.3	0
+	11	5.83	1.8	-0.7	1.5708	1	0	0.3	0
+	12	6.87	1.81	-0.7	1.5708	1	0	0.3	0
+	13	7.48	1.83	-0.7	1.5708	1	0	0.3	0
+	14	6.86	1.76	-0.7	1.58	1	0	0.3	0
+	15	5.65	1.71	-0.7	1.5708	1	0	0.3	0
+	16	4.31	1.8	-0.7	1.5708	1	0	0.3	0

settings/waypoints_braunschweig3.txt

+	0	3.05	1.6	-0.7	1.5708	1	1	0.2	1500
+	1	3	1	-0.7	1.5708	1	0	0.3	0
+	2	2.1	0.5	-0.7	1.5708	1	0	0.5	0
+	3	1	0.73	-0.7	1.5708	1	0	0.3	0
+	4	0.55	1.1	-0.7	1.5708	1	0	0.3	0
+	5	0.55	2.3	-0.7	1.5708	1	0	0.3	0
+	6	1.03	2.98	-0.7	1.58	1	0	0.3	0
+	7	2.15	2.86	-0.7	1.5708	1	0	0.3	0
+	8	2.93	2.25	-0.7	1.5708	1	0	0.3	0
+	9	3.68	1.85	-0.7	1.5708	1	0	0.3	0
+	10	4.84	1.78	-0.7	1.58	1	0	0.3	0
+	11	5.83	1.8	-0.7	1.5708	1	0	0.3	0
+	12	6.87	1.81	-0.7	1.5708	1	0	0.3	0
+	13	7.48	1.83	-0.7	1.5708	1	0	0.3	0
+	14	6.86	1.76	-0.7	1.58	1	0	0.3	0
+	15	5.65	1.71	-0.7	1.5708	1	0	0.3	0
+	16	4.31	1.8	-0.7	1.5708	1	0	0.3	0

src/comm/MAVLinkSimulationLink.cc

@@ -661,15 +661,17 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                         // Iterate through all components, through all parameters and emit them
                         QMap<QString, float>::iterator i;
                         // Iterate through all components / subsystems
+                        int j = 0;
                         for (i = onboardParams.begin(); i != onboardParams.end(); ++i)
                         {
                             // Pack message and get size of encoded byte string
-                            mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value());
+                            mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
                             // Allocate buffer with packet data
                             bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                             //add data into datastream
                             memcpy(stream+streampointer,buffer, bufferlength);
                             streampointer+=bufferlength;
+                            j++;
                         }
 
                         /*
@@ -715,7 +717,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                             onboardParams.insert(key, set.param_value);
 
                             // Pack message and get size of encoded byte string
-                            mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value);
+                            mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), 0);
                             // Allocate buffer with packet data
                             bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                             //add data into datastream

src/uas/PxQuadMAV.cc

@@ -104,10 +104,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "vis. x", pos.x, time);
                 emit valueChanged(uasId, "vis. y", pos.y, time);
                 emit valueChanged(uasId, "vis. z", pos.z, time);
-                emit valueChanged(uasId, "vis. vx", pos.vx, time);
-                emit valueChanged(uasId, "vis. vy", pos.vy, time);
-                emit valueChanged(uasId, "vis. vz", pos.vz, time);
-                emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
             }
             break;
     case MAVLINK_MSG_ID_AUX_STATUS:

src/uas/UAS.cc

@@ -437,6 +437,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
             break;
 
+        case MAVLINK_MSG_ID_WAYPOINT_ACK:
+            {
+                mavlink_waypoint_ack_t wpa;
+                mavlink_msg_waypoint_ack_decode(&message, &wpa);
+                if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
+                {
+                    waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
+                }
+            }
+            break;
+
         case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
             {
                 mavlink_waypoint_request_t wpr;
@@ -456,11 +467,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
             break;
 
-        case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
+        case MAVLINK_MSG_ID_WAYPOINT_CURRENT:
             {
-                mavlink_waypoint_set_current_t wpsc;
-                mavlink_msg_waypoint_set_current_decode(&message, &wpsc);
-                waypointManager.handleWaypointSetCurrent(message.sysid, message.compid, &wpsc);
+                mavlink_waypoint_current_t wpc;
+                mavlink_msg_waypoint_current_decode(&message, &wpc);
+                waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
             }
             break;
 

src/uas/UASWaypointManager.h

@@ -39,38 +39,59 @@ This file is part of the PIXHAWK project
 #include <mavlink.h>
 class UAS;
 
+/**
+ * @brief Implementation of the MAVLINK waypoint protocol
+ *
+ * This class handles the communication with a waypoint manager on the MAV.
+ * All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget.
+ * Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data.
+ *
+ * See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states.
+ */
 class UASWaypointManager : public QObject
 {
 Q_OBJECT
 private:
     enum WaypointState {
-        WP_IDLE = 0,
-        WP_SENDLIST,
-        WP_SENDLIST_SENDWPS,
-        WP_GETLIST,
-        WP_GETLIST_GETWPS
+        WP_IDLE = 0,        ///< Waiting for commands
+        WP_SENDLIST,        ///< Initial state for sending waypoints to the MAV
+        WP_SENDLIST_SENDWPS,///< Sending waypoints
+        WP_GETLIST,         ///< Initial state for retrieving wayppoints from the MAV
+        WP_GETLIST_GETWPS,  ///< Receiving waypoints
+        WP_CLEARLIST,       ///< Clearing waypoint list on the MAV
+        WP_SETCURRENT       ///< Setting new current waypoint on the MAV
     }; ///< The possible states for the waypoint protocol
 
 public:
-    UASWaypointManager(UAS&);
+    UASWaypointManager(UAS&);   ///< Standard constructor.
 
-    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);
-    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);
-    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);
-    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);
-    void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr);
+    /** @name Protocol handlers */
+    /*@{*/
+    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);                            ///< Handles received waypoint count messages
+    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);                        ///< Handles received waypoint messages
+    void handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa);                ///< Handles received waypoint ack messages
+    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);        ///< Handles received waypoint request messages
+    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);        ///< Handles received waypoint reached messages
+    void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc);        ///< Handles received set current waypoint messages
+    /*@}*/
 
-    QVector<Waypoint *> &getWaypointList(void) { return waypoints; }
+    QVector<Waypoint *> &getWaypointList(void) { return waypoints; }    ///< Returns a reference to the local waypoint list. Gives full access to the internal data structure - Subject to change: Public const access and friend access for the waypoint list widget.
 
 private:
-    void sendWaypointRequest(quint16 seq);
-    void sendWaypoint(quint16 seq);
+    void sendWaypointClearAll();
+    void sendWaypointSetCurrent(quint16 seq);
+    void sendWaypointCount();
+    void sendWaypointRequestList();
+    void sendWaypointRequest(quint16 seq);          ///< Requests a waypoint with sequence number seq
+    void sendWaypoint(quint16 seq);                 ///< Sends a waypoint with sequence number seq
+    void sendWaypointAck(quint8 type);              ///< Sends a waypoint ack
 
 public slots:
-    void timeout();
-    void clearWaypointList();
-    void requestWaypoints();
-    void sendWaypoints();
+    void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
+    void setCurrent(quint16 seq);                   ///< Sends the sequence number of the waypoint that should get the new target waypoint
+    void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
+    void readWaypoints();                           ///< Requests the MAV's current waypoint list
+    void writeWaypoints();                          ///< Sends the local waypoint list to the MAV
 
 signals:
     void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget
@@ -79,6 +100,7 @@ signals:
 
 private:
     UAS &uas;                                       ///< Reference to the corresponding UAS
+    quint32 current_retries;                        ///< The current number of retries left
     quint16 current_wp_id;                          ///< The last used waypoint ID in the current protocol transaction
     quint16 current_count;                          ///< The number of waypoints in the current protocol transaction
     WaypointState current_state;                    ///< The current protocol state

src/ui/HSIDisplay.cc

@@ -119,7 +119,7 @@ void HSIDisplay::paintDisplay()
     float bottomMargin = 3.0f;
 
     // Size of the ring instrument
-    const float margin = 0.1f;  // 10% margin of total width on each side
+    //const float margin = 0.1f;  // 10% margin of total width on each side
     float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - bottomMargin / 2.0f;
 
     // Draw instruments
@@ -294,6 +294,7 @@ void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, Q
 
 void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
 {
+    Q_UNUSED(uas);
     positionLock = lock;
 }
 
@@ -876,12 +877,17 @@ void visionLocalizationChanged(UASInterface* uas, int fix);
 
 void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat)
 {
-
+    Q_UNUSED(roll);
+    Q_UNUSED(pitch);
+    Q_UNUSED(yaw);
+    Q_UNUSED(thrust);
+    Q_UNUSED(xHat);
+    Q_UNUSED(yHat);
 }
 
 void HSIDisplay::pressKey(int key)
 {
-
+    Q_UNUSED(key);
 }
 
 

src/ui/HUD.cc

@@ -569,7 +569,10 @@ void HUD::paintHUD()
 
     float yawTrans = yawInt * (double)maxYawTrans;
     yawInt *= 0.6f;
-    //qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw << "asin(yawInt)" << asinYaw;
+
+    if ((yawTrans < 5.0) && (yawTrans > -5.0)) yawTrans = 0;
+
+    qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw;
 
     // Update scaling factor
     // adjust scaling to fit both horizontally and vertically