Технояхтинг с Ардуино

[maxlab]

Привет!
Продолжаем...
Приехала апельсинка (orange pi 3 lts). Налепил на нее радиаторов, установил на MicroSD образ убунты. Взлетело!
Прикрутил OpenCPN, подкинул морские карты, настроил GPS/GLONASS.
Собрал из исходников rtl-ais (идентификация судов)... тест соединения проходит. Завтра натурные испытания на побережье сделаю. Может успею
и прием погодных факсов настроить через свисток SDR

[НовыйДень]

Ну, яхты у меня нету, я не родственник Абрамовича, поэтому оценить затею не могу А касательно жипиэс-трекера, я когда-то "для побаловаться" делал на базе отладочной платы STM32F746G-Disco.

[maxlab]

НовыйДень писал(а):

Ну, яхты у меня нету, я не родственник Абрамовича...

Миром правят стереотипы Для того чтобы иметь яхту не нужно быть олигархом. Можно уложиться в бюджет подержанного авто. Например Maxi 77 в удовлетворительном состоянии можно найти по цене до 600 килорублей. В такой же бюджет ляжет и самостоятельная постройка... Правда это долго и не всем по плечу.

[maxlab]

Нашел я апноут на MPU6500 на процедуру самотестирования. Исправил вендорский код.
Вуаля ...


Код... Может кому понадобится

Спойлер

Код:


// Accelerometer and gyroscope self test; check calibration wrt factory settings

bool self_test_impl()  // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
    {
        uint8_t raw_data[6] = {0, 0, 0, 0, 0, 0};
        int32_t gAvg[3] = {0}, aAvg[3] = {0};
        int32_t aSTAvg[3] = {0}, gSTAvg[3] = {0};
        float factoryTrim[6];
        uint8_t FS = 0;
        float self_test_result[6] {0.f};
       
        writeByte(MPU6500_ADDRESS, SMPLRT_DIV, 0x00);       // Set gyro sample rate to 1 kHz
        writeByte(MPU6500_ADDRESS, CONFIG, 0x02);       // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
        writeByte(MPU6500_ADDRESS, GYRO_CONFIG, FS << 3);   // Set full scale range for the gyro to 250 dps
        writeByte(MPU6500_ADDRESS, ACCEL_CONFIG2, 0x02);    // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
        writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, FS << 3);  // Set full scale range for the accelerometer to 2 g

        for (int ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer

            readBytes(MPU6500_ADDRESS, ACCEL_XOUT_H, 6, &raw_data[0]);          // Read the six raw data registers into data array
            aAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]);  // Turn the MSB and LSB into a signed 16-bit value
            aAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
            aAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);

            readBytes(MPU6500_ADDRESS, GYRO_XOUT_H, 6, &raw_data[0]);           // Read the six raw data registers sequentially into data array
            gAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]);  // Turn the MSB and LSB into a signed 16-bit value
            gAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
            gAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
        }

        for (int ii = 0; ii < 3; ii++) {  // Get average of 200 values and store as average current readings
            aAvg[ii] /= 200; aAvg[ii] <<= 24; aAvg[ii] >>= 24;  // Не хватало сдвига туда-сюда чтобы получить LSB
            gAvg[ii] /= 200; gAvg[ii] <<= 24; gAvg[ii] >>= 24;
        }

        // Configure the accelerometer for self-test
        writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, 0xE0);  // Enable self test on all three axes and set accelerometer range to +/- 2 g
        writeByte(MPU6500_ADDRESS, GYRO_CONFIG, 0xE0);   // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
        delay(25);                                     // Delay a while to let the device stabilize

        for (int ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer

            readBytes(MPU6500_ADDRESS, ACCEL_XOUT_H, 6, &raw_data[0]);            // Read the six raw data registers into data array
            aSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]);  // Turn the MSB and LSB into a signed 16-bit value
            aSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
            aSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
           
            readBytes(MPU6500_ADDRESS, GYRO_XOUT_H, 6, &raw_data[0]);             // Read the six raw data registers sequentially into data array
            gSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]);  // Turn the MSB and LSB into a signed 16-bit value
            gSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]);
            gSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]);
        }
        for (int ii = 0; ii < 3; ii++) {  // Get average of 200 values and store as average self-test readings
            aSTAvg[ii] /= 200; aSTAvg[ii] <<= 24; aSTAvg[ii] >>= 24; // Не хватало сдвига туда-сюда чтобы получить LSB
            gSTAvg[ii] /= 200; gSTAvg[ii] <<= 24; gSTAvg[ii] >>= 24;
        }

        // Configure the gyro and accelerometer for normal operation
        writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, 0x00);
        writeByte(MPU6500_ADDRESS, GYRO_CONFIG, 0x00);
        delay(25);  // Delay a while to let the device stabilize

        // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
        uint8_t self_test_data[6];
        self_test_data[0] = readByte(MPU6500_ADDRESS, SELF_TEST_X_ACCEL);  // X-axis accel self-test results
        self_test_data[1] = readByte(MPU6500_ADDRESS, SELF_TEST_Y_ACCEL);  // Y-axis accel self-test results
        self_test_data[2] = readByte(MPU6500_ADDRESS, SELF_TEST_Z_ACCEL);  // Z-axis accel self-test results
        self_test_data[3] = readByte(MPU6500_ADDRESS, SELF_TEST_X_GYRO);   // X-axis gyro self-test results
        self_test_data[4] = readByte(MPU6500_ADDRESS, SELF_TEST_Y_GYRO);   // Y-axis gyro self-test results
        self_test_data[5] = readByte(MPU6500_ADDRESS, SELF_TEST_Z_GYRO);   // Z-axis gyro self-test results

        // Retrieve factory self-test value from self-test code reads
        factoryTrim[0] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[0] - 1.0)));  // FT[Xa] factory trim calculation
        factoryTrim[1] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[1] - 1.0)));  // FT[Ya] factory trim calculation
        factoryTrim[2] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[2] - 1.0)));  // FT[Za] factory trim calculation
        factoryTrim[3] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[3] - 1.0)));  // FT[Xg] factory trim calculation
        factoryTrim[4] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[4] - 1.0)));  // FT[Yg] factory trim calculation
        factoryTrim[5] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[5] - 1.0)));  // FT[Zg] factory trim calculation

        // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
        // To get percent, must multiply by 100
        for (int i = 0; i < 3; i++) {
            self_test_result[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i];          // Report percent differences
            self_test_result[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3];  // Report percent differences
        }

            Serial.print("x-axis self test: acceleration trim within : ");
            Serial.print(self_test_result[0], 1);
            Serial.println("% of factory value");
            Serial.print("y-axis self test: acceleration trim within : ");
            Serial.print(self_test_result[1], 1);
            Serial.println("% of factory value");
            Serial.print("z-axis self test: acceleration trim within : ");
            Serial.print(self_test_result[2], 1);
            Serial.println("% of factory value");
            Serial.print("x-axis self test: gyration trim within : ");
            Serial.print(self_test_result[3], 1);
            Serial.println("% of factory value");
            Serial.print("y-axis self test: gyration trim within : ");
            Serial.print(self_test_result[4], 1);
            Serial.println("% of factory value");
            Serial.print("z-axis self test: gyration trim within : ");
            Serial.print(self_test_result[5], 1);
            Serial.println("% of factory value");
 
        bool b = true;
        for (uint8_t i = 0; i < 6; ++i) {
            b &= fabs(self_test_result[i]) <= 14.f;
        }
        return b;
    }


Можно двигаться дальше

[maxlab]

Добрый вечер! За все лето ни разу не брал паяло в руки и не открывал Arduino IDE.

Купить подержанную лодку мне не удалось... по понятным причинам, поэтому, чтобы иметь точку приложения
к своим идеям по технояхтингу - решил построить. Так что все лето занимался судостроением. Вот что из этого вышло:

Строим вот это

Длина 6.5, щирина 2.5, осадка будет 1.10
В разрезе

Пока выглядит так

Сегодня монтировал килевую балку


Еще две недели будет сносная погода. Затем стройку консервирую до следующей весны и займусь электроникой

[maxlab]

