OpenMarine

Full Version: Second MPU for windfane
You're currently viewing a stripped down version of our content. View the full version with proper formatting.
Pages: 1 2
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.
I bought two MPU9250 very cheap. The first one is on address 0x68. By powering de AD0 pin of the second MPU the address changed to 0x69.
Then I needed to read out the device, a challenge. 

First reference:

Code:
https://kingtidesailing.blogspot.com/2016/02/how-to-setup-mpu-9250-on-raspberry-pi_25.html

 
Very good how-to on setting up the MPU. But there are some changes to be made.


Second reference:
Code:
https://kingtidesailing.blogspot.com/2016/04/make-wireless-nmea-0183-multiplexer.html
In addition to the first reference to automate at startup.

Getting the right sentence for opencpn I used this reference:
Code:
http://home.btconnect.com/Amaya/Example%20NMEA%20Sentences.pdf


After cloning
git clone https://github.com/richards-tech/RTIMULib2.git
(so before make,make install)

edit the follwing:
Code:
nano /home/pi/kts/RTIMULib2/RTIMULib/IMUDrivers/RTIMUDefs.h

edit line 151,152,153
Code:
#define MPU9250_ADDRESS0            0x69     //0x68
#define MPU9250_ADDRESS1            0x68     //0x69
#define MPU9250_ID                  0x73     //0x71


Then follow the how-to by (instead of -j4 I used -j2 for RPI):
Code:
cd RTIMULib2/Linux/RTIMULibCal
make -j2
sudo make install

After running RTEllipsoidFit for the first time edit the RTEIMULib.ini file that just is created.
Edit line 20, 35 make sure it is set to 1 for RP3 & 51
Code:
20: IMUType=7
25: I2CBus=1
51: I2CSlaveAddress=104

After this I run the calibration as stated in the how-to.


My monitor,py

Code:
#!/usr/bin/env python

import serial
import operator
import time
import os
import sys
import socket
import select

# change the working directory to the scripts folder
os.chdir("/home/pi/kts/scripts")

# log and begin instrument scripts
log = open('log', 'w')
log.write("Monitor Initialized\r\n")
log.write("Starting IMU...\r\n")
log.close()
os.system("python imu.py &")
os.system("sudo kplex &")

IMU_IP = "127.0.0.5"
IMU_PORT = 5005
imusock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
imusock.bind((IMU_IP, IMU_PORT))

log = open('log', 'a')
log.write("Starting Loop\r\n------------------------\r\n")
log.close()

imu_hack = time.time()

while True:

   # monitor imu.py
   imuready = select.select([imusock], [], [], .1)
   if imuready [0]:
       data, addr = imusock.recvfrom(1024)
       imu_hack = float(data)
   if time.time() - imu_hack > 10.0:
       log = open('log', 'a')
       log.write("Restarting IMU...\r\n")
       log.close()
       os.system("pkill -9 -f imu.py")
       os.system("python imu.py &")
       imu_hack = time.time()

my imu.py
Code:
import sys, getopt  
 
sys.path.append('.')  
import RTIMU  
import os.path  
import time  
import math  
import operator  
import socket  
 
IMU_IP = "127.0.0.2"  
IMU_PORT = 5005  

SETTINGS_FILE = "RTIMULib"  

s = RTIMU.Settings(SETTINGS_FILE)  
imu = RTIMU.RTIMU(s)  

# timers  
t_print = time.time()  
t_damp = time.time()  
t_fail = time.time()  
t_fail_timer = 0.0  
t_shutdown = 0  
 
if (not imu.IMUInit()):  
    hack = time.time()  
    imu_sentence = "$IIXDR,IMU_FAILED_TO_INITIALIZE*7C"  
    if (hack - t_print) > 1.0:  
        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
        sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
        t_print = hack  
        t_shutdown += 1  
        if t_shutdown > 9:  
            sys.exit(1)  

imu.setSlerpPower(0.02)  
imu.setGyroEnable(True)  
imu.setAccelEnable(True)  
imu.setCompassEnable(True)  

poll_interval = imu.IMUGetPollInterval()  

# data variables  
roll = 0.0  
pitch = 0.0  
yaw = 0.0  
heading = 0.0  
rollrate = 0.0  
pitchrate = 0.0  
yawrate = 0.0  
magnetic_deviation = -13.7  


