mpu6050.h

Go to the documentation of this file.

//   Copyright 2016-2023 Jean-Francois Poilpret
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
 
 
#ifndef MPU6050_H
#define MPU6050_H
 
#include <math.h>
#include <stdint.h>
#include "common_magneto.h"
#include "../array.h"
#include "../bits.h"
#include "../functors.h"
#include "../i2c_device.h"
#include "../i2c_device_utilities.h"
#include "../utilities.h"
 
//TODO rename namespace: "magneto" is not relevant here, eg "motion" or "motion_sensor"
namespace devices::magneto
{
    enum class GyroRange : uint8_t
    {
        RANGE_250 = 0 << 3,
        RANGE_500 = 1 << 3,
        RANGE_1000 = 2 << 3,
        RANGE_2000 = 3 << 3,
    };
    
    static constexpr uint16_t GYRO_RANGE_DPS(GyroRange range)
    {
        if (range == GyroRange::RANGE_2000) return 2000;
        if (range == GyroRange::RANGE_1000) return 1000;
        if (range == GyroRange::RANGE_500) return 500;
        return 250;
    }
    
    enum class AccelRange : uint8_t
    {
        RANGE_2G = 0 << 3,
        RANGE_4G = 1 << 3,
        RANGE_8G = 2 << 3,
        RANGE_16G = 3 << 3,
    };
    
    static constexpr uint16_t ACCEL_RANGE_G(AccelRange range)
    {
        if (range == AccelRange::RANGE_16G) return 16;
        if (range == AccelRange::RANGE_8G) return 8;
        if (range == AccelRange::RANGE_4G) return 4;
        return 2;
    }
    
    enum class ClockSelect : uint8_t
    {
        INTERNAL_8MHZ = 0,
        PLL_X_AXIS_GYRO = 1,
        PLL_Y_AXIS_GYRO = 2,
        PLL_Z_AXIS_GYRO = 3,
        PLL_EXTERNAL_32KHZ = 4,
        PLL_EXTERNAL_19MHZ = 5,
        STOPPED = 7
    };
 
    enum class DLPF : uint8_t
    {
        ACCEL_BW_260HZ = 0,
        ACCEL_BW_184HZ = 1,
        ACCEL_BW_94HZ = 2,
        ACCEL_BW_44HZ = 3,
        ACCEL_BW_21HZ = 4,
        ACCEL_BW_10HZ = 5,
        ACCEL_BW_5HZ = 6,
 
        GYRO_BW_256HZ = 0,
        GYRO_BW_188HZ = 1,
        GYRO_BW_98HZ = 2,
        GYRO_BW_42HZ = 3,
        GYRO_BW_20HZ = 4,
        GYRO_BW_10HZ = 5,
        GYRO_BW_5HZ = 6
    };
    
    class FIFOEnable
    {
    public:
        constexpr FIFOEnable(
            bool accel = false, bool gyro_x = false, bool gyro_y = false, bool gyro_z = false, bool temperature = false)
            : data_{bits::ORIF8(accel, ACCEL_FIFO_EN, temperature, TEMP_FIFO_EN, 
                gyro_x, XG_FIFO_EN, gyro_y, YG_FIFO_EN, gyro_z, ZG_FIFO_EN)} {}
 
        bool accel() const
        {
            return data_ & ACCEL_FIFO_EN;
        }
 
        bool gyro_x() const
        {
            return data_ & XG_FIFO_EN;
        }
 
        bool gyro_y() const
        {
            return data_ & YG_FIFO_EN;
        }
 
        bool gyro_z() const
        {
            return data_ & ZG_FIFO_EN;
        }
 
        bool temperature() const
        {
            return data_ & TEMP_FIFO_EN;
        }
 
    private:
        static constexpr uint8_t TEMP_FIFO_EN = bits::BV8(7);
        static constexpr uint8_t XG_FIFO_EN = bits::BV8(6);
        static constexpr uint8_t YG_FIFO_EN = bits::BV8(5);
        static constexpr uint8_t ZG_FIFO_EN = bits::BV8(4);
        static constexpr uint8_t ACCEL_FIFO_EN = bits::BV8(3);
 
        uint8_t data_;
    };
 
    class INTStatus
    {
    public:
        constexpr INTStatus(bool data_ready = false, bool overflow = false)
            : data_{bits::ORIF8(data_ready, DATA_RDY_INT, overflow, FIFO_OFLOW_INT)} {}
 
        bool data_ready() const
        {
            return data_ & DATA_RDY_INT;
        }
 
