diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..523faf7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "main.c" "QMC5883L.c" "mpu6050.c" "kalman.c" "I2Cdev.c" + INCLUDE_DIRS "." +) diff --git a/I2Cdev.c b/I2Cdev.c new file mode 100644 index 0000000..5573f6d --- /dev/null +++ b/I2Cdev.c @@ -0,0 +1,153 @@ + + +#include +#include +#include +#include +#include "sdkconfig.h" +#include "I2Cdev.h" + +#define I2C_NUM I2C_NUM_0 + +#undef ESP_ERROR_CHECK +#define ESP_ERROR_CHECK(x) do { esp_err_t rc = (x); if (rc != ESP_OK) { ESP_LOGE("err", "esp_err_t = %d", rc); /*assert(0 && #x);*/} } while(0); + + +/** Initialize I2C0 + */ +// void I2Cdev_initialize() { + +// } + +void I2Cdev_enable(bool isEnabled) { + +} + +/** Default timeout value for read operations. + */ +uint16_t I2Cdev_readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + + +int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { + + uint8_t b; + uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + + +int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { + uint8_t count, b; + if ((count = I2Cdev_readByte(devAddr, regAddr, &b, timeout)) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + b &= mask; + b >>= (bitStart - length + 1); + *data = b; + } + return count; +} + +int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { + return I2Cdev_readBytes(devAddr, regAddr, 1, data, timeout); +} + +int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { + i2c_cmd_handle_t cmd; + I2Cdev_SelectRegister(devAddr, regAddr); + + cmd = i2c_cmd_link_create(); + ESP_ERROR_CHECK(i2c_master_start(cmd)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_READ, 1)); + + if(length>1) + ESP_ERROR_CHECK(i2c_master_read(cmd, data, length-1, I2C_MASTER_ACK)); + + ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+length-1, I2C_MASTER_NACK)); + + ESP_ERROR_CHECK(i2c_master_stop(cmd)); + ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS)); + i2c_cmd_link_delete(cmd); + + return length; +} + +bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data){ + + uint8_t data1[] = {(uint8_t)(data>>8), (uint8_t)(data & 0xff)}; + I2Cdev_writeBytes(devAddr, regAddr, 2, data1); + return true; +} + +void I2Cdev_SelectRegister(uint8_t devAddr, uint8_t regAddr){ + i2c_cmd_handle_t cmd; + + cmd = i2c_cmd_link_create(); + ESP_ERROR_CHECK(i2c_master_start(cmd)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1)); + ESP_ERROR_CHECK(i2c_master_stop(cmd)); + ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS)); + i2c_cmd_link_delete(cmd); +} + + +bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { + uint8_t b; + I2Cdev_readByte(devAddr, regAddr, &b, I2Cdev_readTimeout); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return I2Cdev_writeByte(devAddr, regAddr, b); +} + +bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { + + uint8_t b = 0; + if (I2Cdev_readByte(devAddr, regAddr, &b, I2Cdev_readTimeout) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return I2Cdev_writeByte(devAddr, regAddr, b); + } else { + return false; + } +} + +bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { + i2c_cmd_handle_t cmd; + + cmd = i2c_cmd_link_create(); + ESP_ERROR_CHECK(i2c_master_start(cmd)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data, 1)); + ESP_ERROR_CHECK(i2c_master_stop(cmd)); + ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS)); + i2c_cmd_link_delete(cmd); + + return true; +} + +bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){ + i2c_cmd_handle_t cmd; + + cmd = i2c_cmd_link_create(); + ESP_ERROR_CHECK(i2c_master_start(cmd)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1)); + ESP_ERROR_CHECK(i2c_master_write(cmd, data, length-1, 0)); + ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data[length-1], 1)); + ESP_ERROR_CHECK(i2c_master_stop(cmd)); + ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS)); + i2c_cmd_link_delete(cmd); + return true; +} + + +int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout){ + uint8_t msb[2] = {0,0}; + I2Cdev_readBytes(devAddr, regAddr, 2, msb, I2Cdev_readTimeout); + *data = (int16_t)((msb[0] << 8) | msb[1]); + return 0; +} \ No newline at end of file diff --git a/I2Cdev.h b/I2Cdev.h new file mode 100644 index 0000000..60dbd98 --- /dev/null +++ b/I2Cdev.h @@ -0,0 +1,46 @@ + + +#ifndef _I2CDEV_H_ +#define _I2CDEV_H_ + +#include +#include +#include + +#define I2C_SDA_PORT gpioPortA +#define I2C_SDA_PIN 18 +#define I2C_SDA_MODE gpioModeWiredAnd +#define I2C_SDA_DOUT 1 + +#define I2C_SCL_PORT gpioPortA +#define I2C_SCL_PIN 19 +#define I2C_SCL_MODE gpioModeWiredAnd +#define I2C_SCL_DOUT 1 + +#define I2CDEV_DEFAULT_READ_TIMEOUT 1000 + + // void I2Cdev_initialize(); + void I2Cdev_enable(bool isEnabled); + + int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout); + + int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout); + + int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout); + int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout); + int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout); + + + bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); + + bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); + + bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); + bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); + bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); + + void I2Cdev_SelectRegister(uint8_t devAddr, uint8_t regAddr); + + extern uint16_t I2Cdev_readTimeout; + +#endif /* _I2CDEV_H_ */ \ No newline at end of file diff --git a/Kconfig.projbuild b/Kconfig.projbuild new file mode 100644 index 0000000..7462ca1 --- /dev/null +++ b/Kconfig.projbuild @@ -0,0 +1,48 @@ +menu "Application Configuration" + + config GPIO_RANGE_MAX + int + default 33 if IDF_TARGET_ESP32 + default 46 if IDF_TARGET_ESP32S2 + default 48 if IDF_TARGET_ESP32S3 + default 18 if IDF_TARGET_ESP32C2 + default 19 if IDF_TARGET_ESP32C3 + default 30 if IDF_TARGET_ESP32C6 + + config GPIO_SCL + int "SCL GPIO number" + range 0 GPIO_RANGE_MAX + default 22 if IDF_TARGET_ESP32 + default 12 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3 + default 5 # C3 and others + help + GPIO number (IOxx) to I2C SCL. + + config GPIO_SDA + int "SDA GPIO number" + range 0 GPIO_RANGE_MAX + default 21 if IDF_TARGET_ESP32 + default 11 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3 + default 4 # C3 and others + help + GPIO number (IOxx) of SDA. + + config MAGX + int "Compass offset value X" + default 0 + help + Compass offset value X. + + config MAGY + int "Compass offset value Y" + default 0 + help + Compass offset value Y. + + config MAGZ + int "Compass offset value Z" + default 0 + help + Compass offset value Z. + +endmenu diff --git a/QMC5883L.c b/QMC5883L.c new file mode 100644 index 0000000..e42cb9d --- /dev/null +++ b/QMC5883L.c @@ -0,0 +1,146 @@ +#include "QMC5883L.h" + +void QMC5883L_Init(QMC5883L_st *s_qmc, uint8_t address) { + s_qmc->address_u8 = address; +} + +void QMC5883L_ResetCalibration(QMC5883L_st *s_qmc) { + uint8_t axis; + for (axis = 0; axis < 3; axis++) { + s_qmc->calibration_st.axes[axis].min = -32768; + s_qmc->calibration_st.axes[axis].max = 32767; + } +} + +void QMC5883L_recalcCalibration(QMC5883L_st *s_qmc) +{ + uint8_t axis; + long deltas[3]; + float delta; + + delta = 0; + + for (axis = 0; axis < 3; axis++) + { + s_qmc->biases[axis] = ((long)s_qmc->calibration_st.axes[axis].max + (long)s_qmc->calibration_st.axes[axis].min)/2; + deltas[axis] = (long)s_qmc->calibration_st.axes[axis].max - (long)s_qmc->calibration_st.axes[axis].min ; + delta += deltas[axis]; + } + + delta = delta / 3; + + for (axis = 0; axis < 3; axis++) + { + s_qmc->scales[axis] = delta / deltas[axis]; + } +} + +void QMC5883L_Begin(QMC5883L_st *s_qmc) { + QMC5883L_ResetCalibration(s_qmc); + QMC5883L_recalcCalibration(s_qmc); + I2Cdev_writeByte(s_qmc->address_u8, QMC5883L_SFR_CONTROL_2, QMC5883L_SBIT_SOFT_RST); + I2Cdev_writeByte(s_qmc->address_u8, QMC5883L_SFR_PERIOD, 0x01); +} + +void QMC5883L_SetMode(QMC5883L_st *s_qmc, uint8_t mode, uint8_t odr, uint8_t rng, uint8_t osr) { + uint8_t config = mode | odr | rng | osr; + I2Cdev_writeByte(s_qmc->address_u8, QMC5883L_SFR_CONTROL_1, config); +} + +void QMC5883L_SetConfig(QMC5883L_st *s_qmc) { + QMC5883L_SetMode(s_qmc, QMC5883L_SBIT_MODE_CONTINUOUS, QMC5883L_SBIT_ODR_100HZ, QMC5883L_SBIT_RNG_8G, QMC5883L_SBIT_OSR_512); + I2Cdev_writeByte(s_qmc->address_u8, QMC5883L_SFR_CONTROL_2, QMC5883L_SBIT_INT_ENB_DIS); +} + +void QMC5883L_SetCalibration(QMC5883L_st *s_qmc, const Calibration_st *s_calib) { + s_qmc->calibration_st = *s_calib; + QMC5883L_recalcCalibration(s_qmc); +} + +int16_t QMC5883L_rawDataAxis(QMC5883L_st *s_qmc, uint8_t axis) { + I2Cdev_readBytes(s_qmc->address_u8, QMC5883L_SFR_DataOutXLSB, 6, s_qmc->buffer_u8, I2Cdev_readTimeout); + int16_t data = (((int16_t)s_qmc->buffer_u8[axis * 2 + 1]) << 8) | s_qmc->buffer_u8[axis * 2]; + return data; +} + +int16_t QMC5883L_rawDataX(QMC5883L_st *s_qmc) +{ + return QMC5883L_rawDataAxis(s_qmc, 0); +} + +int16_t QMC5883L_rawDataY(QMC5883L_st *s_qmc) +{ + return QMC5883L_rawDataAxis(s_qmc, 1); +} + +int16_t QMC5883L_rawDataZ(QMC5883L_st *s_qmc) +{ + return QMC5883L_rawDataAxis(s_qmc, 2); +} + +RawDataAxes_st QMC5883L_RawDataAxes(QMC5883L_st *s_qmc) { + RawDataAxes_st s_rawDataAxes; + s_rawDataAxes.x = QMC5883L_rawDataAxis(s_qmc, 0); + s_rawDataAxes.y = QMC5883L_rawDataAxis(s_qmc, 1); + s_rawDataAxes.z = QMC5883L_rawDataAxis(s_qmc, 2); + return s_rawDataAxes; +} + +RawDataArray_st QMC5883L_rawDataArray(QMC5883L *s_qmc) +{ + RawDataArray_st s_rawDataArray; + + s_rawDataArray.axes[0] = QMC5883L_rawDataAxis(s_qmc, 0); + s_rawDataArray.axes[1] = QMC5883L_rawDataAxis(s_qmc, 1); + s_rawDataArray.axes[2] = QMC5883L_rawDataAxis(s_qmc, 2); + + return s_rawDataArray; +} + +float QMC5883L_calibratedDataAxis(QMC5883L_st *s_qmc, uint8_t axis) +{ + return s_qmc->scales[axis] * (QMC5883L_rawDataAxis(s_qmc, axis) - s_qmc->biases[axis]); +} + +float QMC5883L_calibratedDataX(QMC5883L_st *s_qmc) +{ + return QMC5883L_calibratedDataAxis(s_qmc, 0); +} + +float QMC5883L_calibratedDataY(QMC5883L_st *s_qmc) +{ + return QMC5883L_calibratedDataAxis(s_qmc, 1); +} + +float QMC5883L_calibratedDataZ(QMC5883L_st *s_qmc) +{ + return QMC5883L_calibratedDataAxis(s_qmc, 2); +} + +CalibratedDataAxes_st QMC5883L_calibratedDataAxes(QMC5883L_st *s_qmc) +{ + CalibratedDataAxes_st s_calibratedDataAxes; + + s_calibratedDataAxes.x = QMC5883L_calibratedDataAxis(s_qmc, 0); + s_calibratedDataAxes.y = QMC5883L_calibratedDataAxis(s_qmc, 1); + s_calibratedDataAxes.z = QMC5883L_calibratedDataAxis(s_qmc, 2); + + return s_calibratedDataAxes; +} + +CalibratedDataArray_st QMC5883L_calibratedDataArray(QMC5883L_st *s_qmc) +{ + CalibratedDataArray_st s_calibratedDataArray; + + s_calibratedDataArray.axes[0] = QMC5883L_calibratedDataAxis(s_qmc, 0); + s_calibratedDataArray.axes[1] = QMC5883L_calibratedDataAxis(s_qmc, 1); + s_calibratedDataArray.axes[2] = QMC5883L_calibratedDataAxis(s_qmc, 2); + + return s_calibratedDataArray; +} +float QMC5883L_AzimuthZUp(QMC5883L_st *s_qmc) { + float alpha; + alpha = atan2f(QMC5883L_calibratedDataY(s_qmc), QMC5883L_calibratedDataX(s_qmc)); // atan2(y, x), not math. arctan2(x, y)! + return (alpha < 0 ? alpha + 2 * 3.14159265f : alpha) * 180.0f / 3.14159265f; +} + diff --git a/QMC5883L.h b/QMC5883L.h new file mode 100644 index 0000000..4f5ecf0 --- /dev/null +++ b/QMC5883L.h @@ -0,0 +1,245 @@ +#ifndef _QMC5883L_H_ +#define _QMC5883L_H_ + +#include +#include "I2Cdev.h" +#include + +#define QMC5883L_ADDRESS_u8 0x0D + +// Output Data Registers +#define QMC5883L_SFR_DataOutXLSB 0x00 /* store the measurement data from each axis magnetic sensor in continuous-measurement mode*/ +#define QMC5883L_SFR_DataOutXMSB 0x01 +#define QMC5883L_SFR_DataOutYLSB 0x02 +#define QMC5883L_SFR_DataOutYMSB 0x03 +#define QMC5883L_SFR_DataOutZLSB 0x04 +#define QMC5883L_SFR_DataOutZMSB 0x05 + + +// Control Registers +#define QMC5883L_SFR_CONTROL_1 0x09 /*sets the operational modes (MODE). output data update rate (ODR), magnetic field measurement range or sensitivity of the sensors (RNG) and over sampling rate (OSR)*/ +#define QMC5883L_SFR_CONTROL_2 0x0A /*sets the Interrupt enabling by INT_ENB, Pointer roll-over function by ROL_PNT, Soft Reset by SOFT_RST*/ +#define QMC5883L_SFR_PERIOD 0x0B /*SET/RESET Period is controlled by FBR [7:0]*/ + +// config for CONTROL REGISTER 1 +#define QMC5883L_SBIT_MODE_CONTINUOUS 0b00000001 /*set Continuous measurements mode for sensor*/ + +#define QMC5883L_SBIT_ODR_100HZ 0b00001000 /*data update frequency of Output data rate(ODR) is 100HZ*/ + +#define QMC5883L_SBIT_RNG_8G 0b00010000 /* The full scale field range of the sensor is +- 8 Gauss*/ + +#define QMC5883L_SBIT_OSR_512 0b00000000 /*Over sampling rate (OSR) is 512*/ + +//// config for CONTROL REGISTER 2 +#define QMC5883L_SBIT_INT_ENB_DIS 0b00000001 /*disable interrupt PIN*/ +#define QMC5883L_SBIT_SOFT_RST 0b10000000 /*soft reset, restore default value of all registers. */ + +//Structure stores calibration data for each axis (X, Y, Z) +typedef struct { + struct { + int min; + int max; + } axes[3]; +} Calibration_st; + +// Structure stores raw sensor readings for each axis +typedef struct { + int x; + int y; + int z; +} RawDataAxes_st; + +// Structure store raw sensor data as an array +typedef struct + { + int axes[3]; + }RawDataArray_st; + +// Structure stores calibrated (scaled) sensor readings +typedef struct { + float x; + float y; + float z; +} CalibratedDataAxes_st; + +// Structure stores calibrated sensor data as an array. + typedef struct + { + float axes[3]; + }CalibratedDataArray_st; + +// Structure stores information needed to communicate with the QMC5883L sensor +typedef struct { + uint8_t address_u8; //I2C address of the sensor + uint8_t buffer_u8[6]; //Buffer to store sensor readings + Calibration_st s_calibration; //Stores calibration data (min/max values). + long biases[3]; //Bias offsets for X, Y, Z. + float scales[3]; //Scaling factors for converting raw values. +} QMC5883L_st; + +// Function declarations + +/** + * @brief Initializes the QMC5883L sensor with a given I2C address + * + * @param s_qmc Structure of QMC5883L + * @param address I2C address of QMC5883L + */ +void QMC5883L_Init(QMC5883L_st *s_qmc, uint8_t address); + +/** + * @brief Resets the calibration data (sets min/max values to defaults) + * + * @param s_qmc Structure of QMC5883L + */ +void QMC5883L_ResetCalibration(QMC5883L_st *s_qmc); + +/** + * @brief Recalculates calibration parameters based on new sensor data. + * + * @param s_qmc Structure of QMC5883L + */ +void QMC5883L_recalcCalibration(QMC5883L_st *s_qmc); + +/** + * @brief Prepares the QMC5883L for operation after initialization + * + * @param s_qmc Structure of QMC5883L + */ +void QMC5883L_Begin(QMC5883L_st *s_qmc); + +/** + * @brief Configures the operating mode + * + * @param s_qmc Structure of QMC5883L + * @param mode Operating mode + * @param odr Output data rate + * @param rng Range setting + * @param osr Oversampling ratio + */ +void QMC5883L_SetMode(QMC5883L_st *s_qmc, uint8_t mode, uint8_t odr, uint8_t rng, uint8_t osr); + +/** + * @brief Applies configuration settings to the sensor + * + * @param s_qmc Structure of QMC5883L + */ +void QMC5883L_SetConfig(QMC5883L_st *s_qmc); + +/** + * @brief Manually set calibration data + * + * @param s_qmc Structure of QMC5883L + * @param s_calib Calibration Structure + */ +void QMC5883L_SetCalibration(QMC5883L_st *s_qmc, const Calibration_st *s_calib); + +/** + * @brief Reads the raw value of a specific axis (X, Y, or Z) + * + * @param s_qmc Structure of QMC5883L + * @param axis axis index: X=0, Y=1, Z=2 + * @return raw value of a specific axis + */ +int16_t QMC5883L_rawDataAxis(QMC5883L_st *s_qmc, uint8_t axis); + +/** + * @brief Read raw value of X axis + * + * @param s_qmc Structure of QMC5883L + * @return raw value of X axis + */ +int16_t QMC5883L_rawDataX(QMC5883L_st *s_qmc); + +/** + * @brief Read raw value of Y axis + * + * @param s_qmc Structure of QMC5883L + * @return raw value of Y axis + */ +int16_t QMC5883L_rawDataY(QMC5883L_st *s_qmc); + +/** + * @brief Read raw value of Z axis + * + * @param s_qmc Structure of QMC5883L + * @return raw value of X axis + */ +int16_t QMC5883L_rawDataZ(QMC5883L_st *s_qmc); + +/** + * @brief Returns raw X, Y, Z values in a struct + * + * @param s_qmc Structure of QMC5883L + * @return RawDataAxes_st raw X, Y, Z values + */ +RawDataAxes_st QMC5883L_RawDataAxes(QMC5883L_st *s_qmc); + +/** + * @brief Returns raw X, Y, Z values in an array format + * + * @param s_qmc Structure of QMC5883L + * @return RawDataArray_st raw X, Y, Z values + */ +RawDataArray_st QMC5883L_rawDataArray(QMC5883L_st *s_qmc); + +/** + * @brief Get calibrated data for a specific axis + * + * @param s_qmc Structure of QMC5883L + * @param axis axis index: X=0, Y=1, Z=2 + * @return float calibrated data for a specific axis + */ +float QMC5883L_calibratedDataAxis(QMC5883L_st *s_qmc, uint8_t axis); + +/** + * @brief Get calibrated data for X axis + * + * @param s_qmc Structure of QMC5883L + * @return float calibrated data for X axis + */ +float QMC5883L_calibratedDataX(QMC5883L_st *s_qmc); + +/** + * @brief Get calibrated data for Y axis + * + * @param s_qmc Structure of QMC5883L + * @return float calibrated data for Y axis + */ +float QMC5883L_calibratedDataY(QMC5883L_st *s_qmc); + +/** + * @brief Get calibrated data for Z axis + * + * @param s_qmc Structure of QMC5883L + * @return float calibrated data for Z axis + */ +float QMC5883L_calibratedDataZ(QMC5883L_st *s_qmc); + +/** + * @brief Returns calibrated data as a struct + * + * @param s_qmc Structure of QMC5883L + * @return CalibratedDataAxes_st calibrated data + */ +CalibratedDataAxes_st QMC5883L_calibratedDataAxes(QMC5883L_st *s_qmc); + +/** + * @brief Returns calibrated data as an array + * + * @param s_qmc Structure of QMC5883L + * @return CalibratedDataArray_st calibrated data + */ +CalibratedDataArray_st QMC5883L_calibratedDataArray(QMC5883L_st *s_qmc); + +/** + * @brief Calculate the azimuth angle + * + * @param s_qmc Structure of QMC5883L + * @return float azimuth angle + */ +float QMC5883L_AzimuthZUp(QMC5883L_st *s_qmc); + + +#endif + diff --git a/component.mk b/component.mk new file mode 100644 index 0000000..bad5d1a --- /dev/null +++ b/component.mk @@ -0,0 +1,10 @@ +# +# Main component makefile. +# +# This Makefile can be left empty. By default, it will take the sources in the +# src/ directory, compile them and link them into lib(subdirectory_name).a +# in the build directory. This behaviour is entirely configurable, +# please read the ESP-IDF documents if you need to do this. +# +COMPONENT_SRCDIRS := . +COMPONENT_ADD_INCLUDEDIRS := . diff --git a/idf_component.yml b/idf_component.yml new file mode 100644 index 0000000..dd31aa4 --- /dev/null +++ b/idf_component.yml @@ -0,0 +1,6 @@ +## IDF Component Manager Manifest File +dependencies: + espressif/mdns: + version: "^1.0.3" + rules: + - if: "idf_version >=5.0" diff --git a/kalman.c b/kalman.c new file mode 100644 index 0000000..fd88239 --- /dev/null +++ b/kalman.c @@ -0,0 +1,84 @@ +#include "Kalman.h" + +void Kalman_Init(Kalman_st *s_kalman) { + /* Initialize variables */ + kalman->Q_angle = 0.001f; + kalman->Q_bias = 0.003f; + kalman->R_measure = 0.03f; + + kalman->angle = 0.0f; + kalman->bias = 0.0f; + kalman->rate = 0.0f; + + kalman->P[0][0] = 0.0f; + kalman->P[0][1] = 0.0f; + kalman->P[1][0] = 0.0f; + kalman->P[1][1] = 0.0f; +} + +float Kalman_GetAngle(Kalman_st *s_kalman, float newAngle, float newRate, float dt) { + // Step 1: Predict + s_kalman->rate = newRate - s_kalman->bias; + s_kalman->angle += dt * s_kalman->rate; + + // Update estimation error covariance + s_kalman->P[0][0] += dt * (dt * s_kalman->P[1][1] - s_kalman->P[0][1] - s_kalman->P[1][0] + s_kalman->Q_angle); + s_kalman->P[0][1] -= dt * s_kalman->P[1][1]; + s_kalman->P[1][0] -= dt * s_kalman->P[1][1]; + s_kalman->P[1][1] += s_kalman->Q_bias * dt; + + // Step 2: Update + float S = s_kalman->P[0][0] + s_kalman->R_measure; // Estimate error + float K[2]; // Kalman gain + K[0] = s_kalman->P[0][0] / S; + K[1] = s_kalman->P[1][0] / S; + + float y = newAngle - s_kalman->angle; // Angle difference + s_kalman->angle += K[0] * y; + s_kalman->bias += K[1] * y; + + // Update error covariance + float P00_temp = s_kalman->P[0][0]; + float P01_temp = s_kalman->P[0][1]; + + s_kalman->P[0][0] -= K[0] * P00_temp; + s_kalman->P[0][1] -= K[0] * P01_temp; + s_kalman->P[1][0] -= K[1] * P00_temp; + s_kalman->P[1][1] -= K[1] * P01_temp; + + return s_kalman->angle; +} + +void Kalman_SetAngle(Kalman_st *s_kalman, float angle) { + s_kalman->angle = angle; +} + +float Kalman_GetRate(Kalman_st *s_kalman) { + return s_kalman->rate; +} + +void Kalman_SetQangle(Kalman_st *s_kalman, float Q_angle) { + s_kalman->Q_angle = Q_angle; +} + +void Kalman_SetQbias(Kalman_st *s_kalman, float Q_bias) { + s_kalman->Q_bias = Q_bias; +} + +void Kalman_SetRmeasure(Kalman_st *s_kalman, float R_measure) { + s_kalman->R_measure = R_measure; +} + +float Kalman_GetQangle(Kalman_st *s_kalman) { + return s_kalman->Q_angle; +} + + +float Kalman_GetQbias(Kalman_st *s_kalman) { + return s_kalman->Q_bias; +} + +float Kalman_GetRmeasure(Kalman_st *s_kalman) { + return s_kalman->R_measure; +} + diff --git a/kalman.h b/kalman.h new file mode 100644 index 0000000..ae85254 --- /dev/null +++ b/kalman.h @@ -0,0 +1,101 @@ +#ifndef _KALMAN_H_ +#define _KALMAN_H_ + +#include + +typedef struct { + /* Kalman filter variables */ + float Q_angle; // Process noise variance for the accelerometer + float Q_bias; // Process noise variance for the gyro bias + float R_measure; // Measurement noise variance + + float angle; // The angle calculated by the Kalman filter + float bias; // The gyro bias calculated by the Kalman filter + float rate; // Unbiased rate calculated from the rate and the bias + + float P[2][2]; // Error covariance matrix (2x2 matrix) +} Kalman_st; + +/** + * @brief Initialize the Kalman filter + * + * @param s_kalman Structure of Kalman filter + */ +void Kalman_Init(Kalman_st *s_kalman); + +/** + * @brief Get the current angle using Kalman filter + * + * @param s_kalman Structure of Kalman filter + * @param newAngle Measured angle from the accelerometer (in degrees or radians). + * @param newRate Angular rate from the gyroscope (in degrees/second or radians/second). + * @param dt Time interval between measurements (in seconds). + * @return The filtered angle (more stable and accurate than raw sensor data). + */ +float Kalman_GetAngle(Kalman_st *s_kalman, float newAngle, float newRate, float dt); + +/** + * @brief Set the initial angle + * + * @param s_kalman Structure of Kalman filter + * @param angle The initial angle value + */ +void Kalman_SetAngle(Kalman_st *s_kalman, float angle); + +/** + * @brief Get the unbiased rate + * + * @param s_kalman Structure of Kalman filter + * @return The unbiased angular rate. + */ +float Kalman_GetRate(Kalman_st *s_kalman); + +/** + * @brief Set the process noise variance for the accelerometer + * + * @param s_kalman Structure of Kalman filter + * @param Q_angle The process noise variance for the accelerometer (a smaller value assumes less system variability). + */ +void Kalman_SetQangle(Kalman_st *s_kalman, float Q_angle); + +/** + * @brief Set the process noise variance for the gyro bias + * + * @param s_kalman Structure of Kalman filter + * @param Q_bias The noise variance for the gyro bias (smaller values mean higher precision). + */ +void Kalman_SetQbias(Kalman_st *s_kalman, float Q_bias); + +/** + * @brief Set the measurement noise variance + * + * @param s_kalman Structure of Kalman filter + * @param R_measure The noise variance for the measurements (smaller values mean more trusted sensor data). + */ +void Kalman_SetRmeasure(Kalman_st *s_kalman, float R_measure); + +/** + * @brief Get the process noise variance for the accelerometer + * + * @param s_kalman Structure of Kalman filter + * @return The Q_angle value. + */ +float Kalman_GetQangle(Kalman_st *s_kalman); + +/** + * @brief Get the process noise variance for the gyro bias + * + * @param s_kalman Structure of Kalman filter + * @return The Q_bias value + */ +float Kalman_GetQbias(Kalman_st *s_kalman); + +/** + * @brief Get the measurement noise variance + * + * @param s_kalman Structure of Kalman filter + * @return The R_measure value + */ +float Kalman_GetRmeasure(Kalman_st *s_kalman); + +#endif diff --git a/main.c b/main.c new file mode 100644 index 0000000..f71fcc0 --- /dev/null +++ b/main.c @@ -0,0 +1,134 @@ +#include +#include "esp_log.h" +#include "driver/i2c.h" +#include "QMC5883L.h" +#include "mpu6050.h" +#include "kalman.h" +#include "freertos/FreeRTOS.h" + + +SemaphoreHandle_t mutex; // Mutex d? b?o v? truy c?p vào bi?n gz +volatile double shared_gz = 0; // Bi?n chia s? gi?a hai task + + +void task_mpu6050(void *pvParameters) { + MPU6050_st s_mpu; + MPU6050_init(&s_mpu, MPU6050_DefaultAddress_u8); + uint8_t devid_u8 = MPU6050_getDeviceID(&s_mpu); + ESP_LOGI("MPU6050_task", "devid=0x%x", devid_u8); + uint8_t rate_u8 = MPU6050_getRate(&s_mpu); + ESP_LOGI("MPU6050_task", "getRate()=%d", rate_u8); + if (rate_u8 != 0) MPU6050_setRate(&s_mpu, 0); + uint8_t ExternalFrameSync_u8 = MPU6050_getExternalFrameSync(&s_mpu); + ESP_LOGI("MPU6050_task", "getExternalFrameSync()=%d", ExternalFrameSync_u8); + if (ExternalFrameSync_u8 != 0) MPU6050_setExternalFrameSync(&s_mpu, 0);; + uint8_t DLPFMode_u8 = MPU6050_getDLPFMode(&s_mpu); + ESP_LOGI("MPU6050_task", "getDLPFMode()=%d", DLPFMode_u8); + if (DLPFMode_u8 != 6) MPU6050_setDLPFMode(&s_mpu, 6); + uint8_t FullScaleAccelRange_u8 = MPU6050_getFullScaleGyroRange(&s_mpu); + ESP_LOGI("MPU6050_task", "getFullScaleAccelRange()=%d", FullScaleAccelRange_u8); + if (FullScaleAccelRange_u8 != 0) MPU6050_setFullScaleGyroRange(&s_mpu, 0); + float accel_sensitivity = 16384.0; // g + uint8_t FullScaleGyroRange_u8 = MPU6050_getFullScaleAccelRange(&s_mpu); + ESP_LOGI("MPU6050_task", "getFullScaleGyroRange()=%d", FullScaleGyroRange_u8); + if (FullScaleGyroRange_u8 != 0) MPU6050_setFullScaleAccelRange(&s_mpu, 0); + float gyro_sensitivity = 131.0; // Deg/Sec + MPU6050_setI2CBypassEnabled(&s_mpu, true); + double ax, ay, az, gx, gy, gz; + float Elevation; + float prevElevation = 0; // Luu tr? giá tr? Elevation tru?c dó + //kalman filter + Kalman_st s_kalman; + Kalman_Init(&s_kalman); + float dt = 0.1; // Assuming 100 ms delay between readings + + while (1) { + MPU6050_getMotion6(&s_mpu, &ax, &ay, &az, &gx, &gy, &gz, accel_sensitivity, gyro_sensitivity); + // Khóa mutex và c?p nh?t shared_gz + if (xSemaphoreTake(mutex, portMAX_DELAY)) { + shared_gz = gz; // C?p nh?t giá tr? gz chia s? + xSemaphoreGive(mutex); // M? khóa mutex + } + Elevation = MPU6050_getElevation(&s_mpu, ax, ay, az); + float smoothedElevation = Kalman_GetAngle(&s_kalman, Elevation, gy, dt); + printf(" Elevation = %f\n", smoothedElevation); + // Ki?m tra thay d?i góc + if (fabs(Elevation - prevElevation) >= 5.0) { + printf(" Elevation = %f\n", Elevation); + prevElevation = Elevation; // C?p nh?t giá tr? tru?c dó + } + vTaskDelay(100 / portTICK_PERIOD_MS); + } +} + +void task_qmc5883l(void *pvParameters) +{ + QMC5883L_st s_qmc; + + QMC5883L_Init(&s_qmc, QMC5883L_ADDRESS_u8); + QMC5883L_Begin(&s_qmc); + QMC5883L_SetConfig(&s_qmc); + Calibration_st s_calibration = { + .axes = { + {-1008, 738}, // Tr?c X + {26, 2273}, // Tr?c Y + {1363, 2230} // Tr?c Z + } + }; + QMC5883L_SetCalibration(&s_qmc, &s_calibration); + + float Azimuth; + + //kalman filter + Kalman_st s_kalman; + Kalman_Init(&s_kalman); + float dt = 0.1; // Assuming 100 ms delay between readings + float prevAzimuth = 0; // Luu tr? giá tr? Azimuth tru?c dó + + while (1) { + Azimuth = QMC5883L_AzimuthZUp(&qmc); + double gz = 0; + if (xSemaphoreTake(mutex, portMAX_DELAY)) { + gz = shared_gz; // Ð?c giá tr? gz du?c chia s? + xSemaphoreGive(mutex); // M? khóa mutex + } + float smoothedAzimuth = Kalman_GetAngle(&s_kalman, Azimuth, gz, dt); + printf(" Azimuth = %f\n", smoothedAzimuth); + // Ki?m tra thay d?i góc + if (fabs(Azimuth - prevAzimuth) >= 5.0) { + printf(" Azimuth = %f\n", Azimuth); + prevAzimuth = Azimuth; // C?p nh?t giá tr? tru?c dó + } + + vTaskDelay(100 / portTICK_PERIOD_MS); + } +} + +void start_i2c(void) { + i2c_config_t conf; + conf.mode = I2C_MODE_MASTER; + conf.sda_io_num = (gpio_num_t)CONFIG_GPIO_SDA; + conf.scl_io_num = (gpio_num_t)CONFIG_GPIO_SCL; + conf.sda_pullup_en = GPIO_PULLUP_ENABLE; + conf.scl_pullup_en = GPIO_PULLUP_ENABLE; + conf.master.clk_speed = 400000; + conf.clk_flags = 0; + ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf)); + ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0)); +} + +void app_main(void) +{ + start_i2c(); + // T?o mutex + mutex = xSemaphoreCreateMutex(); + if (mutex == NULL) { + ESP_LOGE("app_main", "Failed to create mutex!"); + return; + } + xTaskCreate(task_mpu6050, "MPU6050_task", 1024*8, NULL, 1, NULL); + xTaskCreate(task_qmc5883l, "QMC5883L_task", 1024*8, NULL, 1, NULL); + + vTaskDelay(100); +} + diff --git a/mpu6050.c b/mpu6050.c new file mode 100644 index 0000000..223c96f --- /dev/null +++ b/mpu6050.c @@ -0,0 +1,126 @@ +#include "mpu6050.h" +#include +#include + +#define I2C_NUM I2C_NUM_0 + +#define RAD_TO_DEG (180.0/M_PI) +#define DEG_TO_RAD 0.0174533 + +void MPU6050_init(MPU6050_st *s_mpu, uint8_t address) { + s_mpu->address_u8 = address; + memset(s_mpu->buffer_u8, 0, sizeof(s_mpu->buffer_u8)); + MPU6050_setClockSource(s_mpu, MPU6050_CLKSEL_1_u8); + MPU6050_setFullScaleGyroRange(s_mpu, MPU6050_FS_SEL_250_u8); + MPU6050_setFullScaleAccelRange(s_mpu, MPU6050_AFS_SEL_2); + MPU6050_setSleepEnabled(s_mpu, false); +} + +uint8_t MPU6050_getRate(MPU6050_st *s_mpu) { + I2Cdev_readByte(s_mpu->address_u8, MPU6050_SFR_SMPLRT_DIV , s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setRate(MPU6050_st *s_mpu, uint8_t rate) { + I2Cdev_writeByte(s_mpu->address_u8, MPU6050_SFR_SMPLRT_DIV , rate); +} + +uint8_t MPU6050_getExternalFrameSync(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_CONFIG, MPU6050_SBIT_EXT_SYNC_SET, MPU6050_EXT_SYNC_SET_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setExternalFrameSync(MPU6050_st *s_mpu, uint8_t sync) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_CONFIG, MPU6050_SBIT_EXT_SYNC_SET, MPU6050_EXT_SYNC_SET_LENGTH, sync); +} + +uint8_t MPU6050_getDLPFMode(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_CONFIG, MPU6050_SBIT_DLPF_CFG, MPU6050_DLPF_CFG_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setDLPFMode(MPU6050_st *s_mpu, uint8_t mode) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_CONFIG, MPU6050_SBIT_DLPF_CFG, MPU6050_CFG_DLPF_CFG_LENGTH, mode); +} + +uint8_t MPU6050_getFullScaleGyroRange(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_GYRO_CONFIG, MPU6050_SBIT_FS_SEL, MPU6050_FS_SEL_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setFullScaleGyroRange(MPU6050_st *s_mpu, uint8_t range) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_GYRO_CONFIG, MPU6050_SBIT_FS_SEL, MPU6050_FS_SEL_LENGTH, range); +} + +uint8_t MPU6050_getFullScaleAccelRange(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_ACCEL_CONFIG, MPU6050_SBIT_AFS_SEL, MPU6050_AFS_SEL_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setFullScaleAccelRange(MPU6050_st *s_mpu, uint8_t range) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_ACCEL_CONFIG, MPU6050_SBIT_AFS_SEL, MPU6050_AFS_SEL_LENGTH, range); +} + +bool MPU6050_getI2CBypassEnabled(MPU6050_st *s_mpu) { + I2Cdev_readBit(s_mpu->address_u8, MPU6050_SFR_INT_PIN_CFG, MPU6050_SBIT_I2C_BYPASS_EN, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setI2CBypassEnabled(MPU6050_st *s_mpu, bool enabled) { + I2Cdev_writeBit(s_mpu->address_u8, MPU6050_SFR_INT_PIN_CFG, MPU6050_SBIT_I2C_BYPASS_EN, enabled); +} + +bool MPU6050_getSleepEnabled(MPU6050_st *s_mpu) { + I2Cdev_readBit(s_mpu->address_u8, MPU6050_SFR_PWR_MGMT_1, MPU6050_SBIT_SLEEP, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setSleepEnabled(MPU6050_st *s_mpu, bool enabled) { + I2Cdev_writeBit(s_mpu->address_u8, MPU6050_SFR_PWR_MGMT_1, MPU6050_SBIT_SLEEP, enabled); +} + +uint8_t MPU6050_getClockSource(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_PWR_MGMT_1, MPU6050_SBIT_CLKSEL, MPU6050_CLKSEL_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setClockSource(MPU6050_st *s_mpu, uint8_t source) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_PWR_MGMT_1, MPU6050_SBIT_CLKSEL, MPU6050_CLKSEL_LENGTH, source); +} + +uint8_t MPU6050_getDeviceID(MPU6050_st *s_mpu) { + I2Cdev_readBits(s_mpu->address_u8, MPU6050_SFR_WHO_AM_I, MPU6050_SBIT_WHO_AM_I, MPU6050_WHO_AM_I_LENGTH, s_mpu->buffer_u8, I2Cdev_readTimeout); + return s_mpu->buffer_u8[0]; +} + +void MPU6050_setDeviceID(MPU6050_st *s_mpu, uint8_t id) { + I2Cdev_writeBits(s_mpu->address_u8, MPU6050_SFR_WHO_AM_I, MPU6050_SBIT_WHO_AM_I, MPU6050_WHO_AM_I_LENGTH, id); +} + +void MPU6050_getRawMotion6(MPU6050_st *s_mpu, int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + I2Cdev_readBytes(s_mpu->address_u8, MPU6050_SFR_ACCEL_XOUT_H, 14, s_mpu->buffer_u8, I2Cdev_readTimeout); + *ax = (((int16_t)s_mpu->buffer_u8[0]) << 8) | s_mpu->buffer_u8[1]; + *ay = (((int16_t)s_mpu->buffer_u8[2]) << 8) | s_mpu->buffer_u8[3]; + *az = (((int16_t)s_mpu->buffer_u8[4]) << 8) | s_mpu->buffer_u8[5]; + *gx = (((int16_t)s_mpu->buffer_u8[8]) << 8) | s_mpu->buffer_u8[9]; + *gy = (((int16_t)s_mpu->buffer_u8[10]) << 8) | s_mpu->buffer_u8[11]; + *gz = (((int16_t)s_mpu->buffer_u8[12]) << 8) | s_mpu->buffer_u8[13]; +} + +void MPU6050_getMotion6(MPU6050_st *s_mpu, double *_ax, double *_ay, double *_az, double *_gx, double *_gy, double *_gz, float accel_sensitivity, float gyro_sensitivity) +{ + int16_t ax,ay,az; + int16_t gx,gy,gz; + MPU6050_getRawMotion6(s_mpu, &ax, &ay, &az, &gx, &gy, &gz); + + *_ax = (double)ax / accel_sensitivity; + *_ay = (double)ay / accel_sensitivity; + *_az = (double)az / accel_sensitivity; + *_gx = (double)gx / gyro_sensitivity; + *_gy = (double)gy / gyro_sensitivity; + *_gz = (double)gz / gyro_sensitivity; +} + +float MPU6050_getElevation(MPU6050_st *s_mpu, double accX, double accY, double accZ) { + return atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; +} diff --git a/mpu6050.h b/mpu6050.h new file mode 100644 index 0000000..7f59345 --- /dev/null +++ b/mpu6050.h @@ -0,0 +1,260 @@ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +#include "I2Cdev.h" + +#define MPU6050_DefaultAddress_u8 0x68 /* Default I2C address for MPU6050 */ + + //registors +#define MPU6050_SFR_SMPLRT_DIV 0x19 /* generate MPU6050 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */ + +#define MPU6050_SFR_CONFIG 0x1A /* configures FSYNC pin sampling and DLPF setting for both the gyroscopes and accelerometers*/ + +#define MPU6050_SFR_GYRO_CONFIG 0x1B /* trigger gyroscope self-test and configure the gyroscopes’ full scale range*/ +#define MPU6050_SFR_ACCEL_CONFIG 0x1C /* trigger accelerometer self test and configure the accelerometer full scale range, also configures the Digital High Pass Filter (DHPF)*/ + +#define MPU6050_SFR_INT_PIN_CFG 0x37 /*configures the behavior of the interrupt signals at the INT pins, enable the FSYNC Pin to be used as an interrupt to the host application processor, enable Bypass Mode on the I2C Master*/ + +#define MPU6050_SFR_ACCEL_XOUT_H 0x3B /* ACCEL_XOUT[15:8]: Stores the most recent X axis accelerometer measurement*/ +#define MPU6050_SFR_ACCEL_XOUT_L 0x3C /* ACCEL_XOUT[7:0]: Stores the most recent X axis accelerometer measurement*/ +#define MPU6050_SFR_ACCEL_yOUT_H 0x3D /* ACCEL_YOUT[15:8]: Stores the most recent Y axis accelerometer measurement*/ +#define MPU6050_SFR_ACCEL_YOUT_L 0x3E /* ACCEL_YOUT[7:0]: Stores the most recent Y axis accelerometer measurement*/ +#define MPU6050_SFR_ACCEL_ZOUT_H 0x3F /* ACCEL_ZOUT[15:8]: Stores the most recent Z axis accelerometer measurement*/ +#define MPU6050_SFR_ACCEL_ZOUT_L 0x40 /* ACCEL_ZOUT[7:0]: Stores the most recent Z axis accelerometer measurement*/ + +#define MPU6050_SFR_PWR_MGMT_1 0x6B /*configure the power mode and clock source, provides a bit for resetting the entire device and a bit for disabling the temperature sensor*/ +#define MPU6050_SFR_WHO_AM_I 0x75 /*verify the identity of the device*/ + +//config for CONFIG register +#define MPU6050_SBIT_EXT_SYNC_SET 5 /* first bit of EXT_SYNC_SET[2:0] at 5th position: sample an external signal connected to the FSYNC pin*/ +#define MPU6050_EXT_SYNC_SET_LENGTH 3 /* length of EXT_SYNC_SET[2:0] */ +#define MPU6050_SBIT_DLPF_CFG 2 /* first bit of DLPF_CFG[2:0] at 2nd position: config the DLPF(Digital High Pass Filter) */ +#define MPU6050_DLPF_CFG_LENGTH 3 /* length of DLPF_CFG[2:0] */ + +//config for GYRO_CONFIG register +#define MPU6050_SBIT_FS_SEL 4 /*first bit of FS_SEL[1:0] at 4th position: selects the full scale range of the gyroscope outputs*/ +#define MPU6050_FS_SEL_LENGTH 2 /*length of FS_SEL[1:0]*/ + +#define MPU6050_FS_SEL_250_u8 0x00 /* full scale range of the gyroscope outputs is +-250 degrees per second*/ + +//config for ACCEL_CONFIG register +#define MPU6050_SBIT_AFS_SEL 4 /*first bit of FS_SEL[1:0] at 4th position: selects the full scale range of the accelerometer outputs*/ +#define MPU6050_AFS_SEL_LENGTH 2 /*length of AFS_SEL[1:0]*/ + +#define MPU6050_AFS_SEL_2 0x00 /* full scale range of the accelerometer outputs is +-2g*/ + +//config for INT_PIN_CFG registor +#define MPU6050_SBIT_I2C_BYPASS_EN 1 /*I2C_BYPASS_EN bit at 1st position: the host application processor will be able to directly access the auxiliary I2C bus*/ + +//config for PWR_MGMT_1 registor +#define MPU6050_CLKSEL_1_u8 0x01 /*CLKSEL[2:0] = 1: PLL with X axis gyroscope reference*/ +#define MPU6050_SBIT_CLKSEL 2 /*first bit of CLKSEL[2:0] at 2nd position: Specifies the clock source of the device*/ +#define MPU6050_CLKSEL_LENGTH 3 /*length of CLKSEL[2:0]*/ + +#define MPU6050_SBIT_SLEEP 6 /*first bit of SLEEP[0] at 6th position: When set to 1, this bit puts the MPU-60X0 into sleep mode*/ +//config for WHO_AM_I registor +#define MPU6050_SBIT_WHO_AM_I 6 /* first bit of WHO_AM_I[6:1] at 6th position: the upper 6 bits of the MPU6050’s 7-bit I2C address*/ +#define MPU6050_WHO_AM_I_LENGTH 6 /* length of WHO_AM_I[6:1]*/ + + +typedef struct { + uint8_t address_u8; + uint8_t buffer_u8[14]; +} MPU6050_st + +/** + * @brief This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + * + * @param s_mpu structure of MPU6050 + * @param address I2C address of MPU6050 + */ +void MPU6050_init(MPU6050_st *s_mpu, uint8_t address); + +// SMPLRT_DIV register + +/** + * @brief Get gyroscope output rate divider. The Sample Rate is generated by dividing + * the gyroscope output rate by SMPLRT_DIV: + * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + * + * @param s_mpu structure of MPU6050 + * @return Current sample rate + */ +uint8_t MPU6050_getRate(MPU6050_st *s_mpu); + +/** + * @brief Set gyroscope sample rate divider + * + * @param s_mpu structure of MPU6050 + * @param rate New sample rate divider + */ +void MPU6050_setRate(MPU6050_st *s_mpu, uint8_t rate); + +// CONFIG register + +/** + * @brief Get the external Frame Synchronization (FSYNC) configuration + * + * @param s_mpu structure of MPU6050 + * @return FSYNC configuration value + */ +uint8_t MPU6050_getExternalFrameSync(MPU6050_st *s_mpu); + +/** + * @brief Set external FSYNC configuration + * + * @param s_mpu structure of MPU6050 + * @param sync New FSYNC configuration value + */ +void MPU6050_setExternalFrameSync(MPU6050_st *s_mpu, uint8_t sync); + +/** + * @brief Get digital low-pass filter (DLPF) configuration + * + * @param s_mpu structure of MPU6050 + * @return DLFP configuration + */ +uint8_t MPU6050_getDLPFMode(MPU6050_st *s_mpu); + +/** + * @brief Set digital low-pass filter configuration + * + * @param s_mpu structure of MPU6050 + * @param mode New DLFP configuration setting + */ +void MPU6050_setDLPFMode(MPU6050_st *s_mpu, uint8_t mode); + +// GYRO_CONFIG register + +/** + * @brief Get full-scale gyroscope range (+-250 degrees per second) + * + * @param s_mpu structure of MPU6050 + * @return Current full-scale gyroscope range setting + */ +uint8_t MPU6050_getFullScaleGyroRange(MPU6050_st *s_mpu); + +/** + * @brief Set full-scale gyroscope range + * + * @param s_mpu structure of MPU6050 + * @param range New full-scale gyroscope range value + */ +void MPU6050_setFullScaleGyroRange(MPU6050_st *s_mpu, uint8_t range); + +// ACCEL_CONFIG register + +/** + * @brief Get full-scale accelerometer range + * + * @param s_mpu structure of MPU6050 + * @return Current full-scale accelerometer range setting + */ +uint8_t MPU6050_getFullScaleAccelRange(MPU6050_st *s_mpu); + +/** + * @brief Set full-scale accelerometer range ( +-2g) + * + * @param s_mpu structure of MPU6050 + * @param range New full-scale accelerometer range setting + */ +void MPU6050_setFullScaleAccelRange(MPU6050_st *s_mpu, uint8_t range); + +/** + * @brief Get I2C bypass enabled status + * + * @param s_mpu structure of MPU6050 + * @return true: enabled I2C bypass + * @return false: disabled I2C bypass + */ +bool MPU6050_getI2CBypassEnabled(MPU6050_st *s_mpu); + +/** + * @brief Set I2C bypass enabled status: + * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to + * 0, the host application processor will be able to directly access the + * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host + * application processor will not be able to directly access the auxiliary I2C + * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 + * bit[5]). + * + * @param s_mpu structure of MPU6050 + * @param enabled New I2C bypass enabled status + */ +void MPU6050_setI2CBypassEnabled(MPU6050_st *s_mpu, bool enabled); + +/** + * @brief Get sleep mode status + * + * @param s_mpu structure of MPU6050 + * @return true: enabled sleep mode + * @return false: disabled sleep mode + */ +bool MPU6050_getSleepEnabled(MPU6050_st *s_mpu); + +/** + * @brief Set sleep mode status + * + * @param s_mpu structure of MPU6050 + * @param enabled New sleep mode enabled status + */ +void MPU6050_setSleepEnabled(MPU6050_st *s_mpu, bool enabled); + +/** + * @brief Get clock source setting + * + * @param s_mpu structure of MPU6050 + * @return Current clock source setting + */ +uint8_t MPU6050_getClockSource(MPU6050_st *s_mpu); + +/** + * @brief Set clock source setting + * + * @param s_mpu structure of MPU6050 + * @param source New clock source setting + */ +void MPU6050_setClockSource(MPU6050_st *s_mpu, uint8_t source); + +// WHO_AM_I register + +/** + * @brief Get Device ID, verify the identity of the device + * + * @param s_mpu structure of MPU6050 + * @return Device ID (6 bits only! should be 0x34) + */ +uint8_t MPU6050_getDeviceID(MPU6050_st *s_mpu); + +/** + * @brief Set Device ID. + * Write a new ID into the WHO_AM_I register (no idea why this should ever be + * necessary though) + * + * @param s_mpu structure of MPU6050 + * @param id New device ID to set + */ +void MPU6050_setDeviceID(MPU6050_st *s_mpu, uint8_t id); + +/** + * @brief + * + * @param s_mpu + * @param ax + * @param ay + * @param az + * @param gx + * @param gy + * @param gz + */ +void MPU6050_getRawMotion6(MPU6050_st *s_mpu, int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); +void MPU6050_getMotion6(MPU6050_st *s_mpu, double *_ax, double *_ay, double *_az, double *_gx, double *_gy, double *_gz, float accel_sensitivity, float gyro_sensitivity); +float MPU6050_getElevation(MPU6050_st *s_mpu, double accX, double accY, double accZ); + + +#endif /* _MPU6050_H_ */ \ No newline at end of file