control.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2020 Flight Dynamics and Control Lab
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef FDCL_CONTROL_HPP
#define FDCL_CONTROL_HPP


// user headers
#include "common_types.hpp"
#include "fdcl/param.hpp"
#include "fdcl/integral_utils.hpp"
#include "fdcl/matrix_utils.hpp"


// external headers
#include "Eigen/Dense"


namespace fdcl
{
class control
{
public:
    double t = 0.0;  
    double t_pre = 0.0;  
    double dt = 1e-9;  
    int freq = 100;  
    bool use_integral = false;  
    // for integral controller
    fdcl::integral_error_vec3 eIR; 
    fdcl::integral_error eI1; 
    fdcl::integral_error eI2; 
    fdcl::integral_error eIy; 
    fdcl::integral_error_vec3 eIX; 
    Vector3 eR = Vector3::Zero(); 
    Vector3 eW = Vector3::Zero(); 
    Vector3 ei = Vector3::Zero(); 
    Vector3 M = Vector3::Zero();  
    Vector3 eX = Vector3::Zero(); 
    Vector3 eV = Vector3::Zero(); 
    Vector3 b1 = Vector3::Zero(); 
    Vector3 b2 = Vector3::Zero(); 
    Vector3 b3 = Vector3::Zero(); 
    Vector3 b3_dot = Vector3::Zero(); 
    Eigen::Matrix<double, 4, 4> fM_to_forces_inv; 
    Vector4 f_motor; 
    Vector4 fM = Vector4::Zero(); 
    double f_total = 0.0;  
    control(
        fdcl::state_t *&state_, 
        fdcl::command_t *&command_, 
        fdcl::param *config_file_  
    );
    control(void); // Default constructor (this is should not be used)
    ~control(void);

    void init(void);

    void load_config(void);

    void set_error_to_zero(void);

    void attitude_control(void);

    void attitude_control_decoupled_yaw(void);


    void position_control(void);


    double get_time(void);


    void output_uav_properties(double &m, Matrix3 &J);


    void output_fM(double &f, Vector3 &M);


private:
    fdcl::state_t *state = nullptr; 
    fdcl::command_t *command = nullptr; 
    fdcl::param *config_file = nullptr; 
    bool use_decoupled_yaw = true; 
    Vector3 e1; 
    Vector3 e2; 
    Vector3 e3; 
    double m = 0.0;  
    double g = 9.81;  
    Matrix3 J = Matrix3::Zero();  
    double c_tf = 0.0;  
    double l = 0.0;  
    struct timespec tspec_init; 
    struct timespec tspec_curr; 
     // Attitude gains
    Matrix3 kR = Matrix3::Zero();  
    Matrix3 kW = Matrix3::Zero();  
    double kyw = 0.0; 
    // Position gains
    Matrix3 kX = Matrix3::Zero(); 
    Matrix3 kV = Matrix3::Zero(); 
    // Integral gains
    double kIR = 0.0;  
    double ki = 0.0;  
    double kI = 0.0;  
    double kyI = 0.0;  
    double kIX = 0.0;  
    double c1 = 0.0;  
    double c2 = 0.0;  
    double c3 = 0.0;  
};  // end of control class

}  // end of namespace fdcl

#endif