Changed BlockPIDDfb behaviour.

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

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

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

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() {

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

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