# dampening variables  
t_one = 0  
t_three = 0  
roll_total = 0.0  
roll_run = [0] * 10  
heading_cos_total = 0.0  
heading_sin_total = 0.0  
heading_cos_run = [0] * 30  
heading_sin_run = [0] * 30  

# sentences produces by the imu  
iihdt0 = "$IIHDT,,T*0C"  
iixdr0 = "$IIXDR,A,,D,ROLL,A,,D,PTCH,A,,D,RLLR,A,,D,PTCR,A,,D,YAWR*51"  
iihdt = iihdt0  
iixdr = iixdr0  
freq = 1  

while True:  
    hack = time.time()  

    # if it's been longer than 5 seconds since last print  
    if (hack - t_damp) > 5.0:  
     
        if (hack - t_fail) > 1.0:  
            t_one = 0  
            t_three = 0  
            roll_total = 0.0  
            roll_run = [0] * 10  
            heading_cos_total = 0.0  
            heading_sin_total = 0.0  
            heading_cos_run = [0] * 30  
            heading_sin_run = [0] * 30  
            t_fail_timer += 1  
            imu_sentence = "IIXDR,IMU_FAIL," + str(round(t_fail_timer / 60, 1))  
            cs = format(reduce(operator.xor,map(ord,imu_sentence),0),'X')  
            if len(cs) == 1:  
                cs = "0" + cs  
            imu_sentence = "$" + imu_sentence + "*" + cs  
            sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
            sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
            t_fail = hack  
            t_shutdown += 1  
    
    if imu.IMURead():  
        data = imu.getIMUData()  
        fusionPose = data["fusionPose"]  
        Gyro = data["gyro"]  
        t_fail_timer = 0.0  
 
        if (hack - t_damp) > .1:  
            roll = round(math.degrees(fusionPose[0]), 1)  
            pitch = round(math.degrees(fusionPose[1]), 1)  
            yaw = round(math.degrees(fusionPose[2]), 1)  
            rollrate = round(math.degrees(Gyro[0]), 1)  
            pitchrate = round(math.degrees(Gyro[1]), 1)  
            yawrate = round(math.degrees(Gyro[2]), 1)  
            if yaw < 90.1:  
                heading = yaw + 270 - magnetic_deviation  
            else:  
                heading = yaw - 90 - magnetic_deviation  
            if heading > 360.0:  
                heading = heading - 360.0  
       
            # Dampening functions  
            roll_total = roll_total - roll_run[t_one]  
            roll_run[t_one] = roll  
            roll_total = roll_total + roll_run[t_one]  
            roll = roll_total / 10  
            heading_cos_total = heading_cos_total - heading_cos_run[t_three]  
            heading_sin_total = heading_sin_total - heading_sin_run[t_three]  
            heading_cos_run[t_three] = math.cos(math.radians(heading))  
            heading_sin_run[t_three] = math.sin(math.radians(heading))  
            heading_cos_total = heading_cos_total + heading_cos_run[t_three]  
            heading_sin_total = heading_sin_total + heading_sin_run[t_three]  
            heading = round(math.degrees(math.atan2(heading_sin_total/30,heading_cos_total/30)),1)  
            if heading < 0.1:  
                heading = heading + 360.0  
 
            t_damp = hack  
            t_one += 1  
            if t_one == 10:  
                t_one = 0  
            t_three += 1  
            if t_three == 30:  
                t_three = 0  

            if (hack - t_print) > 1:  
                hdt = "IIMWV," + str(round(heading))[:-2] + ",R,,N,A"  
                hdtcs = format(reduce(operator.xor,map(ord,hdt),0),'X')  
                if len(hdtcs) == 1:  
                    hdtcs = "0" + hdtcs  
                iihdt = "$" + hdt + "*" + hdtcs  
     
                xdr = "IIXDR,A," + str(int(round(roll))) + ",D,ROLL,A," + str(int(round(pitch))) + ",D,PTCH,A," + str(int(round(rollrate))) + ",D,RLLR,A," + str(int(round(pitchrate))) + ",D,PTCR,A," + str(int(round(yawrate))) + ",D,YAWR"  
                xdrcs = format(reduce(operator.xor,map(ord,xdr),0),'X')  
                if len(xdrcs) == 1:  
                    xdrcs = "0" + xdrcs  
                iixdr = "$" + xdr + "*" + xdrcs  
 
                imu_sentence = iihdt + '\r\n' + iixdr  
 
                sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  
                sock.sendto(imu_sentence, (IMU_IP, IMU_PORT))  
 
                t_print = hack  
                print imu_sentence
                
        time.sleep(poll_interval*1.0/1000.0)  

