/home/travis/build/lis-epfl/MAVRIC_Library/control/velocity_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 velocity_controller.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 *
 * \brief  Velocity controller base class
 *
 ******************************************************************************/


#ifndef VELOCITY_CONTROLLER_HPP_
#define VELOCITY_CONTROLLER_HPP_

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

class Velocity_controller : public Controller_1_to_2<velocity_command_t, attitude_command_t, thrust_command_t>
{
public:

    enum control_frame_t
    {
        LOCAL_FRAME     = 0,    
        SEMILOCAL_FRAME = 1,    
        BODY_FRAME      = 2     
    };


    struct conf_t
    {
        control_frame_t         control_frame;          
        pid_controller_conf_t   pid_config[3];          
    };


    struct args_t
    {
        const AHRS&                  ahrs;                      
        const INS&                   ins;                       
        velocity_command_t&          velocity_command;          
        attitude_command_t&          attitude_command;          
        thrust_command_t&            thrust_command;            
    };


    Velocity_controller(const args_t& args, const conf_t& config);


    bool update(void);


    bool set_command(const velocity_command_t& vel);


    bool get_command(velocity_command_t& vel) const;


    bool get_output(attitude_command_t& att) const;


    bool get_output(thrust_command_t& thrust) const;


    pid_controller_t& get_pid_X(void);
    pid_controller_t& get_pid_Y(void);
    pid_controller_t& get_pid_Z(void);

protected:

    virtual bool compute_attitude_and_thrust_from_desired_accel(const std::array<float,3>& accel_vector,
                                                                attitude_command_t& attitude_command,
                                                                thrust_command_t& thrust_command            ) = 0;

    const AHRS&          ahrs_;                      
    const INS&           ins_;                       
    velocity_command_t&  velocity_command_;          
    attitude_command_t&  attitude_command_;          
    thrust_command_t&    thrust_command_;            

    control_frame_t      control_frame_;             
    pid_controller_t     pid_[3];                    
};

#endif /* VELOCITY_CONTROLLER_HPP_ */