        bool overflow() const
        {
            return data_ & FIFO_OFLOW_INT;
        }
 
    private:
        static constexpr uint8_t FIFO_OFLOW_INT = bits::BV8(4);
        static constexpr uint8_t DATA_RDY_INT = bits::BV8(0);
 
        uint8_t data_;
    };
 
    using INTEnable = INTStatus;
 
    struct AllSensors
    {
        Sensor3D accel;
        int16_t temperature;
        Sensor3D gyro;
    };
 
    enum class AD0 : uint8_t
    {
        LOW = 0,
        HIGH = 1
    };
 
    template<typename MANAGER>
    class MPU6050 : public i2c::I2CDevice<MANAGER>
    {
    private:
        using PARENT = i2c::I2CDevice<MANAGER>;
        template<typename OUT, typename IN> using FUTURE = typename PARENT::template FUTURE<OUT, IN>;
 
        // Forward declarations needed by compiler
        template<uint8_t REGISTER, typename T = uint8_t, typename FUNCTOR = functor::ChangeEndianness<T>>
        using TReadRegisterFuture = i2c::TReadRegisterFuture<MANAGER, REGISTER, T, FUNCTOR>;
        template<uint8_t REGISTER, typename T = uint8_t, typename FUNCTOR = functor::ChangeEndianness<T>>
        using TWriteRegisterFuture = i2c::TWriteRegisterFuture<MANAGER, REGISTER, T, FUNCTOR>;
 
        template<uint8_t REGISTER>
        using Sensor3DFuture = TReadRegisterFuture<REGISTER, Sensor3D>;
 
        static constexpr const uint8_t SMPRT_DIV = 0x19;
        static constexpr const uint8_t CONFIG = 0x1A;
        static constexpr const uint8_t GYRO_CONFIG = 0x1B;
        static constexpr const uint8_t ACCEL_CONFIG = 0x1C;
 
        static constexpr const uint8_t FIFO_EN = 0x23;
        static constexpr const uint8_t INT_PIN_CFG = 0x37;
        static constexpr const uint8_t INT_ENABLE = 0x38;
        static constexpr const uint8_t INT_STATUS = 0x3A;
 
        static constexpr const uint8_t ACCEL_XOUT = 0x3B;
        static constexpr const uint8_t TEMP_OUT = 0x41;
        static constexpr const uint8_t GYRO_XOUT = 0x43;
 
        static constexpr const uint8_t USER_CTRL = 0x6A;
        static constexpr const uint8_t FIFO_RESET = 0x04;
        static constexpr const uint8_t FIFO_ENABLE = 0x40;
 
        static constexpr const uint8_t PWR_MGMT_1 = 0x6B;
        static constexpr const uint8_t PWR_MGMT_2 = 0x6C;
 
        static constexpr const uint8_t FIFO_COUNT = 0x72;
        static constexpr const uint8_t FIFO_R_W = 0x74;
 
        static constexpr const uint8_t WHO_AM_I = 0x75;
 
        class PowerManagement;
        using PowerManagementFuture = TWriteRegisterFuture<PWR_MGMT_1, PowerManagement>;
 
    public:
        explicit MPU6050(MANAGER& manager, AD0 ad0 = AD0::LOW)
            : PARENT{manager, DEVICE_ADDRESS(ad0), i2c::I2C_FAST, true} {}
 
        // Asynchronous API
        //==================
        class BeginFuture : public FUTURE<void, containers::array<uint8_t, 6>>
        {
            using PARENT = FUTURE<void, containers::array<uint8_t, 6>>;
        public:
            explicit BeginFuture(   GyroRange gyro_range = GyroRange::RANGE_250,
                                    AccelRange accel_range = AccelRange::RANGE_2G,
                                    DLPF low_pass_filter = DLPF::ACCEL_BW_260HZ,
                                    ClockSelect clock_select = ClockSelect::INTERNAL_8MHZ)
                : PARENT{{  CONFIG, uint8_t(low_pass_filter), uint8_t(gyro_range), uint8_t(accel_range),
                            PWR_MGMT_1, utils::as_uint8_t(PowerManagement{clock_select})}} {}
        };
 
        int begin(BeginFuture& future)
        {
            // We split the transaction in 2 write commands (3 bytes starting at CONFIG, 1 byte at PWR_MGT_1)
            return this->launch_commands(future, {this->write(4), this->write(2)});
        }
 