So this is working, now I have apparent wind angle in OpenCPN


[Image: Knipsel.png]


To-do, enhance the code with true wind angle and direction and some code cleaning in imu.py
 
Please share your thoughts about this project ;-)
nice.
i had an idea with an MPU9250 and an one of these for speed, as wind instrument.. Smile
https://www.google.com/search?q=MPXV7002...59&bih=614
that might be the kick in the ass for me to look at it a little closer..
OpenPlotter uses a modified version of RTIMULib2, if you follow this manual you will overwrite RTIMULib2 and pypilot could not work as expected.
Ah I understand.
Any sugestion how to get it to work otherwise?
I still have a heading reading from de 0x68 MPU. But I don`t use the autopilot option (jet)
maybe use an esp32
library
https://github.com/natanaeljr/esp32-MPU-driver
there is a nmea0183 parser thingy i saw somewhere you could send wind data to op
Thanks, I will look in to that option
Nice stuff those ESP`s.
I bought a NodeMPU. The heading value comes trough Mqtt, and is seen in Signal K.
But I cant get it in OpenCPN. I have tried id with Signal-K signal converter and with the NMEA genarator.
Do I need to send the full NMEA sentence or just the raw value?

Edit:
Now I have it almost working seeding heading direct to SignalK. But still i have trouble with the conversion to NMEA
Also when there is some improvement to be made in the code, let me know ;-)

Edit2:
I now have manual add the scentence in NMEA converter. But still no reading in OpenCPN. Any thoughts?
I have had this issue too, but speed direction are sent to signalK AWS AWD at 10 hz.. It seems Openplotter does not have the appropriate NMEA sentences available.
I send the following two scenteces to signal K

    sendMessageFloat("Windvane","environment.wind.angleApparent",heading);

    sendMessageFloat("Windvane","environment.wind.directionMagnetic",heading);

I let SignalK convert it to NMEA scenteces.
For now I do not have an anemometer so I simulated that with SignalK. Now it is shown in OpenCPN. But my heading caluculation is from North not from the bow. Still have to fix the code, and enhance it with anemometer.
(2018-09-24, 08:38 AM)KolonelP Wrote: [ -> ]I send the following two scenteces to signal K

    sendMessageFloat("Windvane","environment.wind.angleApparent",heading);

    sendMessageFloat("Windvane","environment.wind.directionMagnetic",heading);

I let SignalK convert it to NMEA scenteces.
For now I do not have an anemometer so I simulated that with SignalK. Now it is shown in OpenCPN. But my heading calculation is from North not from the bow. Still have to fix the code, and enhance it with anemometer.

I use the following code to send the sentences to SignalK using python

Code:
import socket
       sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
       SignalK = '{"updates":[{"$source":"OPserial.wind","values":['
       SignalK += '{"path":"environment.wind.speedApparent","value":'+str(aws)+'}'
       SignalK += ',{"path":"environment.wind.angleApparent","value":'+str(awa)+'}'
       SignalK += ']}]}\n'
       #print (SignalK)
       sock.sendto(SignalK, ('127.0.0.1', 55559))
       sock.close()
I don't why you would need two sets of wind sensors.
I built my sensors but i am going to change next season as mine are to clunky and heavy, I added a Var to hold the offset to the bow so it reads correctly regardless of how my sensor is orientated to the bow. true wind is a calculation.
I had some issues when i updated to stretch due to the fact that the SPI bus speed changed. So i added a line to lib_nrf24.py to explicitly set spi bus speed.. I use a NRF240l+ to transmit data to and from the masthead .
The masthead controls my anchor light and transmits wind data to the Pi, I used it because it was cheap and has very low power requirements. the down side is that it is a bit a pain to code and hard to debug. It is a work in-progress.
Code:
self.spidev.open(0, csn_pin)
       # debug
       self.spidev.max_speed_hz = 400000 # change for stretch
       self.ce_pin = ce_pin
I found this after several days of hair pulling and checking all my connections.

How did you set up the conversion in SignalK??
Pages: 1 2