65 lines
1.7 KiB
C++
65 lines
1.7 KiB
C++
#include "headerFile.h"
|
|
|
|
SensorManager sensorManager;
|
|
ESP8266WiFiMulti wifi;
|
|
WebSocketsClient webSocket;
|
|
#define USE_SERIAL Serial
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
Serial.println("startup");
|
|
delay(5000);
|
|
connectWiFi();
|
|
sensorManager.sensorSetup();
|
|
websocketSetup();
|
|
}
|
|
|
|
void loop() {
|
|
SensorManager::Rotation rotation = sensorManager.readLoop();
|
|
|
|
// Convert quaternion to Euler angles in radians
|
|
float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
|
|
float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
|
|
float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
|
|
|
|
// Convert to degrees
|
|
float rollDegrees = roll * 180.0f / PI;
|
|
float pitchDegrees = pitch * 180.0f / PI;
|
|
float yawDegrees = yaw * 180.0f / PI;
|
|
|
|
Serial.print(roll);
|
|
Serial.print(" ");
|
|
Serial.print(pitch);
|
|
Serial.print(" ");
|
|
Serial.print(yaw);
|
|
sendData(roll, pitch, yaw);
|
|
|
|
Serial.println();
|
|
webSocket.loop();
|
|
}
|
|
|
|
|
|
void connectWiFi(){
|
|
|
|
WiFi.mode(WIFI_STA);
|
|
wifi.addAP(ssid, pass);
|
|
WiFi.begin();
|
|
while (WiFi.status() != WL_CONNECTED) {
|
|
Serial.println("connecting to wifi");
|
|
delay(1000);
|
|
}
|
|
Serial.println(WiFi.localIP());
|
|
|
|
}
|
|
|
|
void websocketSetup(){
|
|
//ws server address, port and URL
|
|
webSocket.begin("192.168.178.118", 8001, "");
|
|
// try every 500 again if connection has failed
|
|
webSocket.setReconnectInterval(500);
|
|
}
|
|
|
|
void sendData(float roll, float pitch, float yaw){
|
|
String message = "{\"Sensor\": 1, \"roll\":\"" + String(roll) + "\",\"pitch\":\"" + String(pitch) + "\",\"yaw\":\"" + String(yaw) + "\"}";
|
|
webSocket.sendTXT(message);
|
|
} |