Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Conflicts:
	libraries/APO/AP_Guide.cpp
This commit is contained in:
James Goppert 2011-11-29 18:06:38 -05:00
parent b6dcd70cf9 a0824b4cbe
commit 38c19c50be
8 changed files with 92 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.txt
View File

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

21
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,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;

16
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 = 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

54
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),
@ -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;

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

12
libraries/APO/AP_MavlinkCommand.cpp
View File

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

4
libraries/APO/AP_MavlinkCommand.h
View File

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

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