Всем привет!
Разобрался с QMC5883L. Этот магнетометр весьма не плох, не смотря что стоит сущие копейки.
Показания стабильны после 5 минутного прогрева. Точность показаний зависит от начальной калибровки. Подключал датчик к NodeMCU. Прошивку писал на MicroPython. После женитьбы магнетометра с акселерометром MPU-6500, код портирую на C. На питоне быстрее экспериментировать

Спойлер

Код:

from machine import I2C,Pin
import utime
import sys
import math

# Калибровочная матрица
qmc_matrix = {
    'x':[-1810,1303],
    'y':[-1507,1527],
    'z':[-1332,1505]
    }

# Запуск i2c шины
i2c = I2C(scl=Pin(5), sda=Pin(4))

# QMC5883L - описание регистров

# i2c device addres
qmc_adrs = 0x0D

# qmc output data registers
qmc_x_lsb = 0x00
qmc_x_msb = 0x01
qmc_y_lsb = 0x02
qmc_y_msb = 0x03
qmc_z_lsb = 0x04
qmc_z_msb = 0x05

# qmc status register
qmc_status = 0x06

# qmc temperature data register
qmc_temp_lsb = 0x07
qmc_temp_msb = 0x08

# qmc control register 1
qmc_cntrl_1 = 0x09

# qmc control register 2
qmc_cntrl_2 = 0x0A

# qmc SET/RESET Period register
qmc_period  = 0x0B

# qmc Chip ID register
qmc_chip_id = 0x0D

# Проверка соединения с датчиком
if not (i2c.readfrom_mem(qmc_adrs,qmc_chip_id,1)==bytes([0xFF])):
    print("qmc is abcent!")
    sys.exit()

# Процедура слияния байтов в 16 разрядное слово
def combine_register_values(h, l):
    if not h[0] & 0x80:
        return h[0] << 8 | l[0]
    return ((((h[0] ^ 0xFF) << 8) |  (l[0] ^ 0xFF)) + 1)*(-1)

# Чтение x,y,z данных из регистров
def get_raw():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_x_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_y_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_z_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_z_lsb,1))
    }
    return data

# Данные с учетом калибровочной таблицы
def get_qmc_data():
    data = get_raw()
    x_ofs = (qmc_matrix['x'][0] + qmc_matrix['x'][1])/2
    y_ofs = (qmc_matrix['y'][0] + qmc_matrix['y'][1])/2
    z_ofs = (qmc_matrix['z'][0] + qmc_matrix['z'][1])/2
    x_avg_delta = (qmc_matrix['x'][1] - qmc_matrix['x'][0])/2
    y_avg_delta = (qmc_matrix['y'][1] - qmc_matrix['y'][0])/2
    z_avg_delta = (qmc_matrix['z'][1] - qmc_matrix['z'][0])/2
    avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta)/3
    x_scale = avg_delta / x_avg_delta
    y_scale = avg_delta / y_avg_delta
    z_scale = avg_delta / z_avg_delta
    return {
        'x':(data['x']-x_ofs)*x_scale,
        'y':(data['y']-y_ofs)*y_scale,
        'z':(data['z']-z_ofs)*z_scale
        }
# Инициализация магнитометра
i2c.writeto_mem(qmc_adrs,qmc_period,bytes([0x01]))  # Set recommended SET/RESET period
i2c.writeto_mem(qmc_adrs,qmc_cntrl_1,bytes([0x1D])) # Set mode Continuous,ODR 200Hz,RNG 8g,OSR 512

queue=[] # Конвеер для фильтра НЧ 

while True:
    data = get_qmc_data()
    queue.append(data)
    if len(queue) > 64:queue.pop(0) # Глубина конвеера 64
    x = 0; y = 0; z = 0
    for  val in queue:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue); y = y / len(queue); z = z / len(queue) # Средние значения по всем осям
    azimuth = math.atan2(y,x)*180.0/math.pi # Вычисление азимута
    if azimuth < 0: azimuth = 360+azimuth
    print(round(azimuth))
    utime.sleep_ms(50)


[maxlab]

Добрый вечер! Победил я датчик пространственной ориентации применительно к лодке.
Вот код на питоне, кому интересно

Спойлер

Код:

# Китайский клон на RP2040 с 16 Mb на борту
# Версия MicroPython v1.19.1;Pimoroni Pico LiPo 16MB with RP2040
# В алгоритме компенсации гироскоп не задействован

from machine import I2C,Pin

import utime
import sys
import math

# Калибровочная матрица
qmc_matrix = {
    'x':[-1810,1303],
    'y':[-1507,1527],
    'z':[-1332,1505]
    }

# Калибровочные смещения аксель/гиро
accel_offs = {'x':[161.3976], 'y':[-656.3018], 'z':[8.220917]}
gyro_offs  = {'x':[-852.3802], 'y':[19.42475], 'z':[-2881.868]}

# Запуск i2c шины
i2c = I2C(1,scl=Pin(3), sda=Pin(2))

# QMC5883L - описание регистров

# i2c device addres
qmc_adrs = 0x0D

# qmc output data registers
qmc_x_lsb = 0x00
qmc_x_msb = 0x01
qmc_y_lsb = 0x02
qmc_y_msb = 0x03
qmc_z_lsb = 0x04
qmc_z_msb = 0x05

# qmc status register
qmc_status = 0x06

# qmc temperature data register
qmc_temp_lsb = 0x07
qmc_temp_msb = 0x08

# qmc control register 1
qmc_cntrl_1 = 0x09

# qmc control register 2
qmc_cntrl_2 = 0x0A

# qmc SET/RESET Period register
qmc_period  = 0x0B

# qmc Chip ID register
qmc_chip_id = 0x0D

# MPU6500 - описание регистров

# i2c device addres
mpu_adrs = 0x68

# mpu control registers
mpu_pwr_mgmt_1 = 0x6B
mpu_accel_config = 0x1C
mpu_accel_config2 = 0x1D
mpu_gyro_config = 0x1B
mpu_config = 0x1A

# mpu data registers
gyro_x_msb = 0x43
gyro_x_lsb = 0x44
gyro_y_msb = 0x45
gyro_y_lsb = 0x46
gyro_z_msb = 0x47
gyro_z_lsb = 0x48

accel_x_msb = 0x3B
accel_x_lsb = 0x3C
accel_y_msb = 0x3D
accel_y_lsb = 0x3E
accel_z_msb = 0x3F
accel_z_lsb = 0x40

# mpu Chip ID register
mpu_chip_id = 0x75  # Должен быть ответ - 0x70

# Переменные для вычислений
depth       = 64 # Глубина конвеера фильтра НЧ
queue_accel = [] # Конвеер для акселерометра
queue_mag   = [] # Конвеер для магнитометра
queue_gyro  = [] # Конвеер для гироскопа
# Из даташита
accel_scale = 4096.0
gyro_scale  = 65.5
mag_scale   = 3000

# Проверка соединения с датчиком
if not (i2c.readfrom_mem(qmc_adrs,qmc_chip_id,1)==bytes([0xFF])):
    raise RuntimeError("qmc not found.")
if not (i2c.readfrom_mem(mpu_adrs,mpu_chip_id,1)==bytes([0x70])):
    raise RuntimeError("mpu not found.")

# Процедура слияния байтов в 16 разрядное слово
def combine_register_values(h, l):
    if not h[0] & 0x80:
        return h[0] << 8 | l[0]
    return ((((h[0] ^ 0xFF) << 8) |  (l[0] ^ 0xFF)) + 1)*(-1)

# Чтение x,y,z данных из регистров матнитометра
def get_raw_qmc():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_x_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_y_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_z_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_z_lsb,1))
    }
    queue_mag.append(data)
    if len(queue_mag) > depth: queue_mag.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_mag:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_mag); y = y / len(queue_mag); z = z / len(queue_mag)
    return {'x':x,'y':y,'z':z}

# Данные магнитометра с учетом калибровочной таблицы
def get_qmc_data():
    data = get_raw_qmc()
    x_ofs = (qmc_matrix['x'][0] + qmc_matrix['x'][1])/2
    y_ofs = (qmc_matrix['y'][0] + qmc_matrix['y'][1])/2
    z_ofs = (qmc_matrix['z'][0] + qmc_matrix['z'][1])/2
    x_avg_delta = (qmc_matrix['x'][1] - qmc_matrix['x'][0])/2
    y_avg_delta = (qmc_matrix['y'][1] - qmc_matrix['y'][0])/2
    z_avg_delta = (qmc_matrix['z'][1] - qmc_matrix['z'][0])/2
    avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta)/3
    x_scale = avg_delta / x_avg_delta
    y_scale = avg_delta / y_avg_delta
    z_scale = avg_delta / z_avg_delta
    return {
        'x':((data['x']-x_ofs)*x_scale)/mag_scale,
        'y':((data['y']-y_ofs)*y_scale)/mag_scale,
        'z':((data['z']-z_ofs)*z_scale)/mag_scale
        }

