From 6948c50617731733c10680223c8241cee72856db Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 29 Nov 2011 14:02:27 -0500 Subject: [PATCH 1/2] Fixed typo in readme. --- README.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.txt b/README.txt index c72fe9aa77..d208de95a2 100644 --- a/README.txt +++ b/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 .. From a0824b4cbe06e60f3ebba02c2943510edcb00e92 Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 29 Nov 2011 17:37:42 -0500 Subject: [PATCH 2/2] Fixed guidance bugs in APO. --- apo/ControllerQuad.h | 17 ++++++++---- apo/QuadArducopter.h | 8 ++++++ libraries/APO/AP_Guide.cpp | 43 ++++++++++++++++------------- libraries/APO/AP_Guide.h | 8 ++++-- libraries/APO/AP_MavlinkCommand.cpp | 11 ++++++-- libraries/APO/AP_MavlinkCommand.h | 2 +- libraries/APO/AP_Navigator.h | 9 ++++++ 7 files changed, 67 insertions(+), 31 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 45a7308d3c..127e110ce2 100644 --- a/apo/ControllerQuad.h +++ b/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,14 +90,18 @@ 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()); + + // add velocity based control + _cmdPitch -= cmdSpeed*cos(_nav->getRelativeCourseOverGround()); + _cmdRoll += cmdSpeed*sin(_nav->getRelativeCourseOverGround()); + _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint _thrustMix = THRUST_HOVER_OFFSET - cmdDown; @@ -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; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 387ffd36f1..d0730b9df5 100644 --- a/apo/QuadArducopter.h +++ b/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 = 0.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; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 9b365d8384..8d786426fc 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/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), @@ -34,7 +34,7 @@ void AP_Guide::setCurrentIndex(uint8_t val) { float AP_Guide::getHeadingError() { float headingError = getHeadingCommand() - - _navigator->getYaw(); + - _nav->getYaw(); if (headingError > 180 * deg2Rad) headingError -= 360 * deg2Rad; if (headingError < -180 * deg2Rad) @@ -43,13 +43,17 @@ float AP_Guide::getHeadingError() { } 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")), @@ -62,13 +66,13 @@ void MavlinkGuide::update() { } 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 +171,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 +185,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; @@ -189,9 +194,9 @@ void MavlinkGuide::handleCommand() { temp = -_crossTrackLim * deg2Rad; float bearing = _previousCommand.bearingTo(_command); _headingCommand = bearing - temp; - //_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\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\tcrossTrack: %f\n"), + bearing * rad2Deg, _headingCommand * rad2Deg, getDistanceToNextWaypoint(), alongTrack, dXt); // for these modes just head to current command } else if ( @@ -199,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; diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index fbb6b055f2..8fd6e85559 100644 --- a/libraries/APO/AP_Guide.h +++ b/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; @@ -94,6 +94,8 @@ public: float getGroundSpeedCommand() { return _groundSpeedCommand; } + float getGroundSpeedError(); + float getAltitudeCommand() { return _altitudeCommand; } @@ -111,7 +113,7 @@ public: } protected: - AP_Navigator * _navigator; + AP_Navigator * _nav; AP_HardwareAbstractionLayer * _hal; AP_MavlinkCommand _command, _previousCommand; float _headingCommand; @@ -127,7 +129,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(); diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index da2fdbf7b8..079ccc7e26 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -194,12 +194,17 @@ 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; } diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h index cf4beb47dd..062a345eb4 100644 --- a/libraries/APO/AP_MavlinkCommand.h +++ b/libraries/APO/AP_MavlinkCommand.h @@ -365,7 +365,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 diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 1f71ec8808..a208e70533 100644 --- a/libraries/APO/AP_Navigator.h +++ b/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()); }