        class FifoBeginFuture : public FUTURE<void, containers::array<uint8_t, 15>>
        {
            using PARENT = FUTURE<void, containers::array<uint8_t, 15>>;
        public:
            explicit FifoBeginFuture(   FIFOEnable fifo_enable,
                                        INTEnable int_enable,
                                        uint8_t sample_rate_divider,
                                        GyroRange gyro_range = GyroRange::RANGE_250,
                                        AccelRange accel_range = AccelRange::RANGE_2G,
                                        DLPF low_pass_filter = DLPF::ACCEL_BW_260HZ,
                                        ClockSelect clock_select = ClockSelect::INTERNAL_8MHZ)
                : PARENT{{  CONFIG, uint8_t(low_pass_filter), uint8_t(gyro_range), uint8_t(accel_range),
                            PWR_MGMT_1, utils::as_uint8_t(PowerManagement{clock_select}),
                            SMPRT_DIV, sample_rate_divider,
                            FIFO_EN, utils::as_uint8_t(fifo_enable),
                            INT_PIN_CFG, 0, utils::as_uint8_t(int_enable),
                            USER_CTRL, FIFO_ENABLE | FIFO_RESET}} {}
        };
 
        int begin(FifoBeginFuture& future)
        {
            return this->launch_commands(future, {  
                // CONFIG, GYRO_CONFIG, ACCEL_CONFIG
                this->write(4),
                // PWR_MGMT_1
                this->write(2),
                // SMPRT_DIV
                this->write(2),
                // FIFO_EN
                this->write(2),
                // INT_PIN_CFG
                this->write(3),
                // USER_CONTROL
                this->write(2),
            });
        }
 
        class EndFuture : public PowerManagementFuture
        {
        public:
            EndFuture() : PowerManagementFuture{PowerManagement{false, false, true, false}} {}
        };
 
        int end(EndFuture& future) INLINE
        {
            // Put to sleep mode
            return this->async_write(future);
        }
 
        class ResetFuture : public PowerManagementFuture
        {
        public:
            ResetFuture() : PowerManagementFuture{PowerManagement{false, false, false, true}} {}
        };
 
        int reset(ResetFuture& future) INLINE
        {
            return this->async_write(future);
        }
 
        using GyroFuture = Sensor3DFuture<GYRO_XOUT>;
 
        int gyro_measures(GyroFuture& future)
        {
            return this->async_read(future);
        }
 
        using TemperatureFuture = TReadRegisterFuture<TEMP_OUT, int16_t>;
 
        int temperature(TemperatureFuture& future)
        {
            return this->async_read(future);
        }
 
        static constexpr int16_t convert_temp_to_centi_degrees(int16_t temp)
        {
            // MPU-6000 Register Map datasheet §4.18 formula: Tc = TEMP / 340 + 36.53
            return int16_t(temp * 10L / 34L + 3653);
        }
 
        using AccelFuture = Sensor3DFuture<ACCEL_XOUT>;
 
        int accel_measures(AccelFuture& future)
        {
            return this->async_read(future);
        }
 
        using AllMeasuresFuture = 
            TReadRegisterFuture<ACCEL_XOUT, AllSensors, functor::ChangeEndianness<AllSensors, int16_t>>;
 
        int all_measures(AllMeasuresFuture& future)
        {
            return this->async_read(future);
        }
 
        using InterruptStatusFuture = TReadRegisterFuture<INT_STATUS, INTStatus>;
 
        int interrupt_status(InterruptStatusFuture& future)
        {
            return this->async_read(future);
        }
 
        using ResetFifoFuture = 
            TWriteRegisterFuture<USER_CTRL, uint8_t, functor::Constant<uint8_t, FIFO_ENABLE | FIFO_RESET>>;
 
        int reset_fifo(ResetFifoFuture& future)
        {
            return this->async_write(future);
        }
 
        using FifoCountFuture = TReadRegisterFuture<FIFO_COUNT, uint16_t>;
 
        int fifo_count(FifoCountFuture& future)
        {
            return this->async_read(future);
        }
 
        template<typename T>
        using FifoPopFuture = TReadRegisterFuture<FIFO_R_W, T, functor::ChangeEndianness<T, int16_t>>;
 
        template<typename T> int fifo_pop(FifoPopFuture<T>& future)
        {
            return this->async_read(future);
        }
 
        using FifoPushFuture = TWriteRegisterFuture<FIFO_R_W>;
 
        int fifo_push(FifoPushFuture& future)
        {
            return this->async_write(future);
        }
 
