Сб май 07, 2022 00:35:56
Сб май 07, 2022 12:28:52
Сб май 07, 2022 13:47:31
Сб май 14, 2022 00:46:51
// 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;
}
Вс окт 02, 2022 22:26:43
Ср окт 19, 2022 21:11:51
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)
Чт ноя 03, 2022 20:28:24
# Китайский клон на 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))
Вс ноя 06, 2022 13:33:26
# Китайский клон на 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))
Вс ноя 06, 2022 13:43:09
Вс ноя 06, 2022 13:51:33
Вс ноя 06, 2022 14:41:17
Чт ноя 10, 2022 19:59:49
// 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();
}
Ср ноя 23, 2022 13:22:33
//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();
}
Пт ноя 25, 2022 19:39:04
Сб ноя 26, 2022 13:21:13
#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");
}
Сб ноя 26, 2022 16:23:13
Сб ноя 26, 2022 21:46:29
Вс ноя 27, 2022 01:42:31
Вс ноя 27, 2022 02:18:52
Вс ноя 27, 2022 12:10:56