Code:
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include "MPU9250.h"
const char* ssid = "OpenPlotter";
const char* password = "*******";
const uint16_t SK_port = 55557;
const char * SK_host = "10.10.10.1"; // RPI 3 with OpenPlotter
const int Sensor = D3;
int status;
char val;
MPU9250 IMU(Wire,0x68);
WiFiUDP Udp;
double roll , pitch, yaw;
void setup() {
Serial.begin(4800);
Wire.begin(D1, D2);
pinMode (Sensor, INPUT) ; // Initialization sensor pin
delay(10);
connectToWifi();
setIMUtoRead();
setCalibrationIMU();
Serial.println("Status: End Setup");
} /* End setup */
void loop() {
IMU.readSensor();
calibrationDisplay();
//------------------------------------------------------------
float accelX = IMU.getAccelX_mss();
float accelY = IMU.getAccelY_mss();
float accelZ = IMU.getAccelZ_mss();
float gyroX = IMU.getGyroX_rads()/57.3;
float gyroY = IMU.getGyroY_rads()/57.3;
float gyroZ = IMU.getGyroZ_rads()/57.3;
float magX = IMU.getMagX_uT();
float magY = IMU.getMagY_uT();
float magZ = IMU.getMagZ_uT();
pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ))));
roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ))));
// yaw from mag
float Yh = (magY * cos(roll)) - (magZ * sin(roll));
float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch));
yaw = atan2(Yh, Xh);
roll = roll*57.3;
pitch = pitch*57.3;
yaw = yaw*57.3; // in degrees /Angle relative to north
if (yaw < 0.1){
yaw = yaw + 360; // in degrees /relative to north
}
// Serial.print("pitch: " + String( pitch) );
// Serial.print("\t");
// Serial.print("roll: " + String( roll));
// Serial.print("\t");
// Serial.println("yaw: " + String( yaw ));
float heading = (yaw * 71) / 4068.0; // in radians /relative to north
float temperatuur = IMU.getTemperature_C() +271.15; // in kelvin //-2 calibration
sendMessageFloat("NodeNCU_Temp","environment.outside.temperature",temperatuur);
sendMessageFloat("Windvane","environment.wind.angleApparent",heading);
sendMessageFloat("Windvane","environment.wind.directionMagnetic",heading);
Serial.println("wait 1 sec...");
delay(1000);
//Serial.println("");
} /* End infinite loop */
void sendMessageFloat(String source, String path, float message){
String cmd;
cmd = "{\"updates\":[";
cmd += "{\"$source\": \""; cmd += source; cmd += "\",\"values\":[ {\"path\":\""; cmd += path; cmd +="\",\"value\":";
cmd += message; cmd += "}]}";
cmd += "]}\0";
char cmd3[cmd.length() + 1];
strncpy(cmd3, cmd.c_str(), sizeof(cmd3));
Udp.beginPacket(SK_host, SK_port);
Udp.write(cmd3);
Udp.endPacket();
Serial.println(cmd3); Serial.print(" Message length is: "); Serial.println(sizeof(cmd3));
}
void setCalibrationIMU(){
float hxb = 9.73;
float hxs = 1.02;
float hyb = 0.81;
float hys = 0.96;
float hzb = -19.53;
float hzs = 1.03;
float azb = 0.00;
float azs = 1.00;
float ayb = 0.00;
float ays = 1.00;
float axb = 0.00;
float axs = 0.00;
float gzb = 0.03;
float gyb = 0.01;
float gxb = -0.01;
IMU.setGyroBiasX_rads(gxb);
IMU.setGyroBiasY_rads(gyb);
IMU.setGyroBiasZ_rads(gzb);
IMU.setAccelCalX(axb,axs);
IMU.setAccelCalY(ayb,ays);
IMU.setAccelCalZ(azb,azs);
IMU.setMagCalX(hxb,hxs);
IMU.setMagCalY(hyb,hys);
IMU.setMagCalZ(hzb,hzs);
}
void connectToWifi(){
// Connect to WiFi network
Serial.println();
Serial.print("Connecting to ");
Serial.println(ssid);
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("");
Serial.println("WiFi connected");
Serial.println("IP address: ");
Serial.println(WiFi.localIP());
delay(500);
Udp.begin(SK_port);
}
void setIMUtoRead(){
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("IMU-ERROR");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_10HZ);
IMU.setSrd(25);
// Calibration of MPU
//Serial.println("Status: set stuff");
//Serial.println("Status: calibrate mag");
//Serial.println("Status: tail 8");
//status = IMU.calibrateMag();
//Serial.println("Status: Done");
//delay(10000);
//
//Serial.println("Status: calibrate Accel ");
//status = IMU.calibrateAccel();
//Serial.println("Status: calibratie Gyro");
//status = IMU.calibrateGyro();
//Serial.println("Status: done");
//Setting Values
}Then NMEA converter of SignalK is doing its magic to show NMEA in OpenCPN.
This link shows the scenteces OpenCPN accepts. OpenCPN dashboard accepts MWD and MWV.
This link could be a good reference for the calculations, I havent read it totaly I was searching for this link it shows the description of AWA and others but AWA is relative to the bow. So to have the right information shown in OpenCPN there is some calculations to be made. Or I have it totaly wrong ;-)