/home/travis/build/lis-epfl/MAVRIC_Library/sensing/ins_complementary.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 ins_complementary.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 *
 * \brief This file performs the 3D position estimation, either by direct
 * integration or with correction with the GPS and pos_est->barometer
 *
 ******************************************************************************/


#ifndef INS_COMPLEMENTARY_HPP__
#define INS_COMPLEMENTARY_HPP__

#include <cstdbool>

#include "status/state.hpp"
#include "drivers/gps.hpp"
#include "drivers/barometer.hpp"
#include "drivers/sonar.hpp"
#include "drivers/px4flow.hpp"

#include "sensing/ahrs.hpp"
#include "sensing/ins.hpp"
#include "util/coord_conventions.hpp"
#include "util/constants.hpp"

class INS_complementary: public INS
{
public:

    friend class Mavlink_waypoint_handler;

    struct conf_t
    {
        global_position_t origin;   

        float kp_gps_XY_pos;                    
        float kp_gps_Z_pos;                     
        float kp_gps_XY_vel;                    
        float kp_gps_Z_vel;                     
        float kp_gps_XY_pos_dgps;               
        float kp_gps_Z_pos_dgps;                
        float kp_gps_XY_vel_dgps;               
        float kp_gps_Z_vel_dgps;                
        float kp_gps_XY_pos_rtk;                
        float kp_gps_Z_pos_rtk;                 
        float kp_gps_XY_vel_rtk;                
        float kp_gps_Z_vel_rtk;                 
        float timeout_gps_us;                   
        uint32_t use_gps;                       

        float kp_sonar_alt;                     
        float kp_sonar_vel;                     
        float timeout_sonar_us;                 
        uint32_t use_sonar;                     

        float kp_baro_alt;                      
        float kp_baro_vel;                      
        float timeout_baro_us;                  
        uint32_t use_baro;                      

        float kp_flow_vel;                      
        float flow_gyro_comp_threshold;         
        float timeout_flow_us;                  
        uint32_t use_flow;                      

        float kp_acc_bias;                      
        uint32_t use_acc_bias;                  
    };


    enum fence_violation_state_t
    {
        IN_FENCE = 0,
        OUTSIDE_FENCE1,
        OUTSIDE_FENCE2
    };


    INS_complementary(State& state, const Barometer& barometer, const Sonar& sonar, const Gps& gps, const PX4Flow& flow, const AHRS& ahrs, const conf_t config = default_config());


    bool update(void);


    static inline conf_t default_config();

    float last_update_s(void) const;


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


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


    float absolute_altitude(void) const;


    bool is_healthy(INS::healthy_t type) const;


    local_position_t local_position_;        
    std::array<float,3> vel_;                
    std::array<float,3> acc_bias_;           
    conf_t config_;                          

private:
    const AHRS& ahrs_;                       
    State& state_;                           
    const Gps& gps_;                         
    const Barometer& barometer_;             
    const Sonar& sonar_;                     
    const PX4Flow& flow_;                    

    float dt_s_;                            
    float last_update_s_;                   
    uint64_t last_gps_pos_update_us_;       
    uint64_t last_gps_vel_update_us_;       
    uint64_t last_barometer_update_us_;     
    uint64_t last_sonar_update_us_;         
    uint64_t last_flow_update_us_;          

    bool is_gps_pos_initialized_;           
    local_position_t fence_position_;        

    float barometer_bias_;                   
    bool is_barometer_calibrated_;           

    void reset_velocity_altitude(void);


    void integration(void);

    void correction_from_3d_pos(std::array<float,3> error, std::array<float,3> gain);
    void correction_from_3d_vel(std::array<float,3> error, std::array<float,3> gain);
    void correction_from_z_pos(float error, float gain);
    void correction_from_z_vel(float error, float gain);


    void correction_from_gps_pos(void);


    void correction_from_gps_vel(void);


    void correction_from_gps(void);


    void correction_from_barometer(void);


    void correction_from_sonar(void);


    void correction_from_flow(void);


    void check_first_gps_fix(void);


    void calibrate_barometer(void);

};

INS_complementary::conf_t INS_complementary::default_config()
{
    conf_t conf = {};

    // default origin location (EFPL Esplanade)
    conf.origin        = ORIGIN_EPFL;

    // GPS with 3d fix
    conf.kp_gps_XY_pos  = 2.0f;
    conf.kp_gps_Z_pos   = 1.0f;
    conf.kp_gps_XY_vel  = 2.0f;
    conf.kp_gps_Z_vel   = 1.0f;
    conf.timeout_gps_us = 1e6f;
    conf.use_gps        = 1;

    // GPS with DGPS fix
    conf.kp_gps_XY_pos_dgps  = 20.0f;
    conf.kp_gps_Z_pos_dgps   = 10.0f;
    conf.kp_gps_XY_vel_dgps  = 20.0f;
    conf.kp_gps_Z_vel_dgps   = 10.0f;

    // GPS with RTK fix
    conf.kp_gps_XY_pos_rtk  = 200.0f;
    conf.kp_gps_Z_pos_rtk   = 100.0f;
    conf.kp_gps_XY_vel_rtk  = 200.0f;
    conf.kp_gps_Z_vel_rtk   = 100.0f;


    // Barometer
    conf.kp_baro_alt     = 2.0f;
    conf.kp_baro_vel     = 0.5f;
    conf.timeout_baro_us = 1e6f;
    conf.use_baro        = 1;

    // Sonar
    conf.kp_sonar_alt       = 5.0f;
    conf.kp_sonar_vel       = 3.0f;
    conf.timeout_sonar_us   = 1e6f;
    conf.use_sonar          = 1;

    // Optic flow
    conf.kp_flow_vel                = 4.0f;
    conf.flow_gyro_comp_threshold   = 0.5f;
    conf.timeout_flow_us            = 1e6f;
    conf.use_flow                   = 1;

    // Accelerometer bias
    conf.kp_acc_bias     = 0.001f;
    conf.use_acc_bias    = 0;

    return conf;
};

#endif // INS_COMPLEMENTARY_HPP__