testing app, management improvements
This commit is contained in:
parent
51b1a7b88d
commit
e46483716d
20
README.md
20
README.md
@ -63,7 +63,7 @@ go build .
|
||||
|
||||
Upload sender and reciever code on the 2 arduinos
|
||||
|
||||
Required library for antenna: [RF24](https://nrf24.github.io/RF24)
|
||||
Required library for antenna: [RF24](https://nrf24.github.io/RF24). Installation via Arduino-IDE.
|
||||
|
||||
## Overview
|
||||
|
||||
@ -71,6 +71,7 @@ Required library for antenna: [RF24](https://nrf24.github.io/RF24)
|
||||
`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 (pc)
|
||||
`testing/` - other tools and applications
|
||||
|
||||
### Schema
|
||||
|
||||
@ -93,12 +94,12 @@ Required library for antenna: [RF24](https://nrf24.github.io/RF24)
|
||||
|
||||
| Identifier | Message Code | Value | Verificator |
|
||||
| ---------- | ------------ | -------------------------------- | ------------|
|
||||
| $ | **1**; | temperature [degrees of Celsius] | * |
|
||||
| $ | **2**; | pressure | * |
|
||||
| $ | **3**; | altitude | * |
|
||||
| $ | **4**; | roll | * |
|
||||
| $ | **5**; | pitch | * |
|
||||
| $ | **6**; | yaw | * |
|
||||
| $ | **1**; | roll | * |
|
||||
| $ | **2**; | pitch | * |
|
||||
| $ | **3**; | yaw | * |
|
||||
| $ | **4**; | temperature [degrees of Celsius] | * |
|
||||
| $ | **5**; | pressure | * |
|
||||
| $ | **6**; | altitude | * |
|
||||
| $ | **7**; | gyroscope x | * |
|
||||
| $ | **8**; | gyroscope y | * |
|
||||
| $ | **9**; | gyroscope z | * |
|
||||
@ -188,10 +189,12 @@ $GPGAA,HHMMSS.SS,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx
|
||||
|
||||
- [x] data stops being transmitted from sender after some short period time
|
||||
- [ ] create a version which will send data via **IOT 433MHz LoRa LPWAN SX1278**
|
||||
- [ ] create a communication in both ways, `start`, `stop`, `system health check` commands
|
||||
- [ ] detection of apogeum and recovery system launch
|
||||
|
||||
### Monitor app issues
|
||||
|
||||
- [ ] application crash after some period of time
|
||||
- [ ] application crash after some period of time, if don't recive any input from serial ports
|
||||
- [ ] gui is not updating until it recieves serial input
|
||||
- [ ] parser should be improved
|
||||
- [ ] sender code should be improved
|
||||
@ -199,6 +202,7 @@ $GPGAA,HHMMSS.SS,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx
|
||||
- [ ] error messages as windows not terminal
|
||||
- [ ] improve readability of code
|
||||
- [ ] serial monitor setup port and baudrate
|
||||
- [ ] create a gui way of sending commands
|
||||
|
||||
## Sources
|
||||
|
||||
|
||||
@ -77,14 +77,14 @@ func run() {
|
||||
|
||||
// Update information if it is in the parsed block
|
||||
if _, ok := info[1]; ok {
|
||||
temperature_gui = info[1]
|
||||
temperature_gui = info[4]
|
||||
}
|
||||
|
||||
win.Clear(colornames.Black)
|
||||
|
||||
// Print information to text blocks
|
||||
temperature.WriteString("Temperature: " + temperature_gui)
|
||||
logging_serial.WriteString(data)
|
||||
temperature.WriteString("Temperature: " + temperature_gui)
|
||||
|
||||
// Draw information to screen
|
||||
logging_serial.Draw(win, pixel.IM)
|
||||
|
||||
@ -3,18 +3,23 @@
|
||||
#include <RF24.h>
|
||||
RF24 radio(9, 8); // CE, CSN
|
||||
const byte address[6] = "00001"; //address through which two modules communicate
|
||||
|
||||
|
||||
void setup() {
|
||||
while (!Serial)
|
||||
;
|
||||
Serial.begin(9600);
|
||||
Serial.println("# Reciever Init");
|
||||
Serial.println("#200 Reciever Init");
|
||||
radio.begin();
|
||||
radio.openReadingPipe(0, address); //set the address
|
||||
radio.startListening(); //Set module as receiver
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
//Read the data if available in buffer
|
||||
if (radio.available()) {
|
||||
Serial.println("#200 Communication is available");
|
||||
char text[64] = { 0 };
|
||||
radio.read(&text, sizeof(text));
|
||||
Serial.println(text);
|
||||
|
||||
@ -2,41 +2,28 @@
|
||||
#include <nRF24L01.h>
|
||||
#include <RF24.h>
|
||||
#include "Waveshare_10Dof-D.h"
|
||||
#include "string.h"
|
||||
|
||||
|
||||
bool gbSenserConnectState = false;
|
||||
//create an RF24 objetct
|
||||
RF24 radio(9, 8); // CE, CSN
|
||||
const byte address[6] = "00001"; // address through which two modules communicate
|
||||
|
||||
void setup() {
|
||||
// init radio
|
||||
radio.begin();
|
||||
radio.openWritingPipe(address);
|
||||
radio.stopListening(); // Set module as transmitter
|
||||
|
||||
// init motion sensor
|
||||
bool bRet;
|
||||
IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType;
|
||||
Serial.begin(115200);
|
||||
|
||||
imuInit(&enMotionSensorType, &enPressureType);
|
||||
if(IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType)
|
||||
{
|
||||
Serial.println("# Motion sersor is ICM-20948");
|
||||
if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) {
|
||||
Serial.println("# Motion sensor is ICM-20948");
|
||||
} else {
|
||||
Serial.println("# Motion sensor NULL");
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.println("# Motion sersor NULL");
|
||||
}
|
||||
if(IMU_EN_SENSOR_TYPE_BMP280 == enPressureType)
|
||||
{
|
||||
Serial.println("# Pressure sersor is BMP280");
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.println("# Pressure sersor NULL");
|
||||
if (IMU_EN_SENSOR_TYPE_BMP280 == enPressureType) {
|
||||
Serial.println("# Pressure sensor is BMP280");
|
||||
} else {
|
||||
Serial.println("# Pressure sensor NULL");
|
||||
}
|
||||
delay(1000);
|
||||
}
|
||||
@ -51,103 +38,38 @@ void loop() {
|
||||
imuDataGet(&stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
|
||||
pressSensorDataGet(&s32TemperatureVal, &s32PressureVal, &s32AltitudeVal);
|
||||
|
||||
char temp_str[8], pressure_str[8], altitude_str[8];
|
||||
char roll_str[8], pitch_str[8], yaw_str[8];
|
||||
char gyro_x_str[8], gyro_y_str[8], gyro_z_str[8];
|
||||
char accel_x_str[8], accel_y_str[8], accel_z_str[8];
|
||||
char magn_x_str[8], magn_y_str[8], magn_z_str[8];
|
||||
float temperature = s32TemperatureVal / 100.0;
|
||||
float pressure = s32PressureVal / 100.0;
|
||||
float altitude = s32AltitudeVal / 100.0;
|
||||
|
||||
float temperature = (s32TemperatureVal / 100);
|
||||
float pressure = (s32PressureVal / 100);
|
||||
float altitude = (s32AltitudeVal / 100);
|
||||
|
||||
float roll = stAngles.fRoll;
|
||||
float pitch = stAngles.fPitch;
|
||||
float yaw = stAngles.fYaw;
|
||||
|
||||
float gyro_x = stGyroRawData.s16X;
|
||||
float gyro_y = stGyroRawData.s16Y;
|
||||
float gyro_z = stGyroRawData.s16Z;
|
||||
|
||||
float accel_x = stAccelRawData.s16X;
|
||||
float accel_y = stAccelRawData.s16Y;
|
||||
float accel_z = stAccelRawData.s16Z;
|
||||
|
||||
float magn_x = stMagnRawData.s16X;
|
||||
float magn_y = stMagnRawData.s16Y;
|
||||
float magn_z = stMagnRawData.s16Z;
|
||||
|
||||
dtostrf(temperature, 6, 2, temp_str);
|
||||
dtostrf(pressure, 6, 2, pressure_str);
|
||||
dtostrf(altitude, 6, 2, altitude_str);
|
||||
|
||||
dtostrf(roll, 6, 2, roll_str);
|
||||
dtostrf(pitch, 6, 2, pitch_str);
|
||||
dtostrf(yaw, 6, 2, yaw_str);
|
||||
|
||||
dtostrf(gyro_x, 6, 2, gyro_x_str);
|
||||
dtostrf(gyro_y, 6, 2, gyro_y_str);
|
||||
dtostrf(gyro_z, 6, 2, gyro_z_str);
|
||||
|
||||
dtostrf(accel_x, 6, 2, accel_x_str);
|
||||
dtostrf(accel_y, 6, 2, accel_y_str);
|
||||
dtostrf(accel_z, 6, 2, accel_z_str);
|
||||
|
||||
dtostrf(magn_x, 6, 2, magn_x_str);
|
||||
dtostrf(magn_y, 6, 2, magn_y_str);
|
||||
dtostrf(magn_z, 6, 2, magn_z_str);
|
||||
|
||||
//Serial.println(roll, pitch, yaw);
|
||||
//Serial.println(temperature, pressure, altitude);
|
||||
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];
|
||||
|
||||
String ctemp = ("$1;"+String(temp_str)+"*");
|
||||
String cpressure = ("$2;"+String(pressure_str)+"*");
|
||||
String caltitude = ("$3;"+String(altitude_str)+"*");
|
||||
String croll = ("$4;"+String(roll_str)+"*");
|
||||
String cpitch = ("$5;"+String(pitch_str)+"*");
|
||||
String cyaw = ("$6;"+String(yaw_str) + "*");
|
||||
String cgyro_x = ("$7;"+String(gyro_x_str)+"*");
|
||||
String cgyro_y = ("$8;"+String(gyro_y_str)+"*");
|
||||
String cgyro_z = ("$9;"+String(gyro_z_str)+"*");
|
||||
String caccel_x = ("$10;"+String(accel_x_str)+"*");
|
||||
String caccel_y = ("$11;"+String(accel_y_str)+"*");
|
||||
String caccel_z = ("$12;"+String(accel_z_str)+"*");
|
||||
String cmagn_x = ("$13;"+String(magn_x_str)+"*");
|
||||
String cmagn_y = ("$14;"+String(magn_y_str)+"*");
|
||||
String cmagn_z = ("$15;"+String(magn_z_str)+"*");
|
||||
|
||||
ctemp.toCharArray(msg, sizeof(msg));
|
||||
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));
|
||||
cpressure.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
caltitude.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
croll.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cpitch.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cyaw.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cgyro_x.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cgyro_y.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cgyro_z.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
caccel_x.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
caccel_y.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
caccel_z.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cmagn_x.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cmagn_y.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
cmagn_z.toCharArray(msg, sizeof(msg));
|
||||
radio.write(&msg, sizeof(msg));
|
||||
|
||||
//delay(200);
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,743 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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.
|
||||
*
|
||||
* <h2><center>© COPYRIGHT 2018 Waveshare</center></h2>
|
||||
******************************************************************************
|
||||
*/
|
||||
#include "Waveshare_10Dof-D.h"
|
||||
#include <Wire.h>
|
||||
|
||||
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<u8Len; i++)
|
||||
{
|
||||
*(pu8data+i) = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_EXT_SENS_DATA_00+i);
|
||||
|
||||
}
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3
|
||||
|
||||
u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_I2C_SLV0_CTRL);
|
||||
u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN)&(REG_VAL_BIT_MASK_LEN));
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, u8Temp);
|
||||
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0
|
||||
|
||||
}
|
||||
|
||||
void icm20948WriteSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8data)
|
||||
{
|
||||
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_SLV1_ADDR, u8I2CAddr);
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_REG, u8RegAddr);
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_DO, u8data);
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_CTRL, REG_VAL_BIT_SLV0_EN|1);
|
||||
|
||||
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);
|
||||
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3
|
||||
|
||||
u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948,REG_ADD_I2C_SLV0_CTRL);
|
||||
u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN)&(REG_VAL_BIT_MASK_LEN));
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, u8Temp);
|
||||
|
||||
I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void icm20948CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_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 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; /*<calibration T1 data*/
|
||||
int16_t T2; /*<calibration T2 data*/
|
||||
int16_t T3; /*<calibration T3 data*/
|
||||
uint16_t P1; /*<calibration P1 data*/
|
||||
int16_t P2; /*<calibration P2 data*/
|
||||
int16_t P3; /*<calibration P3 data*/
|
||||
int16_t P4; /*<calibration P4 data*/
|
||||
int16_t P5; /*<calibration P5 data*/
|
||||
int16_t P6; /*<calibration P6 data*/
|
||||
int16_t P7; /*<calibration P7 data*/
|
||||
int16_t P8; /*<calibration P8 data*/
|
||||
int16_t P9; /*<calibration P9 data*/
|
||||
int32_t T_fine; /*<calibration t_fine data*/
|
||||
}BMP280_HandleTypeDef;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t Index;
|
||||
int32_t AvgBuffer[8];
|
||||
}BMP280_AvgTypeDef;
|
||||
|
||||
#define dig_T1 bmp280.T1
|
||||
#define dig_T2 bmp280.T2
|
||||
#define dig_T3 bmp280.T3
|
||||
#define dig_P1 bmp280.P1
|
||||
#define dig_P2 bmp280.P2
|
||||
#define dig_P3 bmp280.P3
|
||||
#define dig_P4 bmp280.P4
|
||||
#define dig_P5 bmp280.P5
|
||||
#define dig_P6 bmp280.P6
|
||||
#define dig_P7 bmp280.P7
|
||||
#define dig_P8 bmp280.P8
|
||||
#define dig_P9 bmp280.P9
|
||||
#define t_fine bmp280.T_fine
|
||||
|
||||
#define MSLP 101325 // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
|
||||
|
||||
BMP280_HandleTypeDef bmp280;
|
||||
int32_t gs32Pressure0 = MSLP;
|
||||
|
||||
bool bmp280Check(void)
|
||||
{
|
||||
bool bRet = false;
|
||||
if(0x58 == I2C_ReadOneByte(BMP280_ADDR, BMP280_REGISTER_CHIPID))
|
||||
{
|
||||
bRet = true;
|
||||
}
|
||||
return bRet;
|
||||
}
|
||||
|
||||
void bmp280ReadCalibration(void)
|
||||
{
|
||||
uint8_t lsb, msb;
|
||||
|
||||
/* read the temperature calibration parameters */
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T1_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T1_MSB_REG);
|
||||
dig_T1 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T2_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T2_MSB_REG);
|
||||
dig_T2 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T3_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_T3_MSB_REG);
|
||||
dig_T3 = msb << 8 | lsb;
|
||||
|
||||
/* read the pressure calibration parameters */
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P1_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P1_MSB_REG);
|
||||
dig_P1 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P2_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P2_MSB_REG);
|
||||
dig_P2 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P3_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P3_MSB_REG);
|
||||
dig_P3 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P4_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P4_MSB_REG);
|
||||
dig_P4 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P5_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P5_MSB_REG);
|
||||
dig_P5 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P6_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P6_MSB_REG);
|
||||
dig_P6 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P7_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P7_MSB_REG);
|
||||
dig_P7 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P8_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P8_MSB_REG);
|
||||
dig_P8 = msb << 8 | lsb;
|
||||
lsb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P9_LSB_REG);
|
||||
msb = I2C_ReadOneByte(BMP280_ADDR, BMP280_DIG_P9_MSB_REG);
|
||||
dig_P9 = msb << 8 | lsb;
|
||||
|
||||
}
|
||||
|
||||
void bmp280Init(void)
|
||||
{
|
||||
I2C_WriteOneByte(BMP280_ADDR, BMP280_REGISTER_CONTROL, 0xFF);
|
||||
I2C_WriteOneByte(BMP280_ADDR, BMP280_REGISTER_CONFIG, 0x14);
|
||||
bmp280ReadCalibration();
|
||||
}
|
||||
|
||||
float bmp280CompensateTemperature(int32_t adc_T)
|
||||
{
|
||||
int64_t var1, var2, temperature;
|
||||
|
||||
var1 = ((((adc_T>>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
|
||||
@ -1,236 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @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.
|
||||
*
|
||||
* <h2><center>© COPYRIGHT 2018 Waveshare</center></h2>
|
||||
******************************************************************************
|
||||
*/
|
||||
#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__
|
||||
@ -1,75 +0,0 @@
|
||||
#include <SPI.h>
|
||||
#include <nRF24L01.h>
|
||||
#include <RF24.h>
|
||||
#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));
|
||||
}
|
||||
}
|
||||
}
|
||||
3
testing/gui_app_test/README.md
Normal file
3
testing/gui_app_test/README.md
Normal file
@ -0,0 +1,3 @@
|
||||
# gui_app_test
|
||||
|
||||
- preview of the gui app without connection to arduino
|
||||
20
testing/gui_app_test/go.mod
Normal file
20
testing/gui_app_test/go.mod
Normal file
@ -0,0 +1,20 @@
|
||||
module test/test
|
||||
|
||||
go 1.21
|
||||
|
||||
toolchain go1.22.1
|
||||
|
||||
require (
|
||||
github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0
|
||||
github.com/gopxl/pixel v1.0.0
|
||||
golang.org/x/image v0.15.0
|
||||
)
|
||||
|
||||
require (
|
||||
github.com/faiface/glhf v0.0.0-20211013000516-57b20770c369 // indirect
|
||||
github.com/faiface/mainthread v0.0.0-20171120011319-8b78f0a41ae3 // indirect
|
||||
github.com/go-gl/gl v0.0.0-20211210172815-726fda9656d6 // indirect
|
||||
github.com/go-gl/glfw/v3.3/glfw v0.0.0-20221017161538-93cebf72946b // indirect
|
||||
github.com/go-gl/mathgl v1.1.0 // indirect
|
||||
github.com/pkg/errors v0.9.1 // indirect
|
||||
)
|
||||
31
testing/gui_app_test/go.sum
Normal file
31
testing/gui_app_test/go.sum
Normal file
@ -0,0 +1,31 @@
|
||||
github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c=
|
||||
github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
|
||||
github.com/faiface/glhf v0.0.0-20211013000516-57b20770c369 h1:gv4BgP50atccdK/1tZHDyP6rMwiiutR2HPreR/OyLzI=
|
||||
github.com/faiface/glhf v0.0.0-20211013000516-57b20770c369/go.mod h1:dDdUO+G9ZnJ9sc8nIUvhLkE45k8PEKW6+A3TdWsfpV0=
|
||||
github.com/faiface/mainthread v0.0.0-20171120011319-8b78f0a41ae3 h1:baVdMKlASEHrj19iqjARrPbaRisD7EuZEVJj6ZMLl1Q=
|
||||
github.com/faiface/mainthread v0.0.0-20171120011319-8b78f0a41ae3/go.mod h1:VEPNJUlxl5KdWjDvz6Q1l+rJlxF2i6xqDeGuGAxa87M=
|
||||
github.com/go-gl/gl v0.0.0-20210905235341-f7a045908259/go.mod h1:wjpnOv6ONl2SuJSxqCPVaPZibGFdSci9HFocT9qtVYM=
|
||||
github.com/go-gl/gl v0.0.0-20211210172815-726fda9656d6 h1:zDw5v7qm4yH7N8C8uWd+8Ii9rROdgWxQuGoJ9WDXxfk=
|
||||
github.com/go-gl/gl v0.0.0-20211210172815-726fda9656d6/go.mod h1:9YTyiznxEY1fVinfM7RvRcjRHbw2xLBJ3AAGIT0I4Nw=
|
||||
github.com/go-gl/glfw v0.0.0-20210727001814-0db043d8d5be/go.mod h1:vR7hzQXu2zJy9AVAgeJqvqgH9Q5CA+iKCZ2gyEVpxRU=
|
||||
github.com/go-gl/glfw/v3.3/glfw v0.0.0-20221017161538-93cebf72946b h1:GgabKamyOYguHqHjSkDACcgoPIz3w0Dis/zJ1wyHHHU=
|
||||
github.com/go-gl/glfw/v3.3/glfw v0.0.0-20221017161538-93cebf72946b/go.mod h1:tQ2UAYgL5IevRw8kRxooKSPJfGvJ9fJQFa0TUsXzTg8=
|
||||
github.com/go-gl/mathgl v1.0.0/go.mod h1:yhpkQzEiH9yPyxDUGzkmgScbaBVlhC06qodikEM0ZwQ=
|
||||
github.com/go-gl/mathgl v1.1.0 h1:0lzZ+rntPX3/oGrDzYGdowSLC2ky8Osirvf5uAwfIEA=
|
||||
github.com/go-gl/mathgl v1.1.0/go.mod h1:yhpkQzEiH9yPyxDUGzkmgScbaBVlhC06qodikEM0ZwQ=
|
||||
github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0 h1:DACJavvAHhabrF08vX0COfcOBJRhZ8lUbR+ZWIs0Y5g=
|
||||
github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0/go.mod h1:E/TSTwGwJL78qG/PmXZO1EjYhfJinVAhrmmHX6Z8B9k=
|
||||
github.com/gopxl/pixel v1.0.0 h1:ZON6ll6/tI6sO8fwrlj93GVUcXReTST5//iKv6lcd8g=
|
||||
github.com/gopxl/pixel v1.0.0/go.mod h1:kPUBG2He7/+alwmi5z0IwnpAc6pw2N7eA08cdBfoE/Q=
|
||||
github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4=
|
||||
github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0=
|
||||
github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM=
|
||||
github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4=
|
||||
github.com/stretchr/testify v1.8.4 h1:CcVxjf3Q8PM0mHUKJCdn+eZZtm5yQwehR5yeSVQQcUk=
|
||||
github.com/stretchr/testify v1.8.4/go.mod h1:sz/lmYIOXD/1dqDmKjjqLyZ2RngseejIcXlSw2iwfAo=
|
||||
golang.org/x/image v0.0.0-20190321063152-3fc05d484e9f/go.mod h1:kZ7UVZpmo3dzQBMxlp+ypCbDeSB+sBbTgSJuh5dn5js=
|
||||
golang.org/x/image v0.15.0 h1:kOELfmgrmJlw4Cdb7g/QGuB3CvDrXbqEIww/pNtNBm8=
|
||||
golang.org/x/image v0.15.0/go.mod h1:HUYqC05R2ZcZ3ejNQsIHQDQiwWM4JBqmm6MKANTp4LE=
|
||||
golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ=
|
||||
gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA=
|
||||
gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM=
|
||||
BIN
testing/gui_app_test/intuitive.ttf
Normal file
BIN
testing/gui_app_test/intuitive.ttf
Normal file
Binary file not shown.
78
testing/gui_app_test/main.go
Normal file
78
testing/gui_app_test/main.go
Normal file
@ -0,0 +1,78 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"io"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
"github.com/golang/freetype/truetype"
|
||||
"github.com/gopxl/pixel"
|
||||
"github.com/gopxl/pixel/pixelgl"
|
||||
"github.com/gopxl/pixel/text"
|
||||
"golang.org/x/image/colornames"
|
||||
"golang.org/x/image/font"
|
||||
)
|
||||
|
||||
func loadTTF(path string, size float64) (font.Face, error) {
|
||||
file, err := os.Open(path)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
defer file.Close()
|
||||
|
||||
bytes, err := io.ReadAll(file)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
font, err := truetype.Parse(bytes)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return truetype.NewFace(font, &truetype.Options{
|
||||
Size: size,
|
||||
GlyphCacheEntries: 1,
|
||||
}), nil
|
||||
}
|
||||
|
||||
func run() {
|
||||
cfg := pixelgl.WindowConfig{
|
||||
Title: "Cobra Monitor",
|
||||
Bounds: pixel.R(0, 0, 1024, 768),
|
||||
}
|
||||
win, err := pixelgl.NewWindow(cfg)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
win.SetSmooth(true)
|
||||
|
||||
face, err := loadTTF("intuitive.ttf", 80)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
|
||||
atlas := text.NewAtlas(face, text.ASCII)
|
||||
txt := text.New(pixel.V(50, 500), atlas)
|
||||
|
||||
txt.Color = colornames.Lightgrey
|
||||
|
||||
fps := time.Tick(time.Second / 120)
|
||||
|
||||
for !win.Closed() {
|
||||
txt.WriteString(win.Typed())
|
||||
if win.JustPressed(pixelgl.KeyEnter) || win.Repeated(pixelgl.KeyEnter) {
|
||||
txt.WriteRune('\n')
|
||||
}
|
||||
|
||||
win.Clear(colornames.Darkcyan)
|
||||
txt.Draw(win, pixel.IM.Moved(win.Bounds().Center().Sub(txt.Bounds().Center())))
|
||||
win.Update()
|
||||
|
||||
<-fps
|
||||
}
|
||||
}
|
||||
|
||||
func main() {
|
||||
pixelgl.Run(run)
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user