From 03544ae16afd7d17411cdee1cd704b1921e944fa Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Fri, 1 May 2020 00:01:30 +0100 Subject: [PATCH] Start with BMX055, but not working yet --- FollowMe.cfg | 5 ++- main/bmx055.h | 87 +++++++++++++++++++++++++++++++++++++++++++++++++++ main/imu.cpp | 67 +++++++++++++++++++++++++++++++++++++++ main/imu.h | 11 +++++++ 4 files changed, 169 insertions(+), 1 deletion(-) create mode 100644 main/bmx055.h create mode 100644 main/imu.cpp create mode 100644 main/imu.h diff --git a/FollowMe.cfg b/FollowMe.cfg index 94ace4a..4372504 100644 --- a/FollowMe.cfg +++ b/FollowMe.cfg @@ -19,7 +19,8 @@ // #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display // #define WITH_OLED2 // 2nd OLED display, I2C address next higher #define WITH_U8G2_OLED // I2C OLED through the U8g2 library -#define WITH_U8G2_SH1106 +#define WITH_U8G2_SH1106 // correct controller for the bigger OLED +#define WITH_U8G2_FLIP // flip the OLED screen (rotate by 180deg) #define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95 @@ -49,6 +50,8 @@ // #define WITH_MS5607 // MS5607 pressure sensor // #define WITH_MS5611 // MS5611 pressure sensor +#define WITH_BMX055 // BMX055 magnetic and IMU sensor + #define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000 // #define WITH_POGNT #define WITH_LOOKOUT diff --git a/main/bmx055.h b/main/bmx055.h new file mode 100644 index 0000000..8b5282b --- /dev/null +++ b/main/bmx055.h @@ -0,0 +1,87 @@ +#include +#include +#include + +#include "hal.h" + +class BMX055_ACC +{ private: + static const uint8_t ADDR0 = 0x18; // possible I2C addresses + static const uint8_t ADDR1 = 0x19; + + static const uint8_t REG_ID = 0x00; + + public: + uint8_t Bus; // which I2C bus + uint8_t ADDR; // detected I2C address + uint8_t ID; // 0x58 for BMP280, 0x60 for BME280 + + public: + uint8_t Error; // error on the I2C bus (0=no error) + + uint8_t CheckID(void) // check ID + { ADDR=0; + Error=I2C_Read(Bus, ADDR0, REG_ID, ID); + if( (!Error) && (ID==0xFA) ) { ADDR=ADDR0; return 0; } + Error=I2C_Read(Bus, ADDR1, REG_ID, ID); + if( (!Error) && (ID==0xFA) ) { ADDR=ADDR1; return 0; } + return 1; } // 0 => no error and correct ID + +} ; + +class BMX055_GYR +{ private: + static const uint8_t ADDR0 = 0x68; // possible I2C addresses + static const uint8_t ADDR1 = 0x69; + + static const uint8_t REG_ID = 0x00; + + public: + uint8_t Bus; // which I2C bus + uint8_t ADDR; // detected I2C address + uint8_t ID; // 0x58 for BMP280, 0x60 for BME280 + + public: + uint8_t Error; // error on the I2C bus (0=no error) + + uint8_t CheckID(void) // check ID + { ADDR=0; + Error=I2C_Read(Bus, ADDR0, REG_ID, ID); + if( (!Error) && (ID==0x0F) ) { ADDR=ADDR0; return 0; } + Error=I2C_Read(Bus, ADDR1, REG_ID, ID); + if( (!Error) && (ID==0x0F) ) { ADDR=ADDR1; return 0; } + return 1; } // 0 => no error and correct ID + +} ; + +class BMX055_MAG +{ private: + static const uint8_t ADDR0 = 0x10; // possible I2C addresses + static const uint8_t ADDR1 = 0x11; + static const uint8_t ADDR2 = 0x12; + static const uint8_t ADDR3 = 0x13; + + static const uint8_t REG_ID = 0x32; + + public: + uint8_t Bus; // which I2C bus + uint8_t ADDR; // detected I2C address + uint8_t ID; // 0x58 for BMP280, 0x60 for BME280 + + public: + uint8_t Error; // error on the I2C bus (0=no error) + + uint8_t CheckID(void) // check ID + { ADDR=0; + Error=I2C_Read(Bus, ADDR0, REG_ID, ID); + if( (!Error) && (ID==0x32) ) { ADDR=ADDR0; return 0; } + Error=I2C_Read(Bus, ADDR1, REG_ID, ID); + if( (!Error) && (ID==0x32) ) { ADDR=ADDR1; return 0; } + Error=I2C_Read(Bus, ADDR2, REG_ID, ID); + if( (!Error) && (ID==0x32) ) { ADDR=ADDR2; return 0; } + Error=I2C_Read(Bus, ADDR3, REG_ID, ID); + if( (!Error) && (ID==0x32) ) { ADDR=ADDR3; return 0; } + return 1; } // 0 => no error and correct ID + +} ; + diff --git a/main/imu.cpp b/main/imu.cpp new file mode 100644 index 0000000..6c7475c --- /dev/null +++ b/main/imu.cpp @@ -0,0 +1,67 @@ +#include "hal.h" +#include "sens.h" +#include "imu.h" + +#include "timesync.h" + +#include "parameters.h" + +#include "proc.h" +#include "ctrl.h" +#include "gps.h" + +#ifdef WITH_BMX055 + +#include "bmx055.h" + +BMX055_MAG MAG; +BMX055_ACC ACC; +BMX055_GYR GYR; + +static uint8_t InitIMU(void) +{ MAG.Bus=BARO_I2C; + ACC.Bus=BARO_I2C; + GYR.Bus=BARO_I2C; + uint8_t Err = MAG.CheckID(); Err<<=1; + if(!Err) Err |= ACC.CheckID(); Err<<=1; + if(!Err) Err |= GYR.CheckID(); + return Err; } + +static void ProcIMU(void) +{ + int16_t Sec = 10*(TimeSync_Time()%60); // [0.1sec] + uint16_t Phase = TimeSync_msTime(); // sync to the GPS PPS + if(Phase>=500) { Sec+=10; vTaskDelay(1000-Phase); } // wait till start of the measuring period + else { Sec+= 5; vTaskDelay( 500-Phase); } + if(Sec>=600) Sec-=600; // [0.1sec] measurement time + +} + +#endif + +extern "C" +void vTaskIMU(void* pvParameters) +{ vTaskDelay(100); // ? + +#ifdef WITH_BMX055 + uint8_t Detected = InitIMU(); + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "TaskIMU: "); + Format_Hex(CONS_UART_Write, Detected); CONS_UART_Write(':'); CONS_UART_Write(' '); + Format_Hex(CONS_UART_Write, MAG.ADDR); CONS_UART_Write(','); + Format_Hex(CONS_UART_Write, ACC.ADDR); CONS_UART_Write(','); + Format_Hex(CONS_UART_Write, GYR.ADDR); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); +#endif + + while(1) + { +#ifdef WITH_BMX055 + if(PowerMode) ProcIMU(); + else vTaskDelay(100); +#else + vTaskDelay(1000); +#endif + } +} diff --git a/main/imu.h b/main/imu.h new file mode 100644 index 0000000..df5ab90 --- /dev/null +++ b/main/imu.h @@ -0,0 +1,11 @@ +#ifndef __IMU_H__ +#define __IMU_H__ + +#ifdef __cplusplus + extern "C" +#endif +void vTaskIMU(void* pvParameters); + +#endif // __IMU_H__ + +