# Чтение x,y,z данных из регистров MPU
def get_gyro_raw():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_x_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_y_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_z_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_z_lsb,1))
    }
    queue_gyro.append(data)
    if len(queue_gyro) > depth: queue_gyro.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_gyro:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_gyro); y = y / len(queue_gyro); z = z / len(queue_gyro)
    return {'x':x,'y':y,'z':z}

def get_accel_raw():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_x_msb,1),i2c.readfrom_mem(mpu_adrs,accel_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_y_msb,1),i2c.readfrom_mem(mpu_adrs,accel_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_z_msb,1),i2c.readfrom_mem(mpu_adrs,accel_z_lsb,1))
    }
    queue_accel.append(data)
    if len(queue_accel) > depth: queue_accel.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_accel:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_accel); y = y / len(queue_accel); z = z / len(queue_accel)
    return {'x':x,'y':y,'z':z}

def get_gyro():
    data = get_gyro_raw()
    return {
        'x':(data['x']-gyro_offs['x'][0])/gyro_scale,
        'y':(data['y']-gyro_offs['y'][0])/gyro_scale,
        'z':(data['z']-gyro_offs['z'][0])/gyro_scale
        }

def get_accel():
    data = get_accel_raw()
    return {
        'x':(data['x']-accel_offs['x'][0])/accel_scale,
        'y':(data['y']-accel_offs['y'][0])/accel_scale,
        'z':(data['z']-accel_offs['z'][0])/accel_scale
        }



# Инициализация магнитометра
i2c.writeto_mem(qmc_adrs,qmc_period,bytes([0x01]))  # Set recommended SET/RESET period
i2c.writeto_mem(qmc_adrs,qmc_cntrl_1,bytes([0x1D])) # Set mode Continuous,ODR 200Hz,RNG 8g,OSR 512

# Инициализация аксель/гиро
i2c.writeto_mem(mpu_adrs, mpu_pwr_mgmt_1, bytes([0x00]))    #Use internal 20MHz clock
i2c.writeto_mem(mpu_adrs, mpu_accel_config, bytes([0x10]))  #Select +/-8g full-scale
i2c.writeto_mem(mpu_adrs, mpu_gyro_config, bytes([0x00]))   #Select 500dps full-scale
i2c.writeto_mem(mpu_adrs, mpu_config, bytes([0x05]))        #DLPF Bandwidth = 10

# Поправки определить на абсолютно ровном столе к горизонту
pitch_err = 5
roll_err = 8

while True:
    accel = get_accel()
    r = math.sqrt(accel['x']*accel['x']+accel['y']*accel['y']+accel['z']*accel['z']) # Вектор пространственной ориентации
    # Получаем крен/тангаж в градусах и суммируем с поправкой
    pitch = 180/math.pi*math.asin(accel['x']/r)+pitch_err
    roll = 180/math.pi*(-1)*math.asin((-1)*accel['y']/r)+roll_err
    # Обратно в радианы с учетом поправок
    pitch_rad = math.pi/180*pitch
    roll_rad = math.pi/180*roll
    # Расчет влияния крена/тангажа на показания магнитометра
    mag = get_qmc_data()
    cx = mag['x']*math.cos(pitch_rad) + (-1)*mag['y']*math.sin(roll_rad)*math.sin(pitch_rad) - mag['z']*math.cos(roll_rad)*math.sin(pitch_rad)
    cy = (-1)*mag['y']*math.cos(roll_rad) + mag['z']*math.sin(roll_rad)
    az_rad = 180/math.pi * math.atan2(cy,cx)
    azimuth = round(((-1)*az_rad+360) % 360)
    print(azimuth, round(pitch), round(roll))



Ориентация датчиков такая


Осталось на С портировать и затолкать это все в lgt8f328p - а..ля ардуино микро
Заталкивать будем сюда

Есть еще идея заморочиться с миниатюрным гравитационным подвесом и поместить туда магнитометр. Тогда компенсация не нужна будет. Но возникнет геморрой с токосъемниками. В общем ... на будущее

Вложения

YMFC-32_document_1.pdf

Источник вдохновения

(85.9 KiB) Скачиваний: 49

[maxlab]

Всем привет! Дочитался я как в компенсацию наклонов подмешать данные гироскопа. Вроде работает. Предстоит все это на стенде испытать.
Теперь код выглядит так:

Спойлер

Код:

# Китайский клон на RP2040 с 16 Mb на борту
# Версия MicroPython v1.19.1;Pimoroni Pico LiPo 16MB with RP2040

from machine import I2C,Pin

import utime
import sys
import math

# Калибровочная матрица
qmc_matrix = {
    'x':[-1810,1303],
    'y':[-1507,1527],
    'z':[-1332,1505]
    }

# Калибровочные смещения аксель/гиро
accel_offs = {'x':[161.3976], 'y':[-656.3018], 'z':[8.220917]}
gyro_offs  = {'x':[-852.3802], 'y':[19.42475], 'z':[-2881.868]}

# Запуск i2c шины
i2c = I2C(1,scl=Pin(3), sda=Pin(2))

# QMC5883L - описание регистров

# i2c device addres
qmc_adrs = 0x0D

# qmc output data registers
qmc_x_lsb = 0x00
qmc_x_msb = 0x01
qmc_y_lsb = 0x02
qmc_y_msb = 0x03
qmc_z_lsb = 0x04
qmc_z_msb = 0x05

# qmc status register
qmc_status = 0x06

# qmc temperature data register
qmc_temp_lsb = 0x07
qmc_temp_msb = 0x08

# qmc control register 1
qmc_cntrl_1 = 0x09

# qmc control register 2
qmc_cntrl_2 = 0x0A

# qmc SET/RESET Period register
qmc_period  = 0x0B

# qmc Chip ID register
qmc_chip_id = 0x0D

# MPU6500 - описание регистров

# i2c device addres
mpu_adrs = 0x68

# mpu control registers
mpu_pwr_mgmt_1 = 0x6B
mpu_accel_config = 0x1C
mpu_accel_config2 = 0x1D
mpu_gyro_config = 0x1B
mpu_config = 0x1A

# mpu data registers
gyro_x_msb = 0x43
gyro_x_lsb = 0x44
gyro_y_msb = 0x45
gyro_y_lsb = 0x46
gyro_z_msb = 0x47
gyro_z_lsb = 0x48

accel_x_msb = 0x3B
accel_x_lsb = 0x3C
accel_y_msb = 0x3D
accel_y_lsb = 0x3E
accel_z_msb = 0x3F
accel_z_lsb = 0x40

# mpu Chip ID register
mpu_chip_id = 0x75  # Должен быть ответ - 0x70

# Переменные для вычислений
depth       = 64 # Глубина конвеера фильтра НЧ
queue_accel = [] # Конвеер для акселерометра
queue_mag   = [] # Конвеер для магнитометра
queue_gyro  = [] # Конвеер для гироскопа

# Из даташита
accel_scale = 4096.0
gyro_scale  = 65.5
mag_scale   = 3000

# Проверка соединения с датчиком
if not (i2c.readfrom_mem(qmc_adrs,qmc_chip_id,1)==bytes([0xFF])):
    raise RuntimeError("qmc not found.")
if not (i2c.readfrom_mem(mpu_adrs,mpu_chip_id,1)==bytes([0x70])):
    raise RuntimeError("mpu not found.")

# Процедура слияния байтов в 16 разрядное слово
def combine_register_values(h, l):
    if not h[0] & 0x80:
        return h[0] << 8 | l[0]
    return ((((h[0] ^ 0xFF) << 8) |  (l[0] ^ 0xFF)) + 1)*(-1)

