Merge branch 'master' of github.com:arktools/ardupilotone

Conflicts:
	libraries/APO/AP_Guide.cpp

README.txt

@@ -14,7 +14,7 @@ Building using cmake
 -----------------------------------------------
  - mkdir build
  - cd build
- - cmake .. -DBOARD=mega -PORT=/dev/ttyUSB0
+ - cmake .. -DBOARD=mega -DPORT=/dev/ttyUSB0
     You can select from mega/mega2560.
     If you have arduino installed in a non-standard location you by specify it by using:
         -DARDUINO_SDK_PATH=/path/to/arduino ..

apo/ControllerQuad.h

@@ -31,6 +31,8 @@ public:
         pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
                    PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
                    PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
+        pidSpeed(new AP_Var_group(k_pidSpeed, PSTR("SPD_")), 1, PID_SPD_P,
+              PID_SPD_I, PID_SPD_D, PID_SPD_AWU, PID_SPD_LIM, PID_SPD_DFCUT),
         pidTilt(new AP_Var_group(k_pidTilt, PSTR("TILT_")), 1, PID_TILT_P,
               PID_TILT_I, PID_TILT_D, PID_TILT_AWU, PID_TILT_LIM, PID_TILT_DFCUT),
         pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
@@ -88,15 +90,19 @@ private:
         autoAttitudeLoop(dt);
     }
     void autoPositionLoop(float dt) {
-        // XXX need to add waypoint coordinates
-        //float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
-        //float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
-        float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
+        float cmdSpeed = pidSpeed.update(_guide->getGroundSpeedError(),dt);
         float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
 
+        // tilt based control
+        float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
         _cmdPitch = -cmdTilt*cos(_guide->getHeadingError());
         _cmdRoll = cmdTilt*sin(_guide->getHeadingError());
-        _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint
+        
+        // add velocity based control
+        _cmdPitch -= cmdSpeed*cos(_nav->getRelativeCourseOverGround()); 
+        _cmdRoll += cmdSpeed*sin(_nav->getRelativeCourseOverGround());
+
+        _cmdYawRate = pidYaw.update(_guide->getYawError(),_nav->getYawRate(),dt); // always points to next waypoint
         _thrustMix = THRUST_HOVER_OFFSET - cmdDown;
 
         // "thrust-trim-adjust"
@@ -107,7 +113,7 @@ private:
         else _thrustMix /= cos(_cmdPitch);
 
         // debug for position loop
-        _hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll);
+        //_hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll);
     }
     void autoAttitudeLoop(float dt) {
         _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@@ -165,6 +171,7 @@ private:
         k_pidGroundSpeed2Throttle = k_controllersStart,
         k_pidStr,
         k_pidTilt,
+        k_pidSpeed,
         k_pidPD,
         k_pidRoll,
         k_pidPitch,
@@ -173,7 +180,7 @@ private:
     };
     BlockPIDDfb pidRoll, pidPitch, pidYaw;
     BlockPID pidYawRate;
-    BlockPID pidTilt;
+    BlockPID pidTilt, pidSpeed;
     BlockPIDDfb pidPD;
     float _thrustMix, _pitchMix, _rollMix, _yawMix;
     float _cmdRoll, _cmdPitch, _cmdYawRate;

apo/QuadArducopter.h

@@ -67,6 +67,14 @@ static const float PID_TILT_D = 0.1;
 static const float PID_TILT_LIM = 0.04; // about 2 deg
 static const float PID_TILT_AWU = 0.02; // about 1 deg
 static const float PID_TILT_DFCUT = 10; // cut derivative feedback at 10 hz
+
+static const float PID_SPD_P = 1;
+static const float PID_SPD_I = 0;
+static const float PID_SPD_D = 0.1;
+static const float PID_SPD_LIM = 0.04; // about 2 deg
+static const float PID_SPD_AWU = 0.02; // about 1 deg
+static const float PID_SPD_DFCUT = 10; // cut derivative feedback at 10 hz
+
 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;
@@ -81,10 +89,10 @@ static const float PID_ATT_D = 0.06;
 static const float PID_ATT_LIM = 0.05; // 5 % motors
 static const float PID_ATT_AWU = 0.005; // 0.5 %
 static const float PID_ATT_DFCUT = 25; // cut derivative feedback at 25 hz
-static const float PID_YAWPOS_P = 0;
+static const float PID_YAWPOS_P = 1;
 static const float PID_YAWPOS_I = 0;
-static const float PID_YAWPOS_D = 0;
-static const float PID_YAWPOS_LIM = 0; // 1 rad/s
+static const float PID_YAWPOS_D = 0.1;
+static const float PID_YAWPOS_LIM = 1; // 1 rad/s
 static const float PID_YAWPOS_AWU = 0; // 1 rad/s
 static const float PID_YAWSPEED_P = 0.5;
 static const float PID_YAWSPEED_I = 0;
