/home/travis/build/lis-epfl/MAVRIC_Library/sensing/imu.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 imu.hpp
 *
 * \author MAV'RIC Team
 * \author Felix Schill
 * \author Gregoire Heitz
 * \author Julien Lecoeur
 *
 * \brief Inertial measurement unit (IMU)
 *
 ******************************************************************************/


#ifndef IMU_HPP_
#define IMU_HPP_

#include <cstdint>
#include <cstdbool>
#include <array>

#include "drivers/accelerometer.hpp"
#include "drivers/gyroscope.hpp"
#include "drivers/magnetometer.hpp"
#include "status/state.hpp"

extern "C"
{
#include "util/quaternions.h"
}

#define GYRO_LPF 0.1f                       ///< The gyroscope linear pass filter gain
#define ACC_LPF 0.05f                       ///< The accelerometer linear pass filter gain
#define MAG_LPF 0.1f                        ///< The magnetometer linear pass filter gain


typedef struct
{
    std::array<float, 3>    bias;         
    std::array<float, 3>    scale_factor; 
    std::array<float, 3>    sign;         
    std::array<uint8_t, 3>  axis;         
    std::array<float, 3>    max_values;   
    std::array<float, 3>    min_values;   
    std::array<float, 3>    mean_values;  
} imu_sensor_config_t;


typedef struct
{
    imu_sensor_config_t accelerometer;  
    imu_sensor_config_t gyroscope;      
    imu_sensor_config_t magnetometer;   
    std::array<float, 3> magnetic_north;
    float lpf_acc;                      
    float lpf_gyro;                     
    float lpf_mag;                      
    float lpf_mean;                     
    float startup_calib_gyro_threshold; 
    float startup_calib_duration_s;     
} imu_conf_t;


static inline imu_conf_t imu_default_config(void);


class Imu
{
public:
    Imu(Accelerometer& accelerometer,
        Gyroscope& gyroscope,
        Magnetometer& magnetometer,
        imu_conf_t config = imu_default_config());


    bool update(void);


    const float& last_update_us(void) const;


    const float& dt_s(void) const;


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


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


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

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


    bool is_ready(void) const;

    imu_conf_t* get_config(void);

    // -------------------------------------------------------------------------
    //
    // TODO implement calibration
    //
    // -------------------------------------------------------------------------

    bool start_accelerometer_bias_calibration(void);


    bool start_gyroscope_bias_calibration(void);


    bool start_magnetometer_bias_calibration(void);


    bool start_magnetic_north_calibration(void);


    bool stop_accelerometer_bias_calibration(void);


    bool stop_gyroscope_bias_calibration(void);


    bool stop_magnetometer_bias_calibration(void);


    bool stop_magnetic_north_calibration(void);


private:
    bool is_calibration_ongoing(void) const;


    void do_calibration(void);


    Accelerometer&  accelerometer_;     
    Gyroscope&      gyroscope_;         
    Magnetometer&   magnetometer_;      

    imu_conf_t      config_;            

    std::array<float, 3> oriented_acc_; 
    std::array<float, 3> oriented_gyro_;
    std::array<float, 3> oriented_mag_; 

    std::array<float, 3> scaled_acc_;   
    std::array<float, 3> scaled_gyro_;  
    std::array<float, 3> scaled_mag_;   
    float magnetic_inclination_;        
    float magnetic_norm_;               

    bool do_startup_calibration_;               
    bool do_accelerometer_bias_calibration_;    
    bool do_gyroscope_bias_calibration_;        
    bool do_magnetometer_bias_calibration_;     
    bool do_magnetic_north_calibration_;        
    bool is_ready_;                             

    float dt_s_;                        
    float last_update_us_;              
    float startup_calibration_start_time_;       
};



