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

Changed BlockPIDDfb behaviour.

This commit is contained in:
James Goppert 2011-12-03 12:13:11 -05:00
parent b9f370e33d
commit 8300c5e738
6 changed files with 22 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduBoat/ControllerBoat.h
View File

@ -18,10 +18,9 @@ public:
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
steeringI, steeringD, steeringIMax, steeringYMax),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0)
throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut), _strCmd(0), _thrustCmd(0)
{
_hal->debug->println_P(PSTR("initializing boat controller"));
@ -44,7 +43,8 @@ private:
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
}
void autoLoop(const float dt) {
_strCmd = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
// neglects heading command derivative
_strCmd = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);

5
ArduRover/ControllerCar.h
View File

@ -18,7 +18,7 @@ public:
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
steeringI, steeringD, steeringIMax, steeringYMax),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0),
@ -59,7 +59,8 @@ private:
}
void autoLoop(const float dt) {
//_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_thrust]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition());
float steering = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
// neglects heading command derivative
float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
float thrust = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);

4
ArduRover/ControllerTank.h
View File

@ -18,7 +18,7 @@ public:
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode,k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
steeringI, steeringD, steeringIMax, steeringYMax),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _headingOutput(0), _throttleOutput(0) {
@ -55,7 +55,7 @@ private:
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_headingOutput = pidStr.update(headingError, _nav->getYawRate(), dt);
_headingOutput = pidStr.update(headingError, -_nav->getYawRate(), dt);
_throttleOutput = pidThr.update(_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}

16
apo/ControllerQuad.h
View File

@ -21,13 +21,13 @@ public:
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
PID_ATT_LIM),
pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
PID_ATT_LIM),
pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1,
PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D,
PID_YAWPOS_AWU, PID_YAWPOS_LIM, PID_ATT_DFCUT),
PID_YAWPOS_AWU, PID_YAWPOS_LIM),
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),
@ -36,7 +36,7 @@ public:
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,
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_Z_DFCUT),
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM),
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
@ -91,7 +91,7 @@ private:
}
void autoPositionLoop(float dt) {
float cmdSpeed = pidSpeed.update(_guide->getGroundSpeedError(),dt);
float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
float cmdDown = pidPD.update(_guide->getPDError(),-_nav->getVD(),dt);
// tilt based control
float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
@ -102,7 +102,7 @@ private:
_cmdPitch -= cmdSpeed*cos(_nav->getRelativeCourseOverGround());
_cmdRoll += cmdSpeed*sin(_nav->getRelativeCourseOverGround());
_cmdYawRate = pidYaw.update(_guide->getYawError(),_nav->getYawRate(),dt); // always points to next waypoint
_cmdYawRate = pidYaw.update(_guide->getYawError(),-_nav->getYawRate(),dt); // always points to next waypoint
_thrustMix = THRUST_HOVER_OFFSET - cmdDown;
// "thrust-trim-adjust"
@ -117,9 +117,9 @@ private:
}
void autoAttitudeLoop(float dt) {
_rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
_nav->getRollRate(), dt);
-_nav->getRollRate(), dt);
_pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(),
_nav->getPitchRate(), dt);
-_nav->getPitchRate(), dt);
_yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
}
void setMotors() {

11
libraries/APO/AP_ControllerBlock.cpp
View File

@ -155,23 +155,20 @@ float BlockPD::update(const float & input, const float & dt) {
}
BlockPIDDfb::BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel,
float kD, float iMax, float yMax,
const prog_char_t * dLabel) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
_kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float BlockPIDDfb::update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
float y = _blockP.update(input) + _blockI.update(input, dt) + _kD
* derivative;
return _blockSaturation.update(y);
}

4
libraries/APO/AP_ControllerBlock.h
View File

@ -218,8 +218,7 @@ protected:
class BlockPIDDfb: public AP_ControllerBlock {
public:
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel = NULL,
float kD, float iMax, float yMax,
const prog_char_t * dLabel = NULL);
float update(const float & input, const float & derivative,
const float & dt);
@ -227,7 +226,6 @@ protected:
BlockP _blockP; /// The proportional block.
BlockI _blockI; /// The integral block.
BlockSaturation _blockSaturation; /// The saturation block.
BlockLowPass _blockLowPass; /// The low-pass filter block.
AP_Float _kD; /// derivative gain
};