/home/travis/build/lis-epfl/MAVRIC_Library/simulation/dynamic_model_quad_diag.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 dynamic_model_quad_diag.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 *
 * \brief Simulated dynamics of a quadcopter in diag configuration
 *
 ******************************************************************************/


#ifndef DYNAMIC_MODEL_QUAD_DIAG_HPP_
#define DYNAMIC_MODEL_QUAD_DIAG_HPP_


#include "simulation/dynamic_model.hpp"
#include "drivers/servo.hpp"
#include "util/constants.hpp"

typedef struct
{
    float rotor_lpf;                    
    float rotor_rpm_gain;               
    float rotor_rpm_offset;             

    float rotor_cd;                     
    float rotor_cl;                     
    float rotor_diameter;               
    float rotor_foil_area;              

    float rotor_pitch;                  
    float total_mass;                   
    float vehicle_drag;                 
    float roll_pitch_momentum;          
    float yaw_momentum;                 

    float rotor_momentum;               
    float rotor_arm_length;             

    float wind_x;                       
    float wind_y;                       

    float gravity;                      
    float air_density;                  

    std::array<float,4> motor_dir;
} dynamic_model_quad_diag_conf_t;


static inline dynamic_model_quad_diag_conf_t dynamic_model_quad_diag_default_config();


class Dynamic_model_quad_diag: public Dynamic_model
{
public:
    Dynamic_model_quad_diag(Servo& servo_rear_left,
                            Servo& servo_front_left,
                            Servo& servo_front_right,
                            Servo& servo_rear_right,
                            dynamic_model_quad_diag_conf_t config = dynamic_model_quad_diag_default_config());


    bool update(void);


    const float& last_update_us(void) const;


    const std::array<float, 3>& acceleration_bf(void) const;


    const std::array<float, 3>& velocity_lf(void) const;


    const local_position_t& position_lf(void) const;


    const global_position_t& position_gf(void) const;


    const std::array<float, 3>& angular_velocity_bf(void) const;


    const quat_t& attitude(void) const;

private:
    Servo& servo_front_right_;              
    Servo& servo_front_left_;               
    Servo& servo_rear_right_;               
    Servo& servo_rear_left_;                

    dynamic_model_quad_diag_conf_t config_; 

    std::array<float, 4> rotorspeeds_;      
    std::array<float, 3> torques_bf_;       
    std::array<float, 3> rates_bf_;         
    std::array<float, 3> lin_forces_bf_;        
    std::array<float, 3> acc_bf_;           
    std::array<float, 3> vel_bf_;           
    std::array<float, 3> vel_;              
    quat_t attitude_;                       

    local_position_t local_position_;       
    global_position_t global_position_;     

    float last_update_us_;                  
    float dt_s_;                            


    void forces_from_servos(void);


    float lift_drag_base(float rpm, float sqr_lat_airspeed, float axial_airspeed);
};



static inline dynamic_model_quad_diag_conf_t dynamic_model_quad_diag_default_config()
{
    dynamic_model_quad_diag_conf_t conf = {};

    conf.rotor_lpf              = 0.1f;                 
    conf.rotor_rpm_gain         = 4000.0f;              
    conf.rotor_rpm_offset       = -1.0f;                
    conf.rotor_cd               = 0.03f;                
    conf.rotor_cl               = 1.0f;                 
    conf.rotor_diameter         = 0.14f;                
    conf.rotor_foil_area        = 0.18f * 0.015f;       
    conf.rotor_pitch            = 0.15f;                
    conf.total_mass             = 0.35f;                
    conf.vehicle_drag           = 0.01f;                
    conf.roll_pitch_momentum    = 0.1f * 0.17f / 1.4142f;   
    conf.yaw_momentum           = 0.1f * 0.17f;         
    conf.rotor_momentum         = 0.005f * 0.03f;       
    conf.rotor_arm_length       = 0.17f;                
    conf.wind_x                 = 0.0f;                 
    conf.wind_y                 = 0.0f;                 
    conf.gravity                = 9.8f;                 
    conf.air_density            = 1.2f;                 
    conf.motor_dir              = std::array<float,4>{{-1.0f, 1.0f , -1.0f, 1.0f}};

    return conf;
}


#endif /* DYNAMIC_MODEL_QUAD_DIAG_HPP_ */