diff --git a/README.md b/README.md
index a87a97d..4f0c8f5 100644
--- a/README.md
+++ b/README.md
@@ -7,7 +7,12 @@
- [Installation and compilation](#installation-and-compilation)
- [Serial monitor tool](#serial-monitor-tool)
- [Monitoring tool](#monitoring-tool)
+ - [Sender and Reciever](#sender-and-reciever)
- [Overview](#overview)
+ - [Schema](#schema)
+ - [Sender](#sender)
+ - [Reciever](#reciever)
+ - [Master](#master)
- [Arduino Sender Format](#arduino-sender-format)
- [Modules](#modules)
- [10 DOF IMU](#10-dof-imu)
@@ -19,6 +24,8 @@
- [L76K GPS Module](#l76k-gps-module)
- [NRF24L01+ Module](#nrf24l01-module)
- [Issues / features](#issues--features)
+ - [Sender issues](#sender-issues)
+ - [Monitor app issues](#monitor-app-issues)
- [Sources](#sources)
## Installation and compilation
@@ -48,14 +55,32 @@ go build .
> Example command: `CC=x86_64-w64-mingw32-gcc GOOS=windows GOARCH=amd64 CGO_ENABLED=1 go build .`
> For more information see: [Pixel2 Wiki | Cross Compiling](https://github.com/gopxl/pixel/wiki/%5BWIP%5D-Cross-Compiling)
+### Sender and Reciever
+
Upload sender and reciever code on the 2 arduino's
+Required library for antenna: [RF24](https://nrf24.github.io/RF24)
+
## Overview
-`reciever_module/` - folder with code for reciver which will send data to the pc
-`sender_module/` - folder with code for sender, which transmit data to the reciever and save it on the micro sd card
+`reciever_module/` - folder with code for reciver which will send data to the pc (arduino)
+`sender_module/` - folder with code for sender, which transmit data to the reciever and save it on the micro sd card (arduino)
`serial_read/` - read serial input and save it
-`monitor` - folder with code for monitor which will recieve data and print them into the gui application
+`monitor` - folder with code for monitor which will recieve data and print them into the gui application (pc)
+
+### Schema
+
+#### Sender
+
+- device (arduino uno, arduino nano or arduino micro...) which recieves data from *[10DOF IMU](#10-dof-imu)* and *[L76K GPS](#l76k-gps)*, then it sends them via *[nrf24l01+](#nrf24l01)* in [sender format](#arduino-sender-format) to [reciever](#reciever)
+
+#### Reciever
+
+- device (arduino uno r4 wifi, arduino uno or arduino nano...) which recieves data from [sender](#sender) via *[nrf24l01+](#nrf24l01)* and then send them through serial line to the [master device](#master) in the [sender format](#arduino-sender-format)
+
+#### Master
+
+- device (pc, notebook, laptop), where the data are processed, parsed, saved and displayed realtime in the application monitor
### Arduino Sender Format
@@ -155,10 +180,20 @@ $GPGAA,HHMMSS.SS,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx
## Issues / features
-- data stops being transmitted after some short time
-- no gui
-- parser should be updated
-- sender code should be updated
+### Sender issues
+
+- [ ] data stops being transmitted from sender after some short period time
+
+### Monitor app issues
+
+- [ ] application crash after some period of time
+- [ ] gui is not updating until it recieves serial input
+- [ ] parser should be improved
+- [ ] sender code should be improved
+- [x] gui window
+- [ ] error messages as windows not terminal
+- [ ] improve readability of code
+- [ ] serial monitor setup port and baudrate
## Sources
@@ -173,4 +208,4 @@ Datasheets, documentation and sources
- [NRF24L01+ Guide - Guide](https://navody.dratek.cz/navody-k-produktum/arduino-wifi-modul-nrf24l01.html)
- [NRF24L01+ PA/LNA - Datasheet](doc/nrf24lo1-datasheet.pdf)
- [NRF24L01+ PA/LNA - Demo code](https://img.gme.cz/files/eshop_data/eshop_data/10/775-034/prg.775-034.1.zip)
-- [Pixel2 (gui library)](https://github.com/gopxl/pixel)
+- [Pixel2 (go gui library) - Github repository](https://github.com/gopxl/pixel)
diff --git a/monitor/gui/gui.go b/monitor/gui/gui.go
index 6cd8671..583ca83 100644
--- a/monitor/gui/gui.go
+++ b/monitor/gui/gui.go
@@ -11,11 +11,7 @@ import (
"golang.org/x/image/font"
)
-// Declare default font and font size
-const FONT string = "gui/JetBrainsMonoNerdFont-Medium.ttf"
-const SIZE float64 = 36
-
-func LoadFont() (*text.Atlas, error) {
+func LoadFont(FONT string, SIZE float64) (*text.Atlas, error) {
face, err := LoadTTF(FONT, SIZE)
if err != nil {
return nil, err
diff --git a/monitor/main.go b/monitor/main.go
index 297bb26..6e412b0 100644
--- a/monitor/main.go
+++ b/monitor/main.go
@@ -4,6 +4,7 @@ import (
"log"
gui "foglar/monitor/gui"
+ p "foglar/monitor/parse"
"foglar/monitor/serial_read"
"github.com/gopxl/pixel"
@@ -13,6 +14,12 @@ import (
)
func run() {
+ const LOG_FONT string = "gui/JetBrainsMonoNerdFont-Medium.ttf"
+ const LOG_SIZE float64 = 12
+
+ const INFO_FONT string = "gui/JetBrainsMonoNerdFont-Medium.ttf"
+ const INFO_SIZE float64 = 24
+
// Initialize serial connection
serialHandler, err := serial_read.NewSerialHandler()
if err != nil {
@@ -31,26 +38,53 @@ func run() {
panic(err)
}
- // Load font
- atlas, err := gui.LoadFont()
+ // Load fonts
+ log_atlas, err := gui.LoadFont(LOG_FONT, LOG_SIZE)
if err != nil {
panic(err)
}
- txt := text.New(pixel.V(100, 500), atlas)
+ info_atlas, err := gui.LoadFont(INFO_FONT, INFO_SIZE)
+ if err != nil {
+ panic(err)
+ }
+
+ logging_serial := text.New(pixel.V(800, 10), log_atlas)
+ temperature := text.New(pixel.V(100, 740), info_atlas)
+
+ var temperature_gui string
// Window update
for !win.Closed() {
+ // Read Serial Port
data, err := serialHandler.ReadSerial()
+ defer serialHandler.Close()
if err != nil {
log.Fatal(err)
}
- txt.WriteString(data)
+ // Parsing data
+ info := p.Parser(data)
+
+ // Clear screen values
+ temperature.Clear()
+
+ // Update information if it is in the parsed block
+ if _, ok := info[1]; ok {
+ temperature_gui = info[1]
+ }
win.Clear(colornames.Black)
- txt.Draw(win, pixel.IM)
+
+ // Print information to text blocks
+ temperature.WriteString("Temperature: " + temperature_gui)
+ logging_serial.WriteString(data)
+
+ // Draw information to screen
+ logging_serial.Draw(win, pixel.IM)
+ temperature.Draw(win, pixel.IM)
+
win.Update()
}
}
diff --git a/testing-sender/Waveshare_10Dof-D.cpp b/testing-sender/Waveshare_10Dof-D.cpp
new file mode 100644
index 0000000..fd05fd2
--- /dev/null
+++ b/testing-sender/Waveshare_10Dof-D.cpp
@@ -0,0 +1,743 @@
+/**
+ ******************************************************************************
+ * @file Waveshare_10Dof-D.cpp
+ * @author Waveshare Team
+ * @version V1.0
+ * @date Dec-2018
+ * @brief T
+
+ ******************************************************************************
+ * @attention
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, WAVESHARE SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * © COPYRIGHT 2018 Waveshare
+ ******************************************************************************
+ */
+#include "Waveshare_10Dof-D.h"
+#include
+
+IMU_ST_SENSOR_DATA gstGyroOffset ={0,0,0};
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+float invSqrt(float x);
+
+void icm20948init(void);
+bool icm20948Check(void);
+void icm20948GyroRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z);
+void icm20948AccelRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z);
+void icm20948MagRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z);
+bool icm20948MagCheck(void);
+void icm20948CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
+void icm20948GyroOffset(void);
+void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8Len, uint8_t *pu8data);
+void icm20948WriteSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8data);
+bool icm20948Check(void);
+
+bool bmp280Check(void);
+void bmp280Init(void);
+/******************************************************************************
+ * interface driver *
+ ******************************************************************************/
+uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr)
+{
+ uint8_t value;
+
+ Wire.beginTransmission(DevAddr);
+ Wire.write((byte)RegAddr);
+ Wire.endTransmission();
+
+ Wire.requestFrom(DevAddr, (byte)1);
+ value = Wire.read();
+
+ return value;
+}
+
+void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t value)
+{
+ Wire.beginTransmission(DevAddr);
+ Wire.write(RegAddr);
+ Wire.write(value);
+ Wire.endTransmission();
+}
+/******************************************************************************
+ * IMU module *
+ ******************************************************************************/
+#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
+
+float angles[3];
+float q0, q1, q2, q3;
+
+void imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType, IMU_EN_SENSOR_TYPE *penPressureType)
+{
+ bool bRet = false;
+ Wire.begin();
+ bRet = icm20948Check();
+ if( true == bRet)
+ {
+ *penMotionSensorType = IMU_EN_SENSOR_TYPE_ICM20948;
+ icm20948init();
+ }
+ else
+ {
+ *penMotionSensorType = IMU_EN_SENSOR_TYPE_NULL;
+ }
+
+ bRet = bmp280Check();
+ if( true == bRet)
+ {
+ *penPressureType = IMU_EN_SENSOR_TYPE_BMP280;
+ bmp280Init();
+ }
+ else
+ {
+ *penPressureType = IMU_EN_SENSOR_TYPE_NULL;
+ }
+
+ q0 = 1.0f;
+ q1 = 0.0f;
+ q2 = 0.0f;
+ q3 = 0.0f;
+
+ return;
+}
+
+void imuDataGet(IMU_ST_ANGLES_DATA *pstAngles,
+ IMU_ST_SENSOR_DATA *pstGyroRawData,
+ IMU_ST_SENSOR_DATA *pstAccelRawData,
+ IMU_ST_SENSOR_DATA *pstMagnRawData)
+{
+ float MotionVal[9];
+ int16_t s16Gyro[3], s16Accel[3], s16Magn[3];
+
+ icm20948AccelRead(&s16Accel[0], &s16Accel[1], &s16Accel[2]);
+ icm20948GyroRead(&s16Gyro[0], &s16Gyro[1], &s16Gyro[2]);
+ icm20948MagRead(&s16Magn[0], &s16Magn[1], &s16Magn[2]);
+
+ MotionVal[0]=s16Gyro[0]/32.8;
+ MotionVal[1]=s16Gyro[1]/32.8;
+ MotionVal[2]=s16Gyro[2]/32.8;
+ MotionVal[3]=s16Accel[0];
+ MotionVal[4]=s16Accel[1];
+ MotionVal[5]=s16Accel[2];
+ MotionVal[6]=s16Magn[0];
+ MotionVal[7]=s16Magn[1];
+ MotionVal[8]=s16Magn[2];
+ imuAHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
+ (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
+ (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
+
+
+ pstAngles->fPitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
+ pstAngles->fRoll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
+ pstAngles->fYaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;
+
+ pstGyroRawData->s16X = s16Gyro[0];
+ pstGyroRawData->s16Y = s16Gyro[1];
+ pstGyroRawData->s16Z = s16Gyro[2];
+
+ pstAccelRawData->s16X = s16Accel[0];
+ pstAccelRawData->s16Y = s16Accel[1];
+ pstAccelRawData->s16Z = s16Accel[2];
+
+ pstMagnRawData->s16X = s16Magn[0];
+ pstMagnRawData->s16Y = s16Magn[1];
+ pstMagnRawData->s16Z = s16Magn[2];
+
+ return;
+}
+
+void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+ float norm;
+ float hx, hy, hz, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
+ float ex, ey, ez, halfT = 0.024f;
+
+ float q0q0 = q0 * q0;
+ float q0q1 = q0 * q1;
+ float q0q2 = q0 * q2;
+ float q0q3 = q0 * q3;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q3q3 = q3 * q3;
+
+ norm = invSqrt(ax * ax + ay * ay + az * az);
+ ax = ax * norm;
+ ay = ay * norm;
+ az = az * norm;
+
+ norm = invSqrt(mx * mx + my * my + mz * mz);
+ mx = mx * norm;
+ my = my * norm;
+ mz = mz * norm;
+
+ // compute reference direction of flux
+ hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
+ hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
+ hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = hz;
+
+ // estimated direction of gravity and flux (v and w)
+ vx = 2 * (q1q3 - q0q2);
+ vy = 2 * (q0q1 + q2q3);
+ vz = q0q0 - q1q1 - q2q2 + q3q3;
+ wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
+ wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
+ wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
+
+ // error is sum of cross product between reference direction of fields and direction measured by sensors
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+
+ if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
+ {
+ exInt = exInt + ex * Ki * halfT;
+ eyInt = eyInt + ey * Ki * halfT;
+ ezInt = ezInt + ez * Ki * halfT;
+
+ gx = gx + Kp * ex + exInt;
+ gy = gy + Kp * ey + eyInt;
+ gz = gz + Kp * ez + ezInt;
+ }
+
+ q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
+ q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
+ q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
+ q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
+
+ norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 = q0 * norm;
+ q1 = q1 * norm;
+ q2 = q2 * norm;
+ q3 = q3 * norm;
+}
+
+float invSqrt(float x)
+{
+ float halfx = 0.5f * x;
+ float y = x;
+
+ long i = *(long*)&y; //get bits for floating value
+ i = 0x5f3759df - (i >> 1); //gives initial guss you
+ y = *(float*)&i; //convert bits back to float
+ y = y * (1.5f - (halfx * y * y)); //newtop step, repeating increases accuracy
+
+ return y;
+}
+/******************************************************************************
+ * icm20948 sensor device *
+ ******************************************************************************/
+void icm20948init(void)
+{
+ /* user bank 0 register */
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1, REG_VAL_ALL_RGE_RESET);
+ delay(10);
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1, REG_VAL_RUN_MODE);
+
+ /* user bank 2 register */
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_2);
+ I2C_WriteOneByte( I2C_ADD_ICM20948, REG_ADD_GYRO_SMPLRT_DIV, 0x07);
+ I2C_WriteOneByte( I2C_ADD_ICM20948, REG_ADD_GYRO_CONFIG_1,
+ REG_VAL_BIT_GYRO_DLPCFG_6 | REG_VAL_BIT_GYRO_FS_1000DPS | REG_VAL_BIT_GYRO_DLPF);
+ I2C_WriteOneByte( I2C_ADD_ICM20948, REG_ADD_ACCEL_SMPLRT_DIV_2, 0x07);
+ I2C_WriteOneByte( I2C_ADD_ICM20948, REG_ADD_ACCEL_CONFIG,
+ REG_VAL_BIT_ACCEL_DLPCFG_6 | REG_VAL_BIT_ACCEL_FS_2g | REG_VAL_BIT_ACCEL_DLPF);
+
+ /* user bank 0 register */
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
+
+ delay(100);
+ /* offset */
+ icm20948GyroOffset();
+
+ icm20948MagCheck();
+
+ icm20948WriteSecondary( I2C_ADD_ICM20948_AK09916|I2C_ADD_ICM20948_AK09916_WRITE,
+ REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_20HZ);
+ return;
+}
+
+bool icm20948Check(void)
+{
+ bool bRet = false;
+ if(REG_VAL_WIA == I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_WIA))
+ {
+ bRet = true;
+ }
+ return bRet;
+}
+void icm20948GyroRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z)
+{
+ uint8_t u8Buf[6];
+ int16_t s16Buf[3] = {0};
+ uint8_t i;
+ int32_t s32OutBuf[3] = {0};
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+ static int16_t ss16c = 0;
+ ss16c++;
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_XOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_XOUT_H);
+ s16Buf[0]= (u8Buf[1]<<8)|u8Buf[0];
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_YOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_YOUT_H);
+ s16Buf[1]= (u8Buf[1]<<8)|u8Buf[0];
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_ZOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_GYRO_ZOUT_H);
+ s16Buf[2]= (u8Buf[1]<<8)|u8Buf[0];
+
+ for(i = 0; i < 3; i ++)
+ {
+ icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
+ }
+ *ps16X = s32OutBuf[0] - gstGyroOffset.s16X;
+ *ps16Y = s32OutBuf[1] - gstGyroOffset.s16Y;
+ *ps16Z = s32OutBuf[2] - gstGyroOffset.s16Z;
+
+ return;
+}
+void icm20948AccelRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z)
+{
+ uint8_t u8Buf[2];
+ int16_t s16Buf[3] = {0};
+ uint8_t i;
+ int32_t s32OutBuf[3] = {0};
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_XOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_XOUT_H);
+ s16Buf[0]= (u8Buf[1]<<8)|u8Buf[0];
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_YOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_YOUT_H);
+ s16Buf[1]= (u8Buf[1]<<8)|u8Buf[0];
+
+ u8Buf[0]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_ZOUT_L);
+ u8Buf[1]=I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_ACCEL_ZOUT_H);
+ s16Buf[2]= (u8Buf[1]<<8)|u8Buf[0];
+
+ for(i = 0; i < 3; i ++)
+ {
+ icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
+ }
+ *ps16X = s32OutBuf[0];
+ *ps16Y = s32OutBuf[1];
+ *ps16Z = s32OutBuf[2];
+
+ return;
+
+}
+void icm20948MagRead(int16_t* ps16X, int16_t* ps16Y, int16_t* ps16Z)
+{
+ uint8_t counter = 20;
+ uint8_t u8Data[MAG_DATA_LEN];
+ int16_t s16Buf[3] = {0};
+ uint8_t i;
+ int32_t s32OutBuf[3] = {0};
+ static ICM20948_ST_AVG_DATA sstAvgBuf[3];
+ while( counter>0 )
+ {
+ delay(10);
+ icm20948ReadSecondary( I2C_ADD_ICM20948_AK09916|I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_ST2, 1, u8Data);
+
+ if ((u8Data[0] & 0x01) != 0)
+ break;
+
+ counter--;
+ }
+
+ if(counter != 0)
+ {
+ icm20948ReadSecondary( I2C_ADD_ICM20948_AK09916|I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_DATA,
+ MAG_DATA_LEN,
+ u8Data);
+ s16Buf[0] = ((int16_t)u8Data[1]<<8) | u8Data[0];
+ s16Buf[1] = ((int16_t)u8Data[3]<<8) | u8Data[2];
+ s16Buf[2] = ((int16_t)u8Data[5]<<8) | u8Data[4];
+ }
+
+ for(i = 0; i < 3; i ++)
+ {
+ icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
+ }
+
+ *ps16X = s32OutBuf[0];
+ *ps16Y = -s32OutBuf[1];
+ *ps16Z = -s32OutBuf[2];
+ return;
+}
+
+void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8Len, uint8_t *pu8data)
+{
+ uint8_t i;
+ uint8_t u8Temp;
+
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_ADDR, u8I2CAddr);
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_REG, u8RegAddr);
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN|u8Len);
+
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0
+
+ u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_USER_CTRL);
+ u8Temp |= REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);
+ delay(5);
+ u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
+ I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);
+
+ for(i=0; i>= 3;
+}
+
+void icm20948GyroOffset(void)
+{
+ uint8_t i;
+ int16_t s16Gx = 0, s16Gy = 0, s16Gz = 0;
+ int32_t s32TempGx = 0, s32TempGy = 0, s32TempGz = 0;
+ for(i = 0; i < 32; i ++)
+ {
+ icm20948GyroRead(&s16Gx, &s16Gy, &s16Gz);
+ s32TempGx += s16Gx;
+ s32TempGy += s16Gy;
+ s32TempGz += s16Gz;
+ delay(10);
+ }
+ gstGyroOffset.s16X = s32TempGx >> 5;
+ gstGyroOffset.s16Y = s32TempGy >> 5;
+ gstGyroOffset.s16Z = s32TempGz >> 5;
+ return;
+}
+
+bool icm20948MagCheck(void)
+{
+ bool bRet = false;
+ uint8_t u8Ret[2];
+
+ icm20948ReadSecondary( I2C_ADD_ICM20948_AK09916|I2C_ADD_ICM20948_AK09916_READ,
+ REG_ADD_MAG_WIA1, 2,u8Ret);
+ if( (u8Ret[0] == REG_VAL_MAG_WIA1) && ( u8Ret[1] == REG_VAL_MAG_WIA2) )
+ {
+ bRet = true;
+ }
+
+ return bRet;
+}
+
+/******************************************************************************
+ * BMP280 sensor device *
+ ******************************************************************************/
+ typedef struct {
+ uint16_t T1; /*>3) - ((int64_t)dig_T1 <<1))) *((int64_t)dig_T2)) >> 11;
+ var2 = (((((adc_T>>4) - ((int64_t)dig_T1)) *((adc_T>>4) - ((int64_t)dig_T1))) >> 12) *
+ ((int64_t)dig_T3)) >> 14;
+ t_fine = var1 + var2;
+
+ temperature = (t_fine * 5 + 128) >> 8;
+
+ return (float)temperature;
+}
+
+float bmp280CompensatePressure(int32_t adc_P)
+{
+ int64_t var1, var2;
+ uint64_t pressure;
+#if 1
+ var1 = ((int64_t)t_fine) - 128000;
+ var2 = var1 * var1 * (int64_t)dig_P6;
+ var2 = var2 + ((var1*(int64_t)dig_P5)<<17);
+ var2 = var2 + (((int64_t)dig_P4)<<35);
+ var1 = ((var1 * var1 * (int64_t)dig_P3)>>8) + ((var1 * (int64_t)dig_P2)<<12);
+ var1 = (((((int64_t)1)<<47)+var1))*((int64_t)dig_P1)>>33;
+
+ if (var1 == 0) {
+ return 0; // avoid exception caused by division by zero
+ }
+
+ pressure = 1048576.0 - adc_P;
+ pressure = (((pressure<<31) - var2)*3125) / var1;
+ var1 = (((int64_t)dig_P9) * (pressure>>13) * (pressure>>13)) >> 25;
+ var2 = (((int64_t)dig_P8) * pressure) >> 19;
+ pressure = ((pressure + var1 + var2) >> 8) + (((int64_t)dig_P7)<<4);
+ return (float)pressure/256;
+#else
+ var1 = (((int64_t)t_fine)>>1) - (int64_t)64000;
+ var2 = (((var1>>2) * (var1>>2)) >> 11 ) *((int64_t)dig_P6);
+ var2 = var2 + ((var1 *((int64_t)dig_P5))<<1);
+ var2 = (var2>>2) + (((int64_t)dig_P4)<<16);
+ var1 = (((dig_P3 * (((var1>>2) * (var1>>2))>>13))>>3) + ((((int64_t)dig_P2) * var1)>>1))>>18;
+ var1 = ((((32768+var1))*((int64_t)dig_P1))>>15);
+ if(var1 ==0)
+ {
+ return 0;
+ }
+ pressure = (1048576.0 - adc_P) - (var2>>12)*3125;
+ if(pressure < 0x80000000)
+ {
+ pressure = (pressure<<1)/((uint64_t)var1);
+ }
+ else
+ {
+ pressure = (pressure/(uint64_t)var1)*2;
+ }
+ var1 = (((int64_t)dig_P9) *((int64_t)(((pressure>>3)*(pressure>>3))>>13)))>>12;
+ var2 = (((int64_t)(pressure>>2))*((int64_t)dig_P8))>>13;
+ pressure = (uint64_t)((int64_t)pressure) +((var1 + var2 + dig_P7)>>4);
+ return (float)pressure;
+#endif
+
+}
+
+void bmp280TandPGet(float *temperature, float *pressure)
+{
+ uint8_t lsb, msb, xlsb;
+ int32_t adc_P,adc_T;
+
+ xlsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_TEMP_XLSB_REG);
+ lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_TEMP_LSB_REG);
+ msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_TEMP_MSB_REG);
+ //adc_T = (msb << 12) | (lsb << 4) | (xlsb >> 4);
+ adc_T = msb;
+ adc_T <<= 8;
+ adc_T |= lsb;
+ adc_T <<= 8;
+ adc_T |= xlsb;
+ adc_T >>= 4;
+ //adc_T = 415148;
+ *temperature = bmp280CompensateTemperature(adc_T);
+
+ xlsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_PRESS_XLSB_REG);
+ lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_PRESS_LSB_REG);
+ msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_PRESS_MSB_REG);
+ //adc_P = (msb << 12) | (lsb << 4) | (xlsb >> 4);
+ adc_P = msb;
+ adc_P <<= 8;
+ adc_P |= lsb;
+ adc_P <<= 8;
+ adc_P |= xlsb;
+ adc_P >>= 4;
+ //adc_P = 51988;
+ *pressure = bmp280CompensatePressure(adc_P);
+}
+
+void bmp280CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal)
+{
+ uint8_t i;
+
+ *(pAvgBuffer + ((*pIndex) ++)) = InVal;
+ *pIndex &= 0x07;
+
+ *pOutVal = 0;
+ for(i = 0; i < 8; i ++)
+ {
+ *pOutVal += *(pAvgBuffer + i);
+ }
+ *pOutVal >>= 3;
+}
+
+void bmp280CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal)
+{
+ *pAltitude = 4433000 * (1 - pow((PressureVal / (float)gs32Pressure0), 0.1903));
+}
+
+void pressSensorDataGet(int32_t *ps32Temperature, int32_t *ps32Pressure, int32_t *ps32Altitude)
+{
+ float CurPressure, CurTemperature;
+ int32_t CurAltitude;
+ static BMP280_AvgTypeDef BMP280_Filter[3];
+
+ bmp280TandPGet(&CurTemperature, &CurPressure);
+ bmp280CalAvgValue(&BMP280_Filter[0].Index, BMP280_Filter[0].AvgBuffer, (int32_t)(CurPressure), ps32Pressure);
+
+ bmp280CalculateAbsoluteAltitude(&CurAltitude, (*ps32Pressure));
+ bmp280CalAvgValue(&BMP280_Filter[1].Index, BMP280_Filter[1].AvgBuffer, CurAltitude, ps32Altitude);
+ bmp280CalAvgValue(&BMP280_Filter[2].Index, BMP280_Filter[2].AvgBuffer, (int32_t)CurTemperature, ps32Temperature);
+ return;
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/testing-sender/Waveshare_10Dof-D.h b/testing-sender/Waveshare_10Dof-D.h
new file mode 100644
index 0000000..dfbd978
--- /dev/null
+++ b/testing-sender/Waveshare_10Dof-D.h
@@ -0,0 +1,236 @@
+/**
+ ******************************************************************************
+ * @file Waveshare_10Dof-D.h
+ * @author Waveshare Team
+ * @version V1.0
+ * @date Dec-2018
+ * @brief
+
+ ******************************************************************************
+ * @attention
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, WAVESHARE SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * © COPYRIGHT 2018 Waveshare
+ ******************************************************************************
+ */
+#ifndef __Waveshare_10DOF_D_H__
+#define __Waveshare_10DOF_D_H__
+#if ARDUINO >= 100
+ #include "Arduino.h"
+#else
+ #include "WProgram.h"
+#endif
+
+/* define ICM-20948 Device I2C address*/
+#define I2C_ADD_ICM20948 0x68
+#define I2C_ADD_ICM20948_AK09916 0x0C
+#define I2C_ADD_ICM20948_AK09916_READ 0x80
+#define I2C_ADD_ICM20948_AK09916_WRITE 0x00
+/* define ICM-20948 Register */
+/* user bank 0 register */
+#define REG_ADD_WIA 0x00
+ #define REG_VAL_WIA 0xEA
+#define REG_ADD_USER_CTRL 0x03
+ #define REG_VAL_BIT_DMP_EN 0x80
+ #define REG_VAL_BIT_FIFO_EN 0x40
+ #define REG_VAL_BIT_I2C_MST_EN 0x20
+ #define REG_VAL_BIT_I2C_IF_DIS 0x10
+ #define REG_VAL_BIT_DMP_RST 0x08
+ #define REG_VAL_BIT_DIAMOND_DMP_RST 0x04
+#define REG_ADD_PWR_MIGMT_1 0x06
+ #define REG_VAL_ALL_RGE_RESET 0x80
+ #define REG_VAL_RUN_MODE 0x01 //Non low-power mode
+#define REG_ADD_LP_CONFIG 0x05
+#define REG_ADD_PWR_MGMT_1 0x06
+#define REG_ADD_PWR_MGMT_2 0x07
+#define REG_ADD_ACCEL_XOUT_H 0x2D
+#define REG_ADD_ACCEL_XOUT_L 0x2E
+#define REG_ADD_ACCEL_YOUT_H 0x2F
+#define REG_ADD_ACCEL_YOUT_L 0x30
+#define REG_ADD_ACCEL_ZOUT_H 0x31
+#define REG_ADD_ACCEL_ZOUT_L 0x32
+#define REG_ADD_GYRO_XOUT_H 0x33
+#define REG_ADD_GYRO_XOUT_L 0x34
+#define REG_ADD_GYRO_YOUT_H 0x35
+#define REG_ADD_GYRO_YOUT_L 0x36
+#define REG_ADD_GYRO_ZOUT_H 0x37
+#define REG_ADD_GYRO_ZOUT_L 0x38
+#define REG_ADD_EXT_SENS_DATA_00 0x3B
+#define REG_ADD_REG_BANK_SEL 0x7F
+ #define REG_VAL_REG_BANK_0 0x00
+ #define REG_VAL_REG_BANK_1 0x10
+ #define REG_VAL_REG_BANK_2 0x20
+ #define REG_VAL_REG_BANK_3 0x30
+
+/* user bank 1 register */
+/* user bank 2 register */
+#define REG_ADD_GYRO_SMPLRT_DIV 0x00
+#define REG_ADD_GYRO_CONFIG_1 0x01
+ #define REG_VAL_BIT_GYRO_DLPCFG_2 0x10 /* bit[5:3] */
+ #define REG_VAL_BIT_GYRO_DLPCFG_4 0x20 /* bit[5:3] */
+ #define REG_VAL_BIT_GYRO_DLPCFG_6 0x30 /* bit[5:3] */
+ #define REG_VAL_BIT_GYRO_FS_250DPS 0x00 /* bit[2:1] */
+ #define REG_VAL_BIT_GYRO_FS_500DPS 0x02 /* bit[2:1] */
+ #define REG_VAL_BIT_GYRO_FS_1000DPS 0x04 /* bit[2:1] */
+ #define REG_VAL_BIT_GYRO_FS_2000DPS 0x06 /* bit[2:1] */
+ #define REG_VAL_BIT_GYRO_DLPF 0x01 /* bit[0] */
+#define REG_ADD_ACCEL_SMPLRT_DIV_2 0x11
+#define REG_ADD_ACCEL_CONFIG 0x14
+ #define REG_VAL_BIT_ACCEL_DLPCFG_2 0x10 /* bit[5:3] */
+ #define REG_VAL_BIT_ACCEL_DLPCFG_4 0x20 /* bit[5:3] */
+ #define REG_VAL_BIT_ACCEL_DLPCFG_6 0x30 /* bit[5:3] */
+ #define REG_VAL_BIT_ACCEL_FS_2g 0x00 /* bit[2:1] */
+ #define REG_VAL_BIT_ACCEL_FS_4g 0x02 /* bit[2:1] */
+ #define REG_VAL_BIT_ACCEL_FS_8g 0x04 /* bit[2:1] */
+ #define REG_VAL_BIT_ACCEL_FS_16g 0x06 /* bit[2:1] */
+ #define REG_VAL_BIT_ACCEL_DLPF 0x01 /* bit[0] */
+
+/* user bank 3 register */
+#define REG_ADD_I2C_SLV0_ADDR 0x03
+#define REG_ADD_I2C_SLV0_REG 0x04
+#define REG_ADD_I2C_SLV0_CTRL 0x05
+ #define REG_VAL_BIT_SLV0_EN 0x80
+ #define REG_VAL_BIT_MASK_LEN 0x07
+#define REG_ADD_I2C_SLV0_DO 0x06
+#define REG_ADD_I2C_SLV1_ADDR 0x07
+#define REG_ADD_I2C_SLV1_REG 0x08
+#define REG_ADD_I2C_SLV1_CTRL 0x09
+#define REG_ADD_I2C_SLV1_DO 0x0A
+
+/* define ICM-20948 Register end */
+
+/* define ICM-20948 MAG Register */
+#define REG_ADD_MAG_WIA1 0x00
+ #define REG_VAL_MAG_WIA1 0x48
+#define REG_ADD_MAG_WIA2 0x01
+ #define REG_VAL_MAG_WIA2 0x09
+#define REG_ADD_MAG_ST2 0x10
+#define REG_ADD_MAG_DATA 0x11
+#define REG_ADD_MAG_CNTL2 0x31
+ #define REG_VAL_MAG_MODE_PD 0x00
+ #define REG_VAL_MAG_MODE_SM 0x01
+ #define REG_VAL_MAG_MODE_10HZ 0x02
+ #define REG_VAL_MAG_MODE_20HZ 0x04
+ #define REG_VAL_MAG_MODE_50HZ 0x05
+ #define REG_VAL_MAG_MODE_100HZ 0x08
+ #define REG_VAL_MAG_MODE_ST 0x10
+/* define ICM-20948 MAG Register end */
+
+#define MAG_DATA_LEN 6
+
+/*
+ * BMP280 I2c address
+ */
+#define BMP280_AD0_LOW 0x76 //address pin low (GND)
+#define BMP280_AD0_HIGH 0x77 //address pin high (VCC)
+#define BMP280_ADDR BMP280_AD0_HIGH // default I2C address
+/*
+ * BMP280 register address
+ */
+#define BMP280_REGISTER_DIG_T1 0x88
+#define BMP280_REGISTER_DIG_T2 0x8A
+#define BMP280_REGISTER_DIG_T3 0x8C
+
+#define BMP280_REGISTER_DIG_P1 0x8E
+#define BMP280_REGISTER_DIG_P2 0x90
+#define BMP280_REGISTER_DIG_P3 0x92
+#define BMP280_REGISTER_DIG_P4 0x94
+#define BMP280_REGISTER_DIG_P5 0x96
+#define BMP280_REGISTER_DIG_P6 0x98
+#define BMP280_REGISTER_DIG_P7 0x9A
+#define BMP280_REGISTER_DIG_P8 0x9C
+#define BMP280_REGISTER_DIG_P9 0x9E
+
+#define BMP280_REGISTER_CHIPID 0xD0
+#define BMP280_REGISTER_VERSION 0xD1
+#define BMP280_REGISTER_SOFTRESET 0xE0
+#define BMP280_REGISTER_STATUS 0xF3
+#define BMP280_REGISTER_CONTROL 0xF4
+#define BMP280_REGISTER_CONFIG 0xF5
+
+#define BMP280_TEMP_XLSB_REG 0xFC /*Temperature XLSB Register */
+#define BMP280_TEMP_LSB_REG 0xFB /*Temperature LSB Register */
+#define BMP280_TEMP_MSB_REG 0xFA /*Temperature LSB Register */
+#define BMP280_PRESS_XLSB_REG 0xF9 /*Pressure XLSB Register */
+#define BMP280_PRESS_LSB_REG 0xF8 /*Pressure LSB Register */
+#define BMP280_PRESS_MSB_REG 0xF7 /*Pressure MSB Register */
+
+/*calibration parameters */
+#define BMP280_DIG_T1_LSB_REG 0x88
+#define BMP280_DIG_T1_MSB_REG 0x89
+#define BMP280_DIG_T2_LSB_REG 0x8A
+#define BMP280_DIG_T2_MSB_REG 0x8B
+#define BMP280_DIG_T3_LSB_REG 0x8C
+#define BMP280_DIG_T3_MSB_REG 0x8D
+#define BMP280_DIG_P1_LSB_REG 0x8E
+#define BMP280_DIG_P1_MSB_REG 0x8F
+#define BMP280_DIG_P2_LSB_REG 0x90
+#define BMP280_DIG_P2_MSB_REG 0x91
+#define BMP280_DIG_P3_LSB_REG 0x92
+#define BMP280_DIG_P3_MSB_REG 0x93
+#define BMP280_DIG_P4_LSB_REG 0x94
+#define BMP280_DIG_P4_MSB_REG 0x95
+#define BMP280_DIG_P5_LSB_REG 0x96
+#define BMP280_DIG_P5_MSB_REG 0x97
+#define BMP280_DIG_P6_LSB_REG 0x98
+#define BMP280_DIG_P6_MSB_REG 0x99
+#define BMP280_DIG_P7_LSB_REG 0x9A
+#define BMP280_DIG_P7_MSB_REG 0x9B
+#define BMP280_DIG_P8_LSB_REG 0x9C
+#define BMP280_DIG_P8_MSB_REG 0x9D
+#define BMP280_DIG_P9_LSB_REG 0x9E
+#define BMP280_DIG_P9_MSB_REG 0x9F
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum
+{
+ IMU_EN_SENSOR_TYPE_NULL = 0,
+ IMU_EN_SENSOR_TYPE_ICM20948,
+ IMU_EN_SENSOR_TYPE_BMP280,
+ IMU_EN_SENSOR_TYPE_MAX
+}IMU_EN_SENSOR_TYPE;
+
+typedef struct imu_st_angles_data_tag
+{
+ float fYaw;
+ float fPitch;
+ float fRoll;
+}IMU_ST_ANGLES_DATA;
+
+typedef struct imu_st_sensor_data_tag
+{
+ int16_t s16X;
+ int16_t s16Y;
+ int16_t s16Z;
+}IMU_ST_SENSOR_DATA;
+
+typedef struct icm20948_st_avg_data_tag
+{
+ uint8_t u8Index;
+ int16_t s16AvgBuffer[8];
+}ICM20948_ST_AVG_DATA;
+
+void imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType, IMU_EN_SENSOR_TYPE *penPressureType);
+void imuDataGet(IMU_ST_ANGLES_DATA *pstAngles,
+ IMU_ST_SENSOR_DATA *pstGyroRawData,
+ IMU_ST_SENSOR_DATA *pstAccelRawData,
+ IMU_ST_SENSOR_DATA *pstMagnRawData);
+void pressSensorDataGet(int32_t *ps32Temperature, int32_t *ps32Pressure, int32_t *ps32Altitude);
+
+uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
+void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t value);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //__Waveshare_10DOF_D_H__
diff --git a/testing-sender/test.ino b/testing-sender/test.ino
new file mode 100644
index 0000000..7a00b77
--- /dev/null
+++ b/testing-sender/test.ino
@@ -0,0 +1,75 @@
+#include
+#include
+#include
+#include "Waveshare_10Dof-D.h"
+
+bool gbSenserConnectState = false;
+RF24 radio(9, 8); // CE, CSN
+const byte address[6] = "00001"; // address through which two modules communicate
+
+void setup() {
+ radio.begin();
+ radio.openWritingPipe(address);
+ radio.stopListening(); // Set module as transmitter
+
+ IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType;
+ Serial.begin(115200);
+ imuInit(&enMotionSensorType, &enPressureType);
+ if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) {
+ Serial.println("# Motion sensor is ICM-20948");
+ } else {
+ Serial.println("# Motion sensor NULL");
+ }
+ if (IMU_EN_SENSOR_TYPE_BMP280 == enPressureType) {
+ Serial.println("# Pressure sensor is BMP280");
+ } else {
+ Serial.println("# Pressure sensor NULL");
+ }
+ delay(1000);
+}
+
+void loop() {
+ IMU_ST_ANGLES_DATA stAngles;
+ IMU_ST_SENSOR_DATA stGyroRawData;
+ IMU_ST_SENSOR_DATA stAccelRawData;
+ IMU_ST_SENSOR_DATA stMagnRawData;
+ int32_t s32PressureVal = 0, s32TemperatureVal = 0, s32AltitudeVal = 0;
+
+ imuDataGet(&stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
+ pressSensorDataGet(&s32TemperatureVal, &s32PressureVal, &s32AltitudeVal);
+
+ float temperature = s32TemperatureVal / 100.0;
+ float pressure = s32PressureVal / 100.0;
+ float altitude = s32AltitudeVal / 100.0;
+
+ float angles[] = {stAngles.fRoll, stAngles.fPitch, stAngles.fYaw};
+ float gyro[] = {stGyroRawData.s16X, stGyroRawData.s16Y, stGyroRawData.s16Z};
+ float accel[] = {stAccelRawData.s16X, stAccelRawData.s16Y, stAccelRawData.s16Z};
+ float magn[] = {stMagnRawData.s16X, stMagnRawData.s16Y, stMagnRawData.s16Z};
+
+ char msg[64];
+ for (int i = 0; i < 3; i++) {
+ char float_str[8];
+ dtostrf(angles[i], 6, 2, float_str);
+ String str = String("$") + String(i + 1) + ";" + String(float_str) + "*";
+ str.toCharArray(msg, sizeof(msg));
+ radio.write(&msg, sizeof(msg));
+ }
+
+ float sensor_data[][3] = {
+ {temperature, pressure, altitude},
+ {gyro[0], gyro[1], gyro[2]},
+ {accel[0], accel[1], accel[2]},
+ {magn[0], magn[1], magn[2]}
+ };
+
+ for (int i = 0; i < 4; i++) {
+ for (int j = 0; j < 3; j++) {
+ char float_str[8];
+ dtostrf(sensor_data[i][j], 6, 2, float_str);
+ String str = String("$") + String(i + 4) + ";" + String(float_str) + "*";
+ str.toCharArray(msg, sizeof(msg));
+ radio.write(&msg, sizeof(msg));
+ }
+ }
+}