static inline imu_conf_t imu_default_config()
{
    imu_conf_t conf = {};

    // Accelerometer
    // Bias
    conf.accelerometer.bias[0] = 0.0f;      
    conf.accelerometer.bias[1] = 0.0f;
    conf.accelerometer.bias[2] = 0.0f;

    // Scale
    conf.accelerometer.scale_factor[0] = 1.0f;      
    conf.accelerometer.scale_factor[1] = 1.0f;
    conf.accelerometer.scale_factor[2] = 1.0f;

    // Axis and sign
    conf.accelerometer.sign[0] = +1.0f; 
    conf.accelerometer.sign[1] = +1.0f;
    conf.accelerometer.sign[2] = +1.0f;
    conf.accelerometer.axis[0] = 0;     
    conf.accelerometer.axis[1] = 1;
    conf.accelerometer.axis[2] = 2;

    // Min and max values
    conf.accelerometer.max_values[0] = -10000.0f;
    conf.accelerometer.max_values[1] = -10000.0f;
    conf.accelerometer.max_values[2] = -10000.0f;
    conf.accelerometer.min_values[0] = 10000.0f;
    conf.accelerometer.min_values[1] = 10000.0f;
    conf.accelerometer.min_values[2] = 10000.0f;
    conf.accelerometer.mean_values[0] = 0.0f;
    conf.accelerometer.mean_values[1] = 0.0f;
    conf.accelerometer.mean_values[2] = 0.0f;

    // Gyroscope
    // Bias
    conf.gyroscope.bias[0] = 0.0f;      
    conf.gyroscope.bias[1] = 0.0f;
    conf.gyroscope.bias[2] = 0.0f;

    // Scale
    conf.gyroscope.scale_factor[0] = 1.0f;      
    conf.gyroscope.scale_factor[1] = 1.0f;
    conf.gyroscope.scale_factor[2] = 1.0f;

    // Axis and sign
    conf.gyroscope.sign[0] = +1.0f; 
    conf.gyroscope.sign[1] = +1.0f;
    conf.gyroscope.sign[2] = +1.0f;
    conf.gyroscope.axis[0] = 0;     
    conf.gyroscope.axis[1] = 1;
    conf.gyroscope.axis[2] = 2;

    // Min and max values
    conf.gyroscope.max_values[0] = -10000.0f;
    conf.gyroscope.max_values[1] = -10000.0f;
    conf.gyroscope.max_values[2] = -10000.0f;
    conf.gyroscope.min_values[0] = 10000.0f;
    conf.gyroscope.min_values[1] = 10000.0f;
    conf.gyroscope.min_values[2] = 10000.0f;
    conf.gyroscope.mean_values[0] = 0.0f;
    conf.gyroscope.mean_values[1] = 0.0f;
    conf.gyroscope.mean_values[2] = 0.0f;

    // Magnetometer
    // Bias
    conf.magnetometer.bias[0] = 0.0f;       
    conf.magnetometer.bias[1] = 0.0f;
    conf.magnetometer.bias[2] = 0.0f;

    // Scale
    conf.magnetometer.scale_factor[0] = 1.0f;       
    conf.magnetometer.scale_factor[1] = 1.0f;
    conf.magnetometer.scale_factor[2] = 1.0f;

    // Axis and sign
    conf.magnetometer.sign[0] = +1.0f;  
    conf.magnetometer.sign[1] = +1.0f;
    conf.magnetometer.sign[2] = +1.0f;
    conf.magnetometer.axis[0] = 0;      
    conf.magnetometer.axis[1] = 1;
    conf.magnetometer.axis[2] = 2;

    // Min and max values
    conf.magnetometer.max_values[0] = -10000.0f;
    conf.magnetometer.max_values[1] = -10000.0f;
    conf.magnetometer.max_values[2] = -10000.0f;
    conf.magnetometer.min_values[0] = 10000.0f;
    conf.magnetometer.min_values[1] = 10000.0f;
    conf.magnetometer.min_values[2] = 10000.0f;
    conf.magnetometer.mean_values[0] = 0.0f;
    conf.magnetometer.mean_values[1] = 0.0f;
    conf.magnetometer.mean_values[2] = 0.0f;

    // Magnetic north (default value in Lausanne: 62° inclination)
    conf.magnetic_north[0] = 0.46947156f;   // cos(62°)
    conf.magnetic_north[1] = 0.0f;
    conf.magnetic_north[2] = 0.88294759f;   // sin(62°)

    // Low pass filter
    conf.lpf_acc    = 0.1f;
    conf.lpf_gyro   = 0.05f;
    conf.lpf_mag    = 0.1f;
    conf.lpf_mean   = 0.01f;

    // startup calibration length
    conf.startup_calib_gyro_threshold = 0.5f;
    conf.startup_calib_duration_s     = 10.0f;

    return conf;
}

#endif /* IMU_HPP_ */