        // Synchronous API
        //=================
        bool begin( GyroRange gyro_range = GyroRange::RANGE_250,
                    AccelRange accel_range = AccelRange::RANGE_2G,
                    DLPF low_pass_filter = DLPF::ACCEL_BW_260HZ,
                    ClockSelect clock_select = ClockSelect::INTERNAL_8MHZ)
        {
            BeginFuture future{gyro_range, accel_range, low_pass_filter, clock_select};
            if (begin(future) != 0) return false;
            return (future.await() == future::FutureStatus::READY);
        }
 
        bool begin( FIFOEnable fifo_enable,
                    INTEnable int_enable,
                    uint8_t sample_rate_divider,
                    GyroRange gyro_range = GyroRange::RANGE_250,
                    AccelRange accel_range = AccelRange::RANGE_2G,
                    DLPF low_pass_filter = DLPF::ACCEL_BW_260HZ,
                    ClockSelect clock_select = ClockSelect::INTERNAL_8MHZ)
        {
            FifoBeginFuture future{
                fifo_enable, int_enable, sample_rate_divider, gyro_range, accel_range, low_pass_filter, clock_select};
            if (begin(future) != 0) return false;
            return (future.await() == future::FutureStatus::READY);
        }
 
        bool end() INLINE
        {
            return this->template sync_write<EndFuture>();
        }
 
        bool reset() INLINE
        {
            return this->template sync_write<ResetFuture>();
        }
 
        bool gyro_measures(Sensor3D& gyro)
        {
            return this->template sync_read<GyroFuture>(gyro);
        }
 
        int16_t temperature()
        {
            int16_t temp = INT16_MIN;
            this->template sync_read<TemperatureFuture>(temp);
            return temp;
        }
 
        bool accel_measures(Sensor3D& accel)
        {
            return this->template sync_read<AccelFuture>(accel);
        }
 
        bool all_measures(AllSensors& sensors)
        {
            return this->template sync_read<AllMeasuresFuture>(sensors);
        }
 
        INTStatus interrupt_status()
        {
            INTStatus status;
            this->template sync_read<InterruptStatusFuture>(status);
            return status;
        }
 
        bool reset_fifo()
        {
            return this->template sync_write<ResetFifoFuture>();
        }
 
        uint16_t fifo_count()
        {
            uint16_t count = 0;
            this->template sync_read<FifoCountFuture>(count);
            return count;
        }
 
        template<typename T> bool fifo_pop(T& output)
        {
            return this->template sync_read<FifoPopFuture<T>>(output);
        }
 
        bool fifo_push(uint8_t data)
        {
            return this->template sync_write<FifoPushFuture>(data);
        }
 
    private:
        class PowerManagement
        {
        public:
            constexpr PowerManagement() : value_{} {}
            explicit constexpr PowerManagement(ClockSelect clock_select, bool temp_disable = false,
                            bool cycle = false, bool sleep = false, bool device_reset = false) 
                :   value_{ bits::OR8(uint8_t(clock_select), bits::IF8(temp_disable, TEMP_DIS_MASK),
                            bits::IF8(cycle, CYCLE_MASK), bits::IF8(sleep, SLEEP_MASK), 
                            bits::IF8(device_reset, RESET_MASK))} {}
            constexpr PowerManagement(bool temp_disable, bool cycle, bool sleep, bool device_reset) 
                :   value_{ bits::ORIF8(temp_disable, TEMP_DIS_MASK, cycle, CYCLE_MASK,
                            sleep, SLEEP_MASK, device_reset, RESET_MASK)} {}
 
            ClockSelect clock_select() const
            {
                return static_cast<ClockSelect>(value_ & CLOCK_SEL_MASK);
            }
 
            bool temp_disable() const
            {
                return value_ & TEMP_DIS_MASK;
            }
 
            bool cycle() const
            {
                return value_ & CYCLE_MASK;
            }
 
            bool sleep() const
            {
                return value_ & SLEEP_MASK;
            }
 
            bool device_reset() const
            {
                return value_ & RESET_MASK;
            }
 
        private:
            static constexpr uint8_t CLOCK_SEL_MASK = bits::BV8(0, 1, 2);
            static constexpr uint8_t TEMP_DIS_MASK = bits::BV8(3);
            static constexpr uint8_t CYCLE_MASK = bits::BV8(5);
            static constexpr uint8_t SLEEP_MASK = bits::BV8(6);
            static constexpr uint8_t RESET_MASK = bits::BV8(7);
            uint8_t value_;
        };
 
        static constexpr uint8_t DEVICE_ADDRESS(AD0 ad0)
        {
            return uint8_t(uint8_t(0x68 | uint8_t(ad0)) << 1);
        }
    };
}
 
#endif /* MPU6050_H */