/home/travis/build/lis-epfl/MAVRIC_Library/control/position_controller.hpp

/*******************************************************************************
 * Copyright (c) 2009-2016, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file position_controller.hpp
 *
 * \author MAV'RIC Team
 * \author Basil Huber
 *
 * \brief Implementation of position controller
 *
 ******************************************************************************/


#ifndef POSITION_CONTROLLER_HPP_
#define POSITION_CONTROLLER_HPP_

#include "control/controller.hpp"
#include "control/control_command.hpp"
#include "control/pid_controller.hpp"
#include "sensing/ahrs.hpp"
#include "sensing/ins.hpp"


class Position_controller : public Controller<position_command_t, velocity_command_t>
{
public:

    struct conf_t
    {
        float cruise_speed;
        float cruise_dist;
        float max_climb_rate;
        float kp_yaw;
        pid_controller_conf_t pid_config;
    };


    static inline conf_t default_config();


    struct args_t
    {
        const AHRS& ahrs;     
        const INS& ins;         
    };


    Position_controller(args_t args, const conf_t& config = default_config());


    virtual bool update();


    bool set_command(const position_command_t& pos);


    bool get_command(position_command_t& pos) const;


    bool get_output(velocity_command_t& vel) const;


    pid_controller_t& get_pid(void);

private:
    const INS&          ins_;                       
    const AHRS&       ahrs_;                      

    position_command_t  position_command_;          
    velocity_command_t  velocity_command_;          

    pid_controller_t    pid_controller_;            //< position pid controller
    float cruise_speed_;                            
    float cruise_dist_;                             
    float max_climb_rate_;                          
    float kp_yaw_;
};


Position_controller::conf_t Position_controller::default_config()
{
    conf_t conf;
    conf.cruise_speed       = 3.0f;
    conf.cruise_dist        = 10.0f;
    conf.max_climb_rate     = 1.0f;
    conf.kp_yaw             = 0.5f;

    conf.pid_config.p_gain                  = 1.0f;
    conf.pid_config.clip_min                = 0.0f;
    conf.pid_config.clip_max                = 3.0f;
    conf.pid_config.integrator.gain         = 0.0f;
    conf.pid_config.integrator.clip_pre     = 0.0f;
    conf.pid_config.integrator.clip         = 0.0f;
    conf.pid_config.differentiator.gain     = 0.0f;
    conf.pid_config.differentiator.clip     = 0.0f;
    conf.pid_config.soft_zone_width         = 0.0f;

    return conf;
};


#endif /* POSITION_CONTROLLER_HPP_ */