84 lines
2.6 KiB
C++
84 lines
2.6 KiB
C++
#include <SPI.h>
|
|
#include <nRF24L01.h>
|
|
#include <RF24.h>
|
|
#include "Waveshare_10Dof-D.h"
|
|
|
|
//----------------------- CONSTANTS -----------------------//
|
|
bool gbSenserConnectState = false;
|
|
RF24 radio(9, 8); // CE, CSN
|
|
const byte address[6] = "00001"; // address through which two modules communicate
|
|
|
|
//----------------------- SETUP -----------------------//
|
|
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);
|
|
}
|
|
|
|
//----------------------- LOOP -----------------------//
|
|
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];
|
|
|
|
// Send angles data
|
|
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));
|
|
}
|
|
|
|
// Send other sensor data
|
|
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]}
|
|
};
|
|
|
|
int index = 4;
|
|
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(index) + ";" + String(float_str) + "*";
|
|
str.toCharArray(msg, sizeof(msg));
|
|
radio.write(&msg, sizeof(msg));
|
|
index++;
|
|
}
|
|
}
|
|
}
|