diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 00b089eaa9..2808d5e492 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -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); diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 75b6a97e17..2a80f32ab4 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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); diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index a72edad6c7..29e273b2c8 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -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); } diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 6adb5a220e..f8bd8ffa77 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -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() { diff --git a/libraries/APO/AP_ControllerBlock.cpp b/libraries/APO/AP_ControllerBlock.cpp index 526c3ae4d9..785df2de7e 100644 --- a/libraries/APO/AP_ControllerBlock.cpp +++ b/libraries/APO/AP_ControllerBlock.cpp @@ -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); } diff --git a/libraries/APO/AP_ControllerBlock.h b/libraries/APO/AP_ControllerBlock.h index 012a34a374..23383554cd 100644 --- a/libraries/APO/AP_ControllerBlock.h +++ b/libraries/APO/AP_ControllerBlock.h @@ -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 };