# Чтение x,y,z данных из регистров матнитометра
def get_raw_qmc():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_x_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_y_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(qmc_adrs,qmc_z_msb,1),i2c.readfrom_mem(qmc_adrs,qmc_z_lsb,1))
    }
    queue_mag.append(data)
    if len(queue_mag) > depth: queue_mag.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_mag:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_mag); y = y / len(queue_mag); z = z / len(queue_mag)
    return {'x':x,'y':y,'z':z}

# Данные магнитометра с учетом калибровочной таблицы
def get_qmc_data():
    data = get_raw_qmc()
    x_ofs = (qmc_matrix['x'][0] + qmc_matrix['x'][1])/2
    y_ofs = (qmc_matrix['y'][0] + qmc_matrix['y'][1])/2
    z_ofs = (qmc_matrix['z'][0] + qmc_matrix['z'][1])/2
    x_avg_delta = (qmc_matrix['x'][1] - qmc_matrix['x'][0])/2
    y_avg_delta = (qmc_matrix['y'][1] - qmc_matrix['y'][0])/2
    z_avg_delta = (qmc_matrix['z'][1] - qmc_matrix['z'][0])/2
    avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta)/3
    x_scale = avg_delta / x_avg_delta
    y_scale = avg_delta / y_avg_delta
    z_scale = avg_delta / z_avg_delta
    return {
        'x':((data['x']-x_ofs)*x_scale)/mag_scale,
        'y':((data['y']-y_ofs)*y_scale)/mag_scale,
        'z':((data['z']-z_ofs)*z_scale)/mag_scale
        }

# Чтение x,y,z данных из регистров MPU
def get_gyro_raw():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_x_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_y_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(mpu_adrs,gyro_z_msb,1),i2c.readfrom_mem(mpu_adrs,gyro_z_lsb,1))
    }
    queue_gyro.append(data)
    if len(queue_gyro) > depth: queue_gyro.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_gyro:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_gyro); y = y / len(queue_gyro); z = z / len(queue_gyro)
    return {'x':x,'y':y,'z':z}

def get_accel_raw():
    data = {
    'x':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_x_msb,1),i2c.readfrom_mem(mpu_adrs,accel_x_lsb,1)),
    'y':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_y_msb,1),i2c.readfrom_mem(mpu_adrs,accel_y_lsb,1)),
    'z':combine_register_values(i2c.readfrom_mem(mpu_adrs,accel_z_msb,1),i2c.readfrom_mem(mpu_adrs,accel_z_lsb,1))
    }
    queue_accel.append(data)
    if len(queue_accel) > depth: queue_accel.pop(0)
    x = 0; y = 0; z = 0
    for  val in queue_accel:
        x += val['x']; y += val['y']; z += val['z']
    x = x / len(queue_accel); y = y / len(queue_accel); z = z / len(queue_accel)
    return {'x':x,'y':y,'z':z}

def get_gyro():
    data = get_gyro_raw()
    return {
        'x':(data['x']-gyro_offs['x'][0])/gyro_scale,
        'y':(data['y']-gyro_offs['y'][0])/gyro_scale,
        'z':(data['z']-gyro_offs['z'][0])/gyro_scale
        }

def get_accel():
    data = get_accel_raw()
    return {
        'x':(data['x']-accel_offs['x'][0])/accel_scale,
        'y':(data['y']-accel_offs['y'][0])/accel_scale,
        'z':(data['z']-accel_offs['z'][0])/accel_scale
        }



# Инициализация магнитометра
i2c.writeto_mem(qmc_adrs,qmc_period,bytes([0x01]))  # Set recommended SET/RESET period
i2c.writeto_mem(qmc_adrs,qmc_cntrl_1,bytes([0x1D])) # Set mode Continuous,ODR 200Hz,RNG 8g,OSR 512

# Инициализация аксель/гиро
i2c.writeto_mem(mpu_adrs, mpu_pwr_mgmt_1, bytes([0x00]))    #Use internal 20MHz clock
i2c.writeto_mem(mpu_adrs, mpu_accel_config, bytes([0x10]))  #Select +/-8g full-scale
i2c.writeto_mem(mpu_adrs, mpu_gyro_config, bytes([0x00]))   #Select 500dps full-scale
i2c.writeto_mem(mpu_adrs, mpu_config, bytes([0x05]))        #DLPF Bandwidth = 10

# Поправки определить на абсолютно ровном столе к горизонту
pitch_err = -5
roll_err = 7

cur_time=0
gyroAngleX=0
gyroAngleY=0

while True:
    accel = get_accel()
    accAngleX = (math.atan(accel['y'] / math.sqrt(math.pow(accel['x'], 2) + math.pow(accel['z'], 2))) * 180 / math.pi)
    accAngleY = (math.atan(-1 * accel['x'] / math.sqrt(math.pow(accel['y'], 2) + math.pow(accel['z'], 2))) * 180 / math.pi)
   
    prev_time=cur_time
    cur_time=utime.ticks_ms()
    elapsed = (cur_time-prev_time)/1000
    gyro = get_gyro()
    gyroAngleX = gyroAngleX + gyro['x'] * elapsed
    gyroAngleY = gyroAngleY + gyro['y'] * elapsed
   
    # Смешиваем данные гиро и аксель. Комплементарный фильтр
    gyroAngleX = 0.96 * gyroAngleX + 0.04 * accAngleX
    gyroAngleY = 0.96 * gyroAngleY + 0.04 * accAngleY
   
    # Учет поправок
    roll =gyroAngleX+roll_err
    pitch = gyroAngleY+pitch_err
   
    # Обратно в радианы
    roll_rad = math.pi/180*roll
    pitch_rad = math.pi/180*pitch
   
    # Расчет влияния крена/тангажа на показания магнитометра
    mag = get_qmc_data()
    cx = mag['x']*math.cos(pitch_rad) + (-1)*mag['y']*math.sin(roll_rad)*math.sin(pitch_rad) - mag['z']*math.cos(roll_rad)*math.sin(pitch_rad)
    cy = (-1)*mag['y']*math.cos(roll_rad) + mag['z']*math.sin(roll_rad)
    az_rad = 180/math.pi * math.atan2(cy,cx)
   
    azimuth = round(((-1)*az_rad+360) % 360)
   
    print(azimuth, round(pitch), round(roll))



[roman.com]

а мы так и не доделали свой кораблик...
кучу всего подключили... всё вывели на экран и комп... но до конца не настроили...
https://www.radiokot.ru/forum/download/ ... ?id=386349
https://www.radiokot.ru/forum/download/ ... ?id=386350
...

правда... у нас и кораблик поменьше))
https://www.radiokot.ru/forum/viewtopic ... 9#p4315325
ну и ладно)) в другой раз))
будем делать управление с ПК и телефона.

[maxlab]

roman.com
А в качестве радиомодема тетю Лору на пробывали? Дальнобойность восхищает однако...

[roman.com]

Лору не пробовали...
Лора имеет дальнобойность только при низкой скорости передачи... десятки бит/с... сотни бит/с.
а нам нужна высокая скорость передачи... тысячи бит/с... миллионы бит/с... для минимального времени отклика.
при таких скоростях дальнобойность у Лоры маленькая))

[maxlab]

Добрый вечер! Портировал код с Python на C.
Рабочая версия без использования гироскопа

Спойлер

Код:

// LGT8F328P 32Mhz SSOP-20

#include <Wire.h>
#include <WDT.h>
#include <ModbusRtu.h>
#include <SoftwareSerial.h>

#define TXEN    7                             
#define DEV_ID  2                             
#define ZUMMER  2

#define MPU6500_ADDRESS       0x68 
#define QMC5883L_ADDRESS      0x0D 
#define PWR_MGMT_1            0x6B
#define ACCEL_CONFIG          0x1D
#define GYRO_CONFIG           0x1B
#define CONFIG                0x1A
#define INT_EN                0x38
#define qmc_period            0x0B
#define qmc_cntrl_1           0x09
#define qmc_status            0x06   
#define mpu_status            0x3A
#define ACCEL_XOUT_H          0x3B
#define qmc_x_lsb             0x00

#define DEPTH 64

int qmc_matrix[] = {-1637, 1508, -1541, 1427, -1421, 1401};

int accel_scale = 4096;
int mag_scale = 3000;
int queue_mag[DEPTH][3];
int queue_accel[DEPTH][3];
float accel_data[] = {0,0,0};
float qmc_data[] = {0,0,0};
unsigned int fld_data[3] = {0,0,0};
int pitch_err=2,roll_err=17;
long lastTime;

