Working on memory problem.

apo/ControllerQuad.h

@@ -96,15 +96,11 @@ private:
         float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
 
         // "transform-to-body"
-        {
-            float trigSin = sin(-_nav->getYaw());
-            float trigCos = cos(-_nav->getYaw());
-            _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
-            _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
-            // note that the north tilt is negative of the pitch
-        }
-        _cmdYawRate = 0;
+        _cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw());
+        _cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw());
+        _cmdPitch *= -1; // note that the north tilt is negative of the pitch
 
+        _cmdYawRate = 0;
         _thrustMix = THRUST_HOVER_OFFSET + cmdDown;
 
         // "thrust-trim-adjust"

cmake/modules/FindArduino.cmake

@@ -460,7 +460,7 @@ function(setup_arduino_upload BOARD_ID TARGET_NAME PORT)
                          -p${${BOARD_ID}.build.mcu} 
                          -c${${BOARD_ID}.upload.protocol} 
                          -P${PORT} -b${${BOARD_ID}.upload.speed}
-                         -D
+                         #-D
                          -Uflash:w:${CMAKE_BINARY_DIR}/${TARGET_NAME}.hex:i
                          DEPENDS ${TARGET_NAME})
     if(NOT TARGET upload)

libraries/APO/AP_MavlinkCommand.cpp

@@ -62,8 +62,8 @@ AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
     setZ(cmd.z);
     save();
 
-    // reload home if sent
-    if (cmd.seq == 0) home.load();
+    // reload home if sent, home must be a global waypoint
+    if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) home.load();
 
     Serial.println("============================================================");
     Serial.println("storing new command from mavlink_waypoint_t");