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 .. diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 45a7308d3c..6adb5a220e 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,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; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 387ffd36f1..604e279a06 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 = 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 diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index b086622351..779a4e82f2 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), @@ -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; diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 93176cb779..95d65069df 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; @@ -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(); diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index da2fdbf7b8..e1b44b10d8 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/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; } diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h index 95f6492f0a..9c541026f0 100644 --- a/libraries/APO/AP_MavlinkCommand.h +++ b/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 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()); }