SoftwareSerial mySerial(9, 10);  // RX, TX
Modbus slave(DEV_ID,mySerial,TXEN);

void setup() {
  pinMode(ZUMMER,OUTPUT);
  Wire.begin();
  Serial.begin(38400);
  mySerial.begin(38400);
  initMPU6500();
  initQMC();
  wdt_enable(WTO_8S);
  slave.start();
}

void loop() {
  float r,pitch,roll,pitch_rad,roll_rad,cx,cy;
  int azimuth,az_rad;
  long newTime;
  wdt_reset();
  newTime = millis();
  readAccelData(accel_data);
  r = sqrtf(pow(accel_data[0],2)+pow(accel_data[1],2)+pow(accel_data[2],2));
  pitch = 180/PI*asin(accel_data[0]/r)+pitch_err;
  roll = 180/PI*(-1)*asin((-1)*accel_data[1]/r)+roll_err;
  pitch_rad = PI/180*pitch;
  roll_rad = PI/180*roll;
  readMagData(qmc_data);
  cx = qmc_data[0]*cos(pitch_rad) + (-1)*qmc_data[1]*sin(roll_rad)*sin(pitch_rad) - qmc_data[2]*cos(roll_rad)*sin(pitch_rad);
  cy = (-1)*qmc_data[1]*cos(roll_rad) + qmc_data[2]*sin(roll_rad);
  az_rad = 180/PI * atan2(cy,cx);
  azimuth = round(((-1)*az_rad+360) % 360);
  fld_data[0] = round(pitch);
  fld_data[1] = round(roll);
  fld_data[2] = azimuth;
  slave.poll(fld_data,3);                     
  if((newTime-lastTime) > 250)
    {
      digitalToggle(LED_BUILTIN);
      Serial.print(round(pitch)); 
      Serial.print("\t"); 
      Serial.print(round(roll)); 
      Serial.print("\t"); 
      Serial.println(azimuth); 
      lastTime = newTime;
    }
}


void initQMC()
{
  writeByte(QMC5883L_ADDRESS,qmc_period,0x01);
  writeByte(QMC5883L_ADDRESS,qmc_cntrl_1,0x1D);
}
 
void initMPU6500()
{
  writeByte(MPU6500_ADDRESS, PWR_MGMT_1, 0x00); delay(100);
  writeByte(MPU6500_ADDRESS, PWR_MGMT_1, 0x01); delay(100);
  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, 0x10);
  writeByte(MPU6500_ADDRESS, INT_EN, 0x01);
}

void readAccelData(float * destination)
{
  byte rawData[6];
  long sumX=0,sumY=0,sumZ=0;   
  for (byte i = DEPTH-1; i > 0 ; i--) {
    queue_accel[i][0] = queue_accel[i-1][0];sumX += queue_accel[i-1][0];   
    queue_accel[i][1] = queue_accel[i-1][1];sumY += queue_accel[i-1][1];   
    queue_accel[i][2] = queue_accel[i-1][2];sumZ += queue_accel[i-1][2];   
   }
   while (! mpu_isReady());
  readBytes(MPU6500_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); 
  queue_accel[0][0] = ((int)rawData[0] << 8) | rawData[1] ; sumX += queue_accel[0][0];
  queue_accel[0][1] = ((int)rawData[2] << 8) | rawData[3] ; sumY += queue_accel[0][1];
  queue_accel[0][2] = ((int)rawData[4] << 8) | rawData[5] ; sumZ += queue_accel[0][2];
  destination[0] = ((float)sumX / DEPTH)/accel_scale;
  destination[1] = ((float)sumY / DEPTH)/accel_scale;
  destination[2] = ((float)sumZ / DEPTH)/accel_scale;
}

void readMagData(float * destination)
{
  byte rawData[6];
  long sumX=0,sumY=0,sumZ=0;   
  float x_ofs,y_ofs,z_ofs,x_avg_delta,y_avg_delta,z_avg_delta,avg_delta,x_scale,y_scale,z_scale;
  for (byte i = DEPTH-1; i > 0 ; i--) {
    queue_mag[i][0] = queue_mag[i-1][0];sumX += queue_mag[i-1][0];   
    queue_mag[i][1] = queue_mag[i-1][1];sumY += queue_mag[i-1][1];   
    queue_mag[i][2] = queue_mag[i-1][2];sumZ += queue_mag[i-1][2];   
   }
   while (! qmc_isReady());
  readBytes(QMC5883L_ADDRESS,qmc_x_lsb, 6, &rawData[0]); 
  queue_mag[0][0] = ((int)rawData[0]) | rawData[1] << 8 ; sumX += queue_mag[0][0];
  queue_mag[0][1] = ((int)rawData[2]) | rawData[3] << 8 ; sumY += queue_mag[0][1];
  queue_mag[0][2] = ((int)rawData[4]) | rawData[5] << 8 ; sumZ += queue_mag[0][2];
  x_ofs = (qmc_matrix[0] + qmc_matrix[1])/2;
  y_ofs = (qmc_matrix[2] + qmc_matrix[3])/2;
  z_ofs = (qmc_matrix[4] + qmc_matrix[5])/2;
  x_avg_delta = (qmc_matrix[1] - qmc_matrix[0])/2;
  y_avg_delta = (qmc_matrix[3] - qmc_matrix[2])/2;
  z_avg_delta = (qmc_matrix[5] - qmc_matrix[4])/2;
  avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta)/3;
  x_scale = avg_delta / x_avg_delta;
  y_scale = avg_delta / y_avg_delta;
  z_scale = avg_delta / z_avg_delta;
  destination[0] = (((float)sumX-x_ofs)*x_scale)/mag_scale;
  destination[1] = (((float)sumY-y_ofs)*y_scale)/mag_scale;
  destination[2] = (((float)sumZ-z_ofs)*z_scale)/mag_scale;
}

bool qmc_isReady(void) {
   byte status = readByte(QMC5883L_ADDRESS,qmc_status);
   return bitRead(status,0); 
}

bool mpu_isReady(void) {
   byte status = readByte(MPU6500_ADDRESS,mpu_status);
   return bitRead(status,0); 
}

byte readByte(byte address, byte subAddress)
{
  byte data;   
  Wire.beginTransmission(address);         
  Wire.write(subAddress);                 
  Wire.endTransmission(false);             
  Wire.requestFrom(address, (byte) 1);   
  data = Wire.read();                     
  return data;                             
}

void writeByte(byte address, byte subAddress, byte data)
{
  Wire.beginTransmission(address); 
  Wire.write(subAddress);           
  Wire.write(data);                 
  Wire.endTransmission();           
}

void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{ 
  Wire.beginTransmission(address);   
  Wire.write(subAddress);           
  Wire.endTransmission(false);       
  uint8_t i = 0;
  Wire.requestFrom(address, count); 
  while (Wire.available()) dest[i++] = Wire.read();         
}


Раздобыл проприетарный код для digital motion processing. Если будет время и необходимость вкручу сюда. И так хорошо работает
При кренах +/- 15 градусов, магнитный курс плавает в пределах 2х градусов. Нормальный результат для датчика, тем более что он нужен не для навигации по компасу.
А всего лишь удержать авторулевого на магнитном меридиане, пока лодка топчется на месте в слабый ветер и данные с GPS не дают точного направления движения.

[maxlab]

Всем наблюдающим привет!
Окончательная редакция магнитного указателя курса.
С возможностью калибровки. Потребление 20mA
Посмотрю как поведет себя в реальных условиях эксплуатации.
Следующая реинкарнация будет с квартернионами, DMP, и с прочей эйлеровской херней.

Спойлер

Код:

//LGT8F328P SSOP-20 32Mhz internal
#include <Wire.h>
#include <ModbusRtu.h>
#include <EEPROM.h>
#include <WDT.h>

#define call_button 3
#define txen 7
#define dev_id 2
#define zummer 2

#define mpu_adr 0x68
#define qmc_adr 0x0D
#define qmc_period 0x0B
#define qmc_cntrl_1 0x09
#define qmc_cntrl_2 0x0A
#define qmc_status 0x06
#define pwr_mgmt_1 0x6B
#define mpu_status 0x3A
#define int_en 0x38
#define gyro_out 0x43
#define accel_out 0x3B
#define qmc_out 0x00
#define depth 64

Modbus slave(dev_id, Serial, txen);

int gyro_offs[] = { 0, 0, 0 };
int accel_offs[] = { 0, 0, 0 };
int qmc_matrix[] = { -1, 1, -1, 1, -1, 1 };
float pitch_offs = 0, roll_offs = 0;

long lastTime=0;
float aver_pitch[depth], aver_roll[depth], aver_mag[depth];
float qmc_data[3];
float accel_data[3];
float gyro_data[3];

unsigned int fld_data[3];

void(* resetFunc) (void) = 0;

void setup() {
  Wire.begin();

  writeByte(mpu_adr, pwr_mgmt_1, 0x00);
  writeByte(mpu_adr, pwr_mgmt_1, 0x01);
  writeByte(qmc_adr, qmc_cntrl_2, 0x80);
  writeByte(qmc_adr, qmc_period, 0x01);
  writeByte(qmc_adr, qmc_cntrl_1, 0x0D);
  Serial.begin(38400);
  pinMode(call_button, INPUT_PULLUP);
  pinMode(zummer,OUTPUT);
  EEPROM.get(0, qmc_matrix[0]);
  EEPROM.get(4, qmc_matrix[1]);
  EEPROM.get(8, qmc_matrix[2]);
  EEPROM.get(12, qmc_matrix[3]);
  EEPROM.get(16, qmc_matrix[4]);
  EEPROM.get(20, qmc_matrix[5]);
  EEPROM.get(24, gyro_offs[0]);
  EEPROM.get(28, gyro_offs[1]);
  EEPROM.get(32, gyro_offs[2]);
  EEPROM.get(36, accel_offs[0]);
  EEPROM.get(40, accel_offs[1]);
  EEPROM.get(44, accel_offs[2]);
  EEPROM.get(50, pitch_offs);
  EEPROM.get(60, roll_offs);
  // Serial.println(); 
  // Serial.println("QMC matrix is:"); 
  // Serial.print(qmc_matrix[0]); 
  // Serial.print("\t"); 
  // Serial.print(qmc_matrix[1]); 
  // Serial.print("\t"); 
  // Serial.print(qmc_matrix[2]);
  // Serial.print("\t"); 
  // Serial.print(qmc_matrix[3]); 
  // Serial.print("\t"); 
  // Serial.print(qmc_matrix[4]);
  // Serial.print("\t"); 
  // Serial.print(qmc_matrix[5]); 
  // Serial.println();
  // Serial.println("GYRO offset is:"); 
  // Serial.print(gyro_offs[0]); 
  // Serial.print("\t"); 
  // Serial.print(gyro_offs[1]); 
  // Serial.print("\t"); 
  // Serial.print(gyro_offs[2]);
  // Serial.println();
  // Serial.println("ACCEL offset is:"); 
  // Serial.print(accel_offs[0]); 
  // Serial.print("\t"); 
  // Serial.print(accel_offs[1]); 
  // Serial.print("\t"); 
  // Serial.print(accel_offs[2]);
  // Serial.println(); 
  // Serial.println("Roll/Pitch correction is:"); 
  // Serial.print(roll_offs); 
  // Serial.print("\t"); 
  // Serial.print(pitch_offs);
  // delay(5000);
  slave.start();   
  wdt_enable(WTO_8S);
  lastTime = millis();
}

void loop() {
  static unsigned int count = 0;
  static float g_roll, g_pitch, pitch, roll;     
  check_button();
  wdt_reset();
  long newTime = millis();
  float elapsedTime = (float)(newTime - lastTime) / 1000.0f;

  readMagData(qmc_data);
  readAccelData(accel_data);
  readGyroData(gyro_data);

  float r = sqrtf(pow(accel_data[0],2)+pow(accel_data[1],2)+pow(accel_data[2],2));
  roll = (-asin(accel_data[1]/r)* 180 / PI) - roll_offs;
  pitch = (asin(accel_data[0]/r)* 180 / PI) - pitch_offs;       

  g_roll += gyro_data[0] * elapsedTime;
  g_pitch += gyro_data[1] * elapsedTime;
  pitch = 0.90 * g_pitch + 0.10 * pitch;
  roll = 0.90 * g_roll + 0.10 * roll;
 
  float sum_pitch = 0, sum_roll = 0;
  for (byte i = depth - 1; i > 0; i--) {
    aver_pitch[i] = aver_pitch[i - 1];
    sum_pitch += aver_pitch[i - 1];
    aver_roll[i] = aver_roll[i - 1];
    sum_roll += aver_roll[i - 1];
  }
  aver_pitch[0] = pitch;
  aver_roll[0] = roll;

  roll = (sum_roll + roll) / depth;
  pitch = (sum_pitch + pitch) / depth;
  g_roll = roll;
  g_pitch = pitch;

  float pitch_rad = PI / 180 * pitch ;
  float roll_rad = PI / 180 * roll;
 
  float cx = qmc_data[0] * cos(pitch_rad) + qmc_data[1] * sin(roll_rad) * sin(pitch_rad) - qmc_data[2] * cos(roll_rad) * sin(pitch_rad);
  float cy = qmc_data[1] * cos(roll_rad) + qmc_data[2] * sin(roll_rad);
 
  int az_rad = 180 / PI * atan2(cy, cx);
  int azimuth = round((int)(az_rad + 360.0) % 360);
 
  int sum_mag = 0;
  for (byte i = depth - 1; i > 0; i--) {
    aver_mag[i] = aver_mag[i - 1];
    sum_mag += aver_mag[i - 1];
  }
  aver_mag[0] = azimuth;
  azimuth = (azimuth + sum_mag) / depth;

  fld_data[0] = round(pitch);
  fld_data[1] = round(roll);
  fld_data[2] = azimuth;
  slave.poll(fld_data, 3);

  // Serial.println(); 
  // Serial.print(roll); 
  // Serial.print("\t"); 
  // Serial.print(pitch);
  // Serial.print("\t"); 
  // Serial.print(azimuth);

  if (count % 100 == 0) digitalToggle(LED_BUILTIN);
  lastTime = newTime;
  count++;
 
}

void readAccelData(float * destination)
{
  byte rawData[6];
  readBytes(mpu_adr, accel_out, 6, &rawData[0]);
  // destination[0] = (float)(((int)rawData[0] << 8 | rawData[1]) - accel_offs[0] ) / 16384.0;
  // destination[1] = (float)(((int)rawData[2] << 8 | rawData[3]) - accel_offs[1] ) / 16384.0;
  // destination[2] = (float)(((int)rawData[4] << 8 | rawData[5]) - accel_offs[2] ) / 16384.0;
  destination[0] = (float)(((int)rawData[0] << 8 | rawData[1]) ) / 16384.0;
  destination[1] = (float)(((int)rawData[2] << 8 | rawData[3]) ) / 16384.0;
  destination[2] = (float)(((int)rawData[4] << 8 | rawData[5]) ) / 16384.0;

}

void readGyroData(float * destination)
{
  byte rawData[6];
  readBytes(mpu_adr, gyro_out, 6, &rawData[0]);
  destination[0] = (float)(((int)rawData[0] << 8 | rawData[1]) - gyro_offs[0]) /  131.0;
  destination[1] = (float)(((int)rawData[2] << 8 | rawData[3]) - gyro_offs[1]) /  131.0;
  destination[2] = (float)(((int)rawData[4] << 8 | rawData[5]) - gyro_offs[2]) /  131.0;
}

void readMagData(float* destination) {
  byte rawData[6];
  while (!qmc_isReady()){}
  readBytes(qmc_adr, qmc_out, 6, &rawData[0]);
  int mX = (int)rawData[0] | rawData[1] << 8;
  int mY = (int)rawData[2] | rawData[3] << 8;
  int mZ = (int)rawData[4] | rawData[5] << 8;
  float x_ofs = (float)(qmc_matrix[0] + qmc_matrix[1]) / 2.0;
  float y_ofs = (float)(qmc_matrix[2] + qmc_matrix[3]) / 2.0;
  float z_ofs = (float)(qmc_matrix[4] + qmc_matrix[5]) / 2.0;
  float x_avg_delta = (float)(qmc_matrix[1] - qmc_matrix[0]) / 2.0;
  float y_avg_delta = (float)(qmc_matrix[3] - qmc_matrix[2]) / 2.0;
  float z_avg_delta = (float)(qmc_matrix[5] - qmc_matrix[4]) / 2.0;
  float avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3.0;
  float x_scale = avg_delta / x_avg_delta;
  float y_scale = avg_delta / y_avg_delta;
  float z_scale = avg_delta / z_avg_delta;
  destination[0] = (float)((mX - x_ofs) * x_scale) ;
  destination[1] = (float)((mY - y_ofs) * y_scale) ;
  destination[2] = (float)((mZ - z_ofs) * z_scale) ;
}

void check_button() {
  byte rawData[6];
  long gX = 0, gY = 0, gZ = 0;
  long a_X = 0, a_Y = 0, a_Z = 0;
  float pitch_err = 0, roll_err = 0; 
  if (!digitalRead(call_button)) {
    zummer_play(1);   
    //Serial.println();
    //Serial.println("Put MPU on horizontal surface.");
    for (int i = 0; i < 5000; i++) {
      readBytes(mpu_adr, gyro_out, 6, &rawData[0]);
      gX += (int)rawData[0] << 8 | rawData[1];
      gY += (int)rawData[2] << 8 | rawData[3];
      gZ += (int)rawData[4] << 8 | rawData[5];
      readBytes(mpu_adr, accel_out, 6, &rawData[0]);
      a_X += (int)rawData[0] << 8 | rawData[1];
      a_Y += (int)rawData[2] << 8 | rawData[3];
      a_Z += (int)rawData[4] << 8 | rawData[5];
      if (i % 150 == 0) {
        digitalToggle(LED_BUILTIN);
        wdt_reset();
        // Serial.print(".");
        zummer_play(5);                                   
      }
    }
    gyro_offs[0] = gX / 5000;
    gyro_offs[1] = gY / 5000;
    gyro_offs[2] = gZ / 5000;
    accel_offs[0] = a_X/5000;
    accel_offs[1] = a_Y/5000;
    accel_offs[2] = a_Z/5000;
    EEPROM.put(24, gyro_offs[0]);
    EEPROM.put(28, gyro_offs[1]);
    EEPROM.put(32, gyro_offs[2]);
    EEPROM.put(36, accel_offs[0]);
    EEPROM.put(40, accel_offs[1]);
    EEPROM.put(44, accel_offs[2]);
    // Serial.println();
    // Serial.println("GYRO offset is:"); 
    // Serial.print(gyro_offs[0]); 
    // Serial.print("\t"); 
    // Serial.print(gyro_offs[1]); 
    // Serial.print("\t"); 
    // Serial.print(gyro_offs[2]);
    // Serial.println();
    // Serial.println("ACCEL offset is:"); 
    // Serial.print(accel_offs[0]); 
    // Serial.print("\t"); 
    // Serial.print(accel_offs[1]); 
    // Serial.print("\t"); 
    // Serial.print(accel_offs[2]);
    zummer_play(2);   
    // Serial.println();
    // Serial.println("Calculate roll/pitch correction");
    for (int i = 0; i < 5000; i++) {
      readBytes(mpu_adr, accel_out, 6, &rawData[0]);
      // float aX = (float)(((int)rawData[0] << 8 | rawData[1]) - accel_offs[0]) / 16384.0;
      // float aY = (float)(((int)rawData[2] << 8 | rawData[3]) - accel_offs[1]) / 16384.0;
      // float aZ = (float)(((int)rawData[4] << 8 | rawData[5]) - accel_offs[2]) / 16384.0;
      float aX = (float)(((int)rawData[0] << 8 | rawData[1]) ) / 16384.0;
      float aY = (float)(((int)rawData[2] << 8 | rawData[3]) ) / 16384.0;
      float aZ = (float)(((int)rawData[4] << 8 | rawData[5]) ) / 16384.0;

      float r = sqrtf(pow(aX,2)+pow(aY,2)+pow(aZ,2));     
      roll_err += (-asin(aY/r)* 180 / PI);
      pitch_err += (asin(aX/r)* 180 / PI);
      if (i % 100 == 0) {
        digitalToggle(LED_BUILTIN);
        wdt_reset();
        // Serial.print('.');
        zummer_play(5);                                   
      }
    }
    pitch_offs = pitch_err / 5000.0;
    roll_offs = roll_err / 5000.0;
    EEPROM.put(50, pitch_offs);
    EEPROM.put(60, roll_offs);
    // Serial.println(); 
    // Serial.println("Roll/Pitch correction is:"); 
    // Serial.print(roll_offs); 
    // Serial.print("\t"); 
    // Serial.print(pitch_offs); 
    zummer_play(3);   
    // Serial.println();
    // Serial.println("Take QMC and twirl around.");
    for (int i = 0; i < 5000; i++) {
      while (!qmc_isReady())
      readBytes(qmc_adr, qmc_out, 6, &rawData[0]);
      int X = ((int)rawData[0]) | rawData[1] << 8;
      int Y = ((int)rawData[2]) | rawData[3] << 8;
      int Z = ((int)rawData[4]) | rawData[5] << 8;
      delay(10);
      if (X < 0 && X < qmc_matrix[0]) qmc_matrix[0] = X;
      if (X > 0 && X > qmc_matrix[1]) qmc_matrix[1] = X;
      if (Y < 0 && Y < qmc_matrix[2]) qmc_matrix[2] = Y;
      if (Y > 0 && Y > qmc_matrix[3]) qmc_matrix[3] = Y;
      if (Z < 0 && Z < qmc_matrix[4]) qmc_matrix[4] = Z;
      if (Z > 0 && Z > qmc_matrix[5]) qmc_matrix[5] = Z;
      if (i % 50 == 0) {
        wdt_reset();
        digitalToggle(LED_BUILTIN);
        // Serial.print('.');
        zummer_play(5);                                   
      }
    }
    EEPROM.put(0, qmc_matrix[0]);
    EEPROM.put(4, qmc_matrix[1]);
    EEPROM.put(8, qmc_matrix[2]);
    EEPROM.put(12, qmc_matrix[3]);
    EEPROM.put(16, qmc_matrix[4]);
    EEPROM.put(20, qmc_matrix[5]);
    // Serial.println();
    // Serial.println("QMC matrix is:"); 
    // Serial.print(qmc_matrix[0]); 
    // Serial.print("\t"); 
    // Serial.print(qmc_matrix[1]); 
    // Serial.print("\t"); 
    // Serial.print(qmc_matrix[2]);
    // Serial.print("\t"); 
    // Serial.print(qmc_matrix[3]); 
    // Serial.print("\t"); 
    // Serial.print(qmc_matrix[4]);
    // Serial.print("\t"); 
    // Serial.print(qmc_matrix[5]); 
    // Serial.println();
    zummer_play(4);       
    resetFunc();
  }
}

void zummer_play(byte par) {
  // Morse "K"
  switch (par) {
    case 1:
      // Morse "K"
      wdt_reset();
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(250);
      break;
    case 2:
      // Morse "C"
      wdt_reset();
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(250);
      break;   
    case 3:
      // Morse "S"
      wdt_reset();
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,LOW);
      delay(250);
      break;
    case 4:
      // Morse "Q"
      wdt_reset();
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(250);
      digitalWrite(zummer,LOW);
      delay(100);
      digitalWrite(zummer,HIGH);
      delay(500);
      digitalWrite(zummer,LOW);
      delay(250);
      break;
    case 5:
      digitalWrite(zummer,HIGH);
      delay(10);
      digitalWrite(zummer,LOW);
      break;
    default: break;
  }
}

bool qmc_isReady(void) {
  byte status = readByte(qmc_adr, qmc_status);
  return bitRead(status, 0);
}

byte readByte(byte address, byte subAddress) {
  byte data;
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.endTransmission(false);
  Wire.requestFrom(address, (byte)1);
  data = Wire.read();
  return data;
}

void writeByte(byte address, byte subAddress, byte data) {
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.write(data);
  Wire.endTransmission();
}

void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest) {
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.endTransmission(false);
  uint8_t i = 0;
  Wire.requestFrom(address, count);
  while (Wire.available()) dest[i++] = Wire.read();
}


Два устройства в шине ModBus уже есть, пора запилить master-device.

[maxlab]

Всем привет! Пока то да сё, замакетировал master-device.
Задачи возлагаются на него следующие:

1 Опрос всех немыслимых навигационных и сервисных датчиков.
2 Обработка данных, запись на флеш исторических событий (log).
3 Функция автопилотирования и сигналов вахтенному экипажу
4 Охранная функция (GSM SIM800L)+LORA
5 Графический интерфейс
6 WEB сервер для доступа с телефона.
7 Еще не придумал....

В качестве процессора ESP32. Задачи буду распараллеливать, а это значит привет мьютексам, таскам и семафорам
Надеюсь его производительности хватит.

[maxlab]

Ночь прошла результативно
Изучал устройство продвинутой библиотеки для ModBus RTU отсюда https://github.com/emelianov/modbus-esp8266
Не все понятно с их асинхронщиной, и нет пока уверенности что все правильно понял что понял (каламбурчик)
Но датчики стабильно и корректно опрашиваются. Ошибок чтения нет

Спойлер

Код:

#include <Arduino.h>
#include <ModbusRTU.h>

#define TXEN 33
#define U2RXD 16
#define U2TXD 17
#define MODBUS_HW_SERIAL Serial2

#define SLAVE_ADRS      0 // Начальный адрес

#define Y_DEV1_ID       1 // Датчик температуры,атмосферного давления,влажности и качества воздуха
#define Y_DEV1_REGCOUNT 4

#define Y_DEV2_ID       2 // Датчик крена,тангажа и магнитного курса
#define Y_DEV2_REGCOUNT 3

ModbusRTU modbus;
// Callback функция для диагностики
bool cb(Modbus::ResultCode event, uint16_t transactionId, void* data)
{
  if (event != Modbus::EX_SUCCESS) {
    Serial.print("Request result: 0x");
    Serial.println(event, HEX);
  }
  return true;
}

void setup()
{
  Serial.begin(115200);
  MODBUS_HW_SERIAL.begin(38400, SERIAL_8N1, U2RXD, U2TXD);
  Serial.println();
  modbus.begin(&MODBUS_HW_SERIAL,TXEN);
  modbus.master();
}

uint16_t dev1_data[Y_DEV1_REGCOUNT];
uint16_t dev2_data[Y_DEV2_REGCOUNT];

void loop()
{
  if (!modbus.slave()) {   
    modbus.readHreg(Y_DEV1_ID, SLAVE_ADRS, dev1_data, Y_DEV1_REGCOUNT, cb);
    while(modbus.slave()) {modbus.task();delay(15);}
  }
  if (!modbus.slave()) {   
    modbus.readHreg(Y_DEV2_ID, SLAVE_ADRS, dev2_data, Y_DEV2_REGCOUNT, cb);
    while(modbus.slave()) {modbus.task();delay(15);}
  }
    Serial.println();
    Serial.print("Quality of air:");Serial.print(dev1_data[0]);Serial.print("\t");
    Serial.print("Humidity:");Serial.print((float)dev1_data[1]/10,2);Serial.print("\t");
    Serial.print("Temperature:");Serial.print((float)dev1_data[2]/10,2);Serial.print("\t");
    Serial.print("Pressure:");Serial.print((float)dev1_data[3]/10,2);Serial.print("\t");
    Serial.print("Pitch:");Serial.print((int16_t)dev2_data[0]);Serial.print("\t");
    Serial.print("Roll:");Serial.print((int16_t)dev2_data[1]);Serial.print("\t");
    Serial.print("Azimuth:");Serial.print(dev2_data[2]);Serial.print("\t");
}




Сейчас пока на фишки RTOS заморачиваться не буду. Потому что датчиков будет много, и не решил какие и с какой частотой нужно опрашивать.
На Core1 по умолчанию пусть рутина молотит... а там посмотрим

[roman.com]

maxlab писал(а):

6 WEB сервер для доступа с телефона.

WEB сервер ? ты что ! хочешь чтоб твою систему взломали ?

любой школьник...
используя простой IP сканер...

1.jpg

(85.29 KiB) Скачиваний: 55

и сканер портов...

2.jpg

(85.8 KiB) Скачиваний: 55

подключится к твоему WEB серверу...

браузер_1.jpg

(179.81 KiB) Скачиваний: 51

и с помощью сниффера причитает все твои логины и пароли))

анализатор_1.jpg

(161.67 KiB) Скачиваний: 55

или с помощью трояна украдёт все твои данные WEB сервер с телефона... (браузер сохраняет все логины... пароли... куки... ключи шифрования всех WEB серверов к котором подключается телефон).


поэтому никаких WEB серверов ! ))
пишем своё приложение на телефоне... с использованием UDP... для защиты от любых сканеров))
UDP сервер ни один сканер не найдёт ! ))

виртуальный пульт.jpg

(55.8 KiB) Скачиваний: 52

пульт управления.jpg

(96.76 KiB) Скачиваний: 48

и т.д.
при этом приложение на телефоне НЕ сохраняет все логины... пароли... куки... ключи шифрования.
всё это зашито в приложении... откуда достать это не так то и просто))
в отличии от браузера, который хранит это всё в телефоне, в отдельном файле, в открытом виде ! ))

maxlab писал(а):

Изучал устройство продвинутой библиотеки для ModBus RTU

ModBus я не использую... у меня есть Ethernet с PoE ))

схема_1.jpg

(196.87 KiB) Скачиваний: 52

-во первых он работает быстрей (10 мбит/c против ModBus 115200)...
-во вторых PoE избавляет меня от протягивания лишних проводов...
-в третьих более универсальная схема (не нужны переходники ModBus <> Ethernet, ModBus <> Wi-Fi и т.д.)
и т.д.

[maxlab]

roman.com

Да ну...нахер...

Что то в море я не видел школьников с ноутами и с веслами наперевес.
То, о чем ты говоришь, имеет место в обычной жизни обывателя. Но когда ты от берега отвалишь хотя бы на пару миль, жизнь начинает играть другими красками.
WEB сервер, локального исполнения и без доступа к WAN. Я понимаю что можно для телефона написать приложение и использовать TCP сокеты. Но это все сложно и
время-затратно. Поэтому пара тройка страниц HTML&CSS&JS в рамках веб сервера на esp32 - это быстро и дешево. Мощность wi-fi передатчика можно программно притушить,
чтобы у ботанов от IT не возбуждать не здоровое любопытство когда на стоянке или близко к берегу. Да и когда стоишь это все обесточено кроме пожарной сигнализации.

По поводу ModBus. Это просто и надежно. Видеопоток мне гонять не нужно, а с задачами навигации и пилотирования скорости на шине и в 38400 за глаза хватит. Парусная лодка такого размера, как я строю,
ходит не быстрее велосипеда. Т.е. 20 км/ч для нее недостижимый потолок. Это водоизмещающее плавание а не глиссирование. Хотя, тут коллеги меня уже почти переубедили пересесть на CAN шину.
Потому что тогда можно будет наращивать аппаратное обеспечение и стандартным оборудованием Raymarine или B&G например. Некоторые поставщики морского оборудования уже тоже используют
Ethernet в качестве шины. Но ассоциацией NMEA это еще не стандартизировано и не сертифицировано. Это называется OneNet. Кстати, кто то мне рассказывал что на наших современных субмаринах уже
на всю катушку оптику юзают.

[roman.com]

maxlab писал(а):

Мощность wi-fi передатчика можно программно притушить,
чтобы у ботанов от IT не возбуждать не здоровое любопытство когда на стоянке или близко к берегу.

гуляют себе люди... с телефонами с Wi-Fi...
а тут стоит лодка.. .с бесплатным Wi-Fi ! ))

1.jpg

(85.74 KiB) Скачиваний: 40

О ! бесплатный Wi-Fi ! )) надо подключится))


Каждый раз тушить Мощность wi-fi передатчика... чтоб нельзя было подключиться... это не серьёзно.

[Martian]

roman.com, если сумеете взломать мой веб-сервер через вай-фай, дам шоколадку. Но вряд-ли. ,Потому что сниффер, трояны... у Вас богатый набор слов и совершенно отсутствует понимание процесса. И ещё: UDP сканируется.

[roman.com]

Martian писал(а):

UDP сканируется.

чем ? приведите хоть один пример...
или опять пустая болтология ? ))