Second MPU for windfane

[KolonelP]

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





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 ;-)