aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed guidance bugs in APO.

Browse Source
This commit is contained in:
Wenyao Xie 2011-11-29 17:37:42 -05:00
parent e2ab6d96f2
commit 35f74dd2db
7 changed files with 67 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
apo/ControllerQuad.h
View File

@ -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;

8
apo/QuadArducopter.h
View File

@ -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;

43
libraries/APO/AP_Guide.cpp
View File

@ -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;

8
libraries/APO/AP_Guide.h
View File

@ -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();

11
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -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;
}

2
libraries/APO/AP_MavlinkCommand.h
View File

@ -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

9
libraries/APO/AP_Navigator.h
View File

@ -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());
}