/home/travis/build/lis-epfl/MAVRIC_Library/navigation/navigation_directto.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 navigation_directto.hpp
 *
 * \author MAV'RIC Team
 * \author Basil Huber
 *
 * \brief   Navigation module allowing to navigated to goal position on direct trajectory
 * \details Inherits from Position_controller, which inherits from a Velocity_controller_I
 *          i.e. can take navigation, position and velocity commands
 *
 ******************************************************************************/


#ifndef NAVIGATION_DIRECTTO_HPP_
#define NAVIGATION_DIRECTTO_HPP_

#include "navigation/navigation.hpp"
#include "control/position_controller.hpp"


class Navigation_directto : public Navigation
{
    typedef position_command_t nav_command_t;
public:

    struct conf_t
    {
        float min_cruise_dist_sqr;
        pid_controller_conf_t cruise_pid_config;
        pid_controller_conf_t hover_pid_config;
    };

    enum class ctrl_mode_t
    {
        POS_XYZ,        
        POS_XY_VEL_Z    
    };


    struct args_t
    {
        INS& ins;
    };

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


    virtual bool update();

     bool set_navigation_command(const nav_command_t& command);
    bool set_goal(const nav_command_t& command);

    // virtual bool set_position_command(const pos_command_t& pos_command);

    // virtual bool set_xyposition_zvel_command(const typename TPosition_controller::xypos_zvel_command_t& command);


    inline float distance_to_goal_sqr() const {return distance_to_goal_sqr_;};

    static inline conf_t default_config();

protected:

    pid_controller_conf_t& cruise_pid_config(); // TODO: set return to const when onboard parameter use get/set
    pid_controller_conf_t& hover_pid_config(); // TODO: set return to const when onboard parameter use get/set


private:

    void set_cruise_mode(bool cruise_mode);


    const INS&              ins_;                    
    nav_command_t    navigation_command_;            

    /* parameters */
    pid_controller_conf_t cruise_pid_config_;        
    pid_controller_conf_t hover_pid_config_;         
    pid_controller_conf_t std_pid_config_;           
    float min_cruise_dist_sqr_;                      

    /* auxiliary variables */
    float distance_to_goal_sqr_;                     
};


Navigation_directto::conf_t Navigation_directto::default_config()
{
    conf_t conf;
    conf.min_cruise_dist_sqr                        = 4.0f;

    /* config of the cruise pid controller */
    conf.cruise_pid_config.p_gain                   = 0.7f;
    conf.cruise_pid_config.clip_min                 = 0.0f;
    conf.cruise_pid_config.clip_max                 = 3.0f;
    conf.cruise_pid_config.integrator.gain          = 0.0f;
    conf.cruise_pid_config.integrator.clip_pre      = 0.0f;
    conf.cruise_pid_config.integrator.clip          = 0.0f;
    conf.cruise_pid_config.differentiator.gain      = 0.14f;
    conf.cruise_pid_config.differentiator.clip      = 0.46f;
    conf.cruise_pid_config.soft_zone_width          = 0.0f;

    /* config of the hover pid controller */
    conf.hover_pid_config.p_gain                  = 0.4f;
    conf.hover_pid_config.clip_min                = 0.0f;
    conf.hover_pid_config.clip_max                = 3.0f;
    conf.hover_pid_config.integrator.gain         = 0.0f;
    conf.hover_pid_config.integrator.clip_pre     = 0.0f;
    conf.hover_pid_config.integrator.clip         = 0.0f;
    conf.hover_pid_config.differentiator.gain     = 0.28f;
    conf.hover_pid_config.differentiator.clip     = 0.46f;
    conf.hover_pid_config.soft_zone_width         = 0.0f;

    return conf;
};


#endif /* NAVIGATION_DIRECTTO_HPP_ */