zr-v4/apo/ControllerQuad.h

/*
 * ControllerQuad.h
 *
 *  Created on: Jun 30, 2011
 *      Author: jgoppert
 */

#ifndef CONTROLLERQUAD_H_
#define CONTROLLERQUAD_H_

#include "../APO/AP_Controller.h"
#include "../APO/AP_BatteryMonitor.h"
#include "../APO/AP_ArmingMechanism.h"

namespace apo {

class ControllerQuad: public AP_Controller {
public:
    ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
                   AP_HardwareAbstractionLayer * hal) :
        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
        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),
        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),
        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),
        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),
        pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
              PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
        pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
              PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_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_DFCUT),
        _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
        _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
        _hal->debug->println_P(PSTR("initializing quad controller"));

        /*
         * allocate radio channels
         * the order of the channels has to match the enumeration above
         */
        _hal->rc.push_back(
            new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
                             1500, 1900, RC_MODE_IN, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100,
                             1100, 1900, RC_MODE_OUT, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100,
                             1100, 1900, RC_MODE_OUT, false));

        _hal->rc.push_back(
            new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100,
                             1100, 1900, RC_MODE_OUT, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100,
                             1100, 1900, RC_MODE_OUT, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100,
                             1500, 1900, RC_MODE_IN, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100,
                             1500, 1900, RC_MODE_IN, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100,
                             1100, 1900, RC_MODE_IN, false));
        _hal->rc.push_back(
            new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500,
                             1900, RC_MODE_IN, false));
    }

private:
    // methods
    void manualLoop(const float dt) {
        setAllRadioChannelsManually();
        _cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition();
        _cmdPitch = -0.5 * _hal->rc[ch_pitch]->getPosition();
        _cmdYawRate = -1 * _hal->rc[ch_yaw]->getPosition();
        _thrustMix = _hal->rc[ch_thrust]->getPosition();
        autoAttitudeLoop(dt);
    }
    void autoLoop(const float dt) {
        autoPositionLoop(dt);
        autoAttitudeLoop(dt);

        // XXX currently auto loop not tested, so
        // put vehicle in standby
        _hal->setState(MAV_STATE_STANDBY);
    }
    void autoPositionLoop(float dt) {
        float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
        float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
        float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);

        // "transform-to-body"
        {
            float trigSin = sin(-_nav->getYaw());
            float trigCos = cos(-_nav->getYaw());
            _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
            _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
            // note that the north tilt is negative of the pitch
        }
        _cmdYawRate = 0;

        _thrustMix = THRUST_HOVER_OFFSET + cmdDown;

        // "thrust-trim-adjust"
        if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
        else _thrustMix /= cos(_cmdRoll);

        if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
        else _thrustMix /= cos(_cmdPitch);
    }
    void autoAttitudeLoop(float dt) {
        _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
                                  _nav->getRollRate(), dt);
        _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(),
                                    _nav->getPitchRate(), dt);
        _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
    }
    void setMotorsActive() {
        // turn all motors off if below 0.1 throttle
        if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
            setAllRadioChannelsToNeutral();
        } else {
            _hal->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix);
            _hal->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix);
            _hal->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix);
            _hal->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix);
        }
    }

    // attributes
    /**
     * note that these are not the controller radio channel numbers, they are just
     * unique keys so they can be reaccessed from the hal rc vector
     */
    enum {
        ch_mode = 0, // note scicoslab channels set mode, left, right, front, back order
        ch_right,
        ch_left,
        ch_front,
        ch_back,
        ch_roll,
        ch_pitch,
        ch_thrust,
        ch_yaw
    };
    // must match channel enum
    enum {
        k_chMode = k_radioChannelsStart,
        k_chRight,
        k_chLeft,
        k_chFront,
        k_chBack,
        k_chRoll,
        k_chPitch,
        k_chThr,
        k_chYaw
    };
    enum {
        k_pidGroundSpeed2Throttle = k_controllersStart,
        k_pidStr,
        k_pidPN,
        k_pidPE,
        k_pidPD,
        k_pidRoll,
        k_pidPitch,
        k_pidYawRate,
        k_pidYaw,
    };
    BlockPIDDfb pidRoll, pidPitch, pidYaw;
    BlockPID pidYawRate;
    BlockPIDDfb pidPN, pidPE, pidPD;
    float _thrustMix, _pitchMix, _rollMix, _yawMix;
    float _cmdRoll, _cmdPitch, _cmdYawRate;
};

} // namespace apo

#endif /* CONTROLLERQUAD_H_ */
// vim:ts=4:sw=4:expandtab