Fixed most of quad guidance issues.

apo/ControllerQuad.h

@@ -91,16 +91,15 @@ private:
     }
     void autoPositionLoop(float dt) {
         // XXX need to add waypoint coordinates
-        float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
-        float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
-        float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
+        float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
+        float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
+        float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
 
         // "transform-to-body"
         _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;
+        _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint
         _thrustMix = THRUST_HOVER_OFFSET + cmdDown;
 
         // "thrust-trim-adjust"

apo/QuadArducopter.h

@@ -61,11 +61,11 @@ static const float loop2Rate = 1; 	// gcs slow
 static const float loop3Rate = 0.1;
 
 // position control loop
-static const float PID_POS_P = 0;
+static const float PID_POS_P = 0.1;
 static const float PID_POS_I = 0;
-static const float PID_POS_D = 0;
-static const float PID_POS_LIM = 0; // about 5 deg
-static const float PID_POS_AWU = 0; // about 5 deg
+static const float PID_POS_D = 0.1;
+static const float PID_POS_LIM = 0.04; // about 2 deg
+static const float PID_POS_AWU = 0.02; // about 1 deg
 static const float PID_POS_Z_P = 0.1;
 static const float PID_POS_Z_I = 0;
 static const float PID_POS_Z_D = 0.2;

libraries/APO/AP_Guide.cpp

@@ -18,8 +18,8 @@ AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal)
     _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
     _previousCommand(AP_MavlinkCommand::home),
     _headingCommand(0), _airSpeedCommand(0),
-    _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
-    _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
+    _groundSpeedCommand(0), _altitudeCommand(0),
+    _mode(MAV_NAV_LOST),
     _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
     _nextCommandTimer(0) {
 }
@@ -56,6 +56,16 @@ void MavlinkGuide::update() {
     handleCommand();
 }
 
+float MavlinkGuide::getPNError() {
+    return -_command.getPN(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+}
+float MavlinkGuide::getPEError() {
+    return -_command.getPE(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+}
+float MavlinkGuide::getPDError() {
+    return -_command.getPD(_navigator->getAlt_intM());
+}
+
 void MavlinkGuide::nextCommand() {
     // within 1 seconds, check if more than 5 calls to next command occur
     // if they do, go to home waypoint
@@ -209,13 +219,6 @@ void MavlinkGuide::handleCommand() {
 
     _groundSpeedCommand = _velocityCommand;
 
-    // calculate pN,pE,pD from home and gps coordinates
-    _pNCmd = _command.getPN(_navigator->getLat_degInt(),
-                            _navigator->getLon_degInt());
-    _pECmd = _command.getPE(_navigator->getLat_degInt(),
-                            _navigator->getLon_degInt());
-    _pDCmd = _command.getPD(_navigator->getAlt_intM());
-
     // debug
     //_hal->debug->printf_P(
         //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),

libraries/APO/AP_Guide.h

@@ -97,15 +97,10 @@ public:
     float getAltitudeCommand() {
         return _altitudeCommand;
     }
-    float getPNCmd() {
-        return _pNCmd;
-    }
-    float getPECmd() {
-        return _pECmd;
-    }
-    float getPDCmd() {
-        return _pDCmd;
-    }
+    virtual float getPNError() = 0;
+    virtual float getPEError() = 0;
+    virtual float getPDError() = 0;
+
     MAV_NAV getMode() {
         return _mode;
     }
@@ -121,9 +116,6 @@ protected:
     float _airSpeedCommand;
     float _groundSpeedCommand;
     float _altitudeCommand;
-    float _pNCmd;
-    float _pECmd;
-    float _pDCmd;
     MAV_NAV _mode;
     AP_Uint8 _numberOfCommands;
     AP_Uint8 _cmdIndex;
@@ -139,6 +131,9 @@ public:
     void nextCommand();
     void handleCommand();
     void updateCommand();
+    virtual float getPNError();
+    virtual float getPEError();
+    virtual float getPDError();
 
 private:
     AP_Var_group _group;