Posts: 9
Threads: 1
Joined: Sep 2018
Reputation:
1
2018-09-24, 03:37 PM
(This post was last modified: 2018-09-24, 03:40 PM by KolonelP.)
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
}
Here is my code. The difference is the port I send data to 55557
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 ;-)
Posts: 1,162
Threads: 98
Joined: Mar 2016
Reputation:
69
(2018-09-24, 03:37 PM)KolonelP Wrote: 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 ;-)
Nice 
One thought, for sending signalK the arduinoJson library might be worth having a look at - a subroutine below works for me on a ep8266 & 32 - but the beta versions didn't compile on a 32, needed to roll back to an older version.
Code: #include <ArduinoJson.h>
void sendSigK(String sigKey, float data) { // send SigK via UDP *****************************************
DynamicJsonBuffer jsonBuffer;
String deltaText;
// build delta message
JsonObject& delta = jsonBuffer.createObject();
//updated array
JsonArray& updatesArr = delta.createNestedArray("updates");
JsonObject& thisUpdate = updatesArr.createNestedObject(); //Json Object nested inside delta [...
JsonArray& values = thisUpdate.createNestedArray("values"); // Values array nested in delta[ values....
JsonObject& thisValue = values.createNestedObject();
thisValue["path"] = sigKey;
thisValue["value"] = data;
thisUpdate["Source"] = "ESP11";
// Send UDP packet
Udp.beginPacket(remoteIp, remotePort);
delta.printTo(Udp);
Udp.println();
Udp.endPacket();
Serial.print("UDP sent. remoteIP = ");
Serial.println(remoteIp);
Serial.print("remote port = ");
Serial.println(remotePort);
}
Posts: 9
Threads: 1
Joined: Sep 2018
Reputation:
1
Works like a charm!! I used this lib. and your code (thnx!!) :
https://github.com/bblanchon/ArduinoJson
Posts: 1,162
Threads: 98
Joined: Mar 2016
Reputation:
69
(2018-09-24, 09:57 PM)KolonelP Wrote: Works like a charm!! I used this lib. and your code (thnx!!) :
https://github.com/bblanchon/ArduinoJson
Update with this, on an ESP32 I've just spent ages wondering why it didn't work every time - lots of UDP messages seemed to get lost.
Putting in a delay(10); after Udp.endPacket(); solved it, seems rock solid now.
Posts: 2,179
Threads: 20
Joined: Jun 2016
Reputation:
64
(2018-09-16, 01:04 PM)KolonelP Wrote: Hello there
First, I introduce myself, I am a sailor on a low budget ;-)
Thanks you, developpers of OpenPlotter!!
What I would like to do is the usage of an second MPU for wind-vane directions.
It's not a good usage of this sensor for wind vane directions because the absolute angle difference between the two mpu is not constant due to sensor inaccuracy. Maybe it could at least work for relative changes which might be good enough for steering to wind, but how do you connect the mpu so it can rotate?
Why not just use a hall sensor which cost even less and is much more accurate and simple?
Quote:ano /home/pi/kts/RTIMULib2/RTIMULib/IMUDrivers/RTIMUDefs.h[/code]
edit line 151,152,153
Code: #define MPU9250_ADDRESS0 0x69 //0x68
#define MPU9250_ADDRESS1 0x68 //0x69
#define MPU9250_ID 0x73 //0x71
It's great you set up two sensors. This isn't supported by openplotter normally, but eventually it would be interesting to support 2 or more inertial sensors for redundancy, lower noise, and better automatic calibration.
|