@@ -95,7 +103,7 @@ static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
 static const float THRUST_HOVER_OFFSET = 0.475;
 
 // guidance
-static const float velCmd = 1; // m/s
+static const float velCmd = 0.3; // m/s
 static const float xt = 10; // cross track gain
 static const float xtLim = 90; // cross track angle limit, deg
 

libraries/APO/AP_Guide.cpp

@@ -14,8 +14,8 @@
 
 namespace apo {
 
-AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) :
-    _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
+AP_Guide::AP_Guide(AP_Navigator * nav, AP_HardwareAbstractionLayer * hal) :
+    _nav(nav), _hal(hal), _command(AP_MavlinkCommand::home),
     _previousCommand(AP_MavlinkCommand::home),
     _headingCommand(0), _airSpeedCommand(0),
     _groundSpeedCommand(0), _altitudeCommand(0),
@@ -33,42 +33,45 @@ void AP_Guide::setCurrentIndex(uint8_t val) {
 }
 
 float AP_Guide::getHeadingError() {
-    float headingError = getHeadingCommand()
-                         - _navigator->getYaw();
-    if (headingError > 180 * deg2Rad)
-        headingError -= 360 * deg2Rad;
-    if (headingError < -180 * deg2Rad)
-        headingError += 360 * deg2Rad;
-    return headingError;
+    return wrapAngle(getHeadingCommand()
+                         - _nav->getYaw());
 }
 
 float AP_Guide::getDistanceToNextWaypoint() {
-    return _command.distanceTo(_navigator->getLat_degInt(),
-        _navigator->getLon_degInt());
+    return _command.distanceTo(_nav->getLat_degInt(),
+        _nav->getLon_degInt());
 }
 
-MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
+float AP_Guide::getGroundSpeedError() {
+    return _groundSpeedCommand - _nav->getGroundSpeed();
+}
+
+MavlinkGuide::MavlinkGuide(AP_Navigator * nav,
                            AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
-    AP_Guide(navigator, hal),
+    AP_Guide(nav, hal),
     _group(k_guide, PSTR("guide_")),
     _velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
     _crossTrackGain(&_group, 2, xt, PSTR("xt")),
     _crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
 }
 
+float AP_Guide::getYawError(){
+    return wrapAngle(_yawCommand - _nav->getYaw());
+}
+
 void MavlinkGuide::update() {
     // process mavlink commands
     handleCommand();
 }
 
 float MavlinkGuide::getPNError() {
-    return -_command.getPN(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+    return -_command.getPN(_nav->getLat_degInt(), _nav->getLon_degInt());
 }
 float MavlinkGuide::getPEError() {
-    return -_command.getPE(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+    return -_command.getPE(_nav->getLat_degInt(), _nav->getLon_degInt());
 }
 float MavlinkGuide::getPDError() {
-    return -_command.getPD(_navigator->getAlt_intM());
+    return -_command.getPD(_nav->getAlt_intM());
 }
 
 void MavlinkGuide::nextCommand() {
@@ -167,12 +170,13 @@ void MavlinkGuide::handleCommand() {
 
         // check for along track next waypoint requirement
         float alongTrack = _command.alongTrack(_previousCommand,
-                                               _navigator->getLat_degInt(),
-                                               _navigator->getLon_degInt());
+                                               _command,
+                                               _nav->getLat_degInt(),
+                                               _nav->getLon_degInt());
         float segmentLength = _previousCommand.distanceTo(_command);
         if (alongTrack > segmentLength) {
             _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (along track)"));
-            _hal->debug->printf_P(PSTR("waypoint reached (along track)"));
+            _hal->debug->printf_P(PSTR("waypoint reached (along track) segmentLength: %f\t alongTrack: %f\n"),segmentLength,alongTrack);
             nextCommand();
             return;
         }
@@ -180,8 +184,8 @@ void MavlinkGuide::handleCommand() {
         // calculate altitude and heading commands
         _altitudeCommand = _command.getAlt();
         float dXt = _command.crossTrack(_previousCommand,
-                                        _navigator->getLat_degInt(),
-                                        _navigator->getLon_degInt());
+                                        _nav->getLat_degInt(),
+                                        _nav->getLon_degInt());
         float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m
         if (temp > _crossTrackLim * deg2Rad)
             temp = _crossTrackLim * deg2Rad;
@@ -190,9 +194,9 @@ void MavlinkGuide::handleCommand() {
         float bearing = _previousCommand.bearingTo(_command);
         _headingCommand = bearing - temp;
         _yawCommand = _command.getYawCommand();
-        //_hal->debug->printf_P(
-        //	PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
-        //	bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
+        _hal->debug->printf_P(
+        	PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\tyaw command: %f\n"),
+        	bearing * rad2Deg, dXt, _headingCommand * rad2Deg, getDistanceToNextWaypoint(), alongTrack, _yawCommand*rad2Deg);
 
         // for these modes just head to current command
     } else if (
@@ -200,7 +204,7 @@ void MavlinkGuide::handleCommand() {
         _mode == MAV_NAV_RETURNING) {
         _altitudeCommand = AP_MavlinkCommand::home.getAlt();
         _headingCommand = AP_MavlinkCommand::home.bearingTo(
-                              _navigator->getLat_degInt(), _navigator->getLon_degInt())
+                              _nav->getLat_degInt(), _nav->getLon_degInt())
                           + 180 * deg2Rad;
         if (_headingCommand > 360 * deg2Rad)
             _headingCommand -= 360 * deg2Rad;

libraries/APO/AP_Guide.h

@@ -37,7 +37,7 @@ public:
      * This is the constructor, which requires a link to the navigator.
      * @param navigator This is the navigator pointer.
      */
-    AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal);
+    AP_Guide(AP_Navigator * nav, AP_HardwareAbstractionLayer * hal);
 
     virtual void update() = 0;
 
@@ -89,16 +89,27 @@ public:
     float getHeadingCommand() {
         return _headingCommand;
     }
-    /// the yaw attitude command of the vehicle
-    float getYawCommand(){
-        return _yawCommand;
+
+    /// wrap an angle between -180 and 180
+    float wrapAngle(float y) {
+        if (y > 180 * deg2Rad)
+            y -= 360 * deg2Rad;
+        if (y < -180 * deg2Rad)
+            y += 360 * deg2Rad;
+        return y;
     }
+
+    /// the yaw attitude error of the vehicle
+    float getYawError();
+
     float getAirSpeedCommand() {
         return _airSpeedCommand;
     }
     float getGroundSpeedCommand() {
         return _groundSpeedCommand;
     }
+    float getGroundSpeedError();
+
     float getAltitudeCommand() {
         return _altitudeCommand;
     }
@@ -116,7 +127,7 @@ public:
     }
 
 protected:
-    AP_Navigator * _navigator;
+    AP_Navigator * _nav;
     AP_HardwareAbstractionLayer * _hal;
     AP_MavlinkCommand _command, _previousCommand;
     float _headingCommand;
@@ -133,7 +144,7 @@ protected:
 
 class MavlinkGuide: public AP_Guide {
 public:
-    MavlinkGuide(AP_Navigator * navigator,
+    MavlinkGuide(AP_Navigator * nav,
                  AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim);
     virtual void update();
     void nextCommand();

libraries/APO/AP_MavlinkCommand.cpp

@@ -194,12 +194,16 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
 }
 
 // calculates along  track distance of a current location
-float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
+float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next,
                                     int32_t lat_degInt, int32_t lon_degInt) const {
     // ignores lat/lon since single prec.
-    float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
-    float d = previous.distanceTo(lat_degInt, lon_degInt);
-    return dXt / tan(asin(dXt / d));
+    float t1N = previous.getPN(lat_degInt, lon_degInt);
+    float t1E = previous.getPE(lat_degInt, lon_degInt);
+    float t2N = previous.getPN(next.getLat_degInt(), next.getLon_degInt());
+    float t2E = previous.getPE(next.getLat_degInt(), next.getLon_degInt());
+    float segmentLength = previous.distanceTo(next);
+    if (segmentLength == 0) return 0;
+    return (t1N*t2N + t1E*t2E)/segmentLength;
 }
 
 

libraries/APO/AP_MavlinkCommand.h

@@ -125,7 +125,7 @@ public:
     }
 
     float getYawCommand() const {
-        return getParam4();
+        return deg2Rad*getParam4();
     }
 
     float getLatDeg() const {
@@ -370,7 +370,7 @@ public:
     float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
 
     // calculates along  track distance of a current location
-    float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
+    float alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next, int32_t lat_degInt, int32_t lon_degInt) const;
 };
 
 } // namespace apo

libraries/APO/AP_Navigator.h

@@ -146,6 +146,15 @@ public:
         return atan2(getVE(),getVN());
     }
 
+    float getRelativeCourseOverGround() const {
+        float y = getCourseOverGround() - getYaw();
+        if (y > 180 * deg2Rad)
+            y -= 360 * deg2Rad;
+        if (y < -180 * deg2Rad)
+            y += 360 * deg2Rad;
+        return y;
+    }
+
     float getSpeedOverGround() const {
         return sqrt(getVN()*getVN()+getVE()*getVE());
     }