Создание собственной системы стабилизации
серва быстрая цыфровая не дёргается как припадошная в отличии от стандартного wii - немного уводит по курсу
Это трикоптер что ли? 😃
Имхо любую систему стабилизации стоит настраивать на квадрике - чтобы не вносить дополнительную неопределенность в виде серв, механики и прочего.
Остались в принципе лучи и можно делать квадрик (регуль и движек есть), но пока не могу, мне ещё платы заказать надо…
есть некоторые мысли на счёт акселя, но попробую наверно уже на выходных…
Полетал вчера, побадался с ветром, особенно видно в под конец - не мог посадить аппарат, летал в акро, аксель так и не победил 😦
может что в драйвере неправильно сделал?
#include "board.h"
#define LSM330ACC_ADDRESS 0x19
#define LSM330ACC_DEVICE_SIGNATURE 0xE0
#define LSM330_CTRL_REG1_A 0x20 /* rw */
#define LSM330_CTRL_REG2_A 0x21 /*rw */
#define LSM330_CTRL_REG3_A 0x22 /*rw */
#define LSM330_CTRL_REG4_A 0x23 /*rw */
#define LSM330_CTRL_REG5_A 0x24 /*rw */
#define LSM330_CTRL_REG6_A 0x25 /*rw */
#define LSM330_REF_DCAPTURE_A 0x26 /*rw */
#define LSM330_STATUS_REG_A 0x27 /*r */
#define LSM330_OUT_X_L_A 0x28 /*r */
//#define LSM330_OUT_X_H_A 0x29 /*r */
//#define LSM330_OUT_Y_L_A 0x2A /*r */
//#define LSM330_OUT_Y_H_A 0x2B /*r */
//#define LSM330_OUT_Z_L_A 0x2C /*r */
//#define LSM330_OUT_Z_H_A 0x2D /*r */
#define LSM330_FIFO_CTRL_REG_A 0x2E /*rw */
#define LSM330_FIFO_SRC_REG_A 0x2F /*r */
#define LSM330_INT1_CFG_A 0x30 /*rw */
#define LSM330_INT1_SRC_A 0x31 /*r */
#define LSM330_INT1_THS_A 0x32 /*rw */
#define LSM330_INT1_DURATION_A 0x33 /*rw */
#define LSM330_INT2_CFG_A 0x34 /*rw */
#define LSM330_INT2_SOURCE_A 0x35 /*r */
#define LSM330_INT2_THS_A 0x36 /*rw */
#define LSM330_INT2_DURATION_A 0x37 /*rw */
#define LSM330_CLICK_CFG_A 0x38 /*rw */
#define LSM330_CLICK_SRC_A 0x39 /*rw */
#define LSM330_CLICK_THS_A 0x3A /*rw */
#define LSM330_TIME_LIMIT_A 0x3B /*rw */
#define LSM330_TIME_LATENCY_A 0x3C /*rw */
#define LSM330_TIME_WINDOW_A 0x3D /*rw */
/* LSM330_CTRL_REG1_A */
#define LSM330_A_ODR_PDN 0x0 /* Power-down mode */
#define LSM330_A_ODR_1 0x10 /* Normal / low-power mode (1 Hz) */
#define LSM330_A_ODR_10 0x20 /* Normal / low-power mode (10 Hz) */
#define LSM330_A_ODR_25 0x30 /* Normal / low-power mode (25 Hz) */
#define LSM330_A_ODR_50 0x40 /* Normal / low-power mode (50 Hz) */
#define LSM330_A_ODR_100 0x50 /* Normal / low-power mode (100 Hz) */
#define LSM330_A_ODR_200 0x60 /* Normal / low-power mode (200 Hz) */
#define LSM330_A_ODR_400 0x70 /* Normal / low-power mode (400 Hz) */
#define LSM330_A_ODR_LP162 0x80 /* Low-power mode (1.620 kHz) */
#define LSM330_A_ODR_HI 0x81 /* Normal (1.344 kHz) / low-power mode (5.376 kHz) */
#define LSM330_A_Acsis 0X07
/* CTRL_REG2_A */
#define LSM330_A_HPM0 0 /* Normal mode (reset reading HP_RESET_FILTER)*/
#define LSM330_A_HPM1 0x40 /* Reference signal for filtering */
#define LSM330_A_HPM2 0x80 /* Normal mode */
#define LSM330_A_HPM3 0xc0 /* Autoreset on interrupt event */
#define LSM330_A_HPCF0 0 /* High-pass filter cutoff frequency selection */
#define LSM330_A_HPCF1 0x10 /* High-pass filter cutoff frequency selection */
#define LSM330_A_HPCF2 0x20 /* High-pass filter cutoff frequency selection */
#define LSM330_A_HPCF3 0x30 /* High-pass filter cutoff frequency selection */
#define LSM330_A_FDS 0x08 /* Filtered data selection. Default value: 0 (0: internal filter bypassed; 1: data from internal filter sent to output register and FIFO) */
/* CTRL_REG3_A */
#define LSM330_A_I1_CLICK 0x80 /* CLICK interrupt on INT1_A. Default value 0. (0: Disable; 1: Enable) */
#define LSM330_A_I1_AOI1 0x40 /* AOI1 interrupt on INT1_A. Default value 0. (0: Disable; 1: Enable) */
#define LSM330_A_I1_DRDY1 0x10 /* DRDY1 interrupt on INT1_A. Default value 0.(0: Disable; 1: Enable) */
#define LSM330_A_I1_DRDY2 0x08 /* DRDY2 interrupt on INT1_A. Default value 0.(0: Disable; 1: Enable) */
#define LSM330_A_I1_WTM 0x04 /* FIFO watermark interrupt on INT1_A. Default value 0.(0: Disable; 1: Enable)*/
#define LSM330_A_I1_OVERRUN 0x02 /* FIFO overrun interrupt on INT1_A. Default value 0. (0: Disable; 1: Enable) */
/*CTRL_REG4_A */
#define LSM330_BDU 0x80 /* Block data update. Default value: 0(0: continuous update; 1: output registers not updated until MSB and LSB reading) */
#define LSM330_BLE 0x40 /* Big/little endian data selection. Default value 0.(0: Data LSB at lower address; 1: Data MSB at lower address) */
/*FS1-FS0 Full-scale selection. default value: 00 (00: +/- 2G; 01: +/- 4G; 10: +/- 8G; 11: +/- 16G) */
#define LSM330_A_FS2G 0
#define LSM330_A_FS4G 0x10
#define LSM330_A_FS8G 0x20
#define LSM330_A_FS16G 0x30
#define LSM330_A_HR 0x08 /*Normal mode: default value: 0(0: normal mode disable; 1: normal mode enable */
#define LSM330_SIM 0x01 /* SPI serial interface mode selection. Default value: 0(0: 4-wire interface; 1: 3-wire interface) */
/* CTRL_REG5_A */
#define LSM330_A_BOOT 0x80 /* Reboot memory content. Default value: 0 (0: normal mode; 1: reboot memory content) */
#define LSM330_A_FIFO_EN 0x40 /* FIFO enable. Default value: 0(0: FIFO disable; 1: FIFO enable) */
#define LSM330_A_LIR_INT1 0x08 /* Latch interrupt request on INT1_SRC_A register, with INT1_SRC_A register cleared by reading INT1_SRC_A itself. Default value: 0.(0: interrupt request not latched; 1: interrupt request latched) */
#define LSM330_A_D4D_INT1 0x04 /* 4D enable: 4D detection is enabled on INT1_A when 6D bit on INT1_CFG_A is set to 1. */
/* CTRL_REG6_A */
#define LSM330_A_I2_CLICKen 0x80 /* Click interrupt on INT2_A. Default value 0. */
#define LSM330_A_I2_INT1 0x40 /* Interrupt 1 function enabled on INT2_A. Default 0. */
#define LSM330_A_BOOT_I2 0x10 /* Boot on INT2_A. */
#define LSM330_A_H_LACTIVE 0x02 /* 0: interrupt active high; 1: interrupt active low. */
/* STATUS_REG_A */
#define LSM330_A_ZYXOR 0x80 /* X-, Y- and Z-axis data overwrite. Default value: 0 (0: no overwrite has occurred; 1: a new set of data has overwritten the previous ones) */
#define LSM330_A_ZOR 0x40 /* Z-axis data overwrite. Default value: 0(0: no overrun has occurred; 1: a new data for the Z-axis has overwritten the previous one) */
#define LSM330_A_YOR 0x20 /* Y-axis data overwrite. Default value: 0 (0: no overwrite has occurred;1: new data for the Y-axis has overwritten the previous data) */
#define LSM330_A_XOR 0x10 /* X-axis data overwrite. Default value: 0 (0: no overwrite has occurred;1: new data for the X-axis has overwritten the previous data) */
#define LSM330_A_ZYXDA 0x08 /* X-, Y- and Z-axis new data available. Default value: 0(0: a new set of data is not yet available; 1: a new set of data is available) */
#define LSM330_A_ZDA 0x04 /* Z-axis new data available. Default value: 0(0: new data for the Z-axis is not yet available;1: new data for the Z-axis is available) */
#define LSM330_A_YDA 0x02 /* Y-axis new data available. Default value: 0 (0: new data for the Y-axis is not yet available;1: new data for the Y-axis is available) */
#define LSM330_A_XDA 0x01 /* X-axis new data available. Default value: 0 (0: new data for the X-axis is not yet available;1: new data for the X-axis is available) */
extern uint16_t acc_1G;
static void LSM330ACCInit(void);
static void LSM330ACCRead(int16_t *accelData);
static void LSM330ACCAlign(int16_t *accelData);
bool lsm330accDetect(sensor_t *acc)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(LSM330ACC_ADDRESS, LSM330_CTRL_REG1_A , 1, &sig);
sig = sig <<5;
if (!ack || sig != LSM330ACC_DEVICE_SIGNATURE)
return false;
acc->init = LSM330ACCInit;
acc->read = LSM330ACCRead;
acc->align = LSM330ACCAlign;
return true;
}
static void LSM330ACCInit(void)
{
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG1_A, 0x57);// LSM330_A_ODR_400 | LSM330_A_Acsis );
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG2_A, 0x00);
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG3_A, 0x00);
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG4_A, 0x98);// LSM330_A_FS8G );
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG5_A, 0x00);
delay(10);
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG6_A, 0x00);
acc_1G = 256;
}
static void LSM330ACCRead(int16_t *accelData)
{
uint8_t buf[6];
i2cRead(LSM330ACC_ADDRESS, LSM330_OUT_X_L_A +0x80, 6, buf);
accelData[0] = (buf[1] << 8) | buf[0];
accelData[1] = (buf[3] << 8) | buf[2];
accelData[2] = (buf[5] << 8) | buf[4];
}
static void LSM330ACCAlign(int16_t *accelData)
{
int16_t temp[2];
temp[0] = accelData[0];
temp[1] = accelData[1];
// official direction is RPY
accelData[0] = temp[1]/32;
accelData[1] = -temp[0]/32;
accelData[2] = accelData[2]/32;
}
хм… хоть лсм-ка у меня есть но пока ее еще не подключал… но… есть отличия настройки лсм-ки от виевских.
Возможно, ошибся темой. Не знаю куда это запостить, но тут есть интересное видео
по квадрикам
Там идет теория первые минут 9-10, ее можно пропустить.
По ЛСМ-ке. Вас старая версия чипа 5Х7мм? Если да,то попробуйте
// official direction is RPY
accelData[0] = temp[1]/32;
accelData[1] = -temp[0]/32;
accelData[2] = accelData[2]/32;
поиграться с делителем.У моего знакомого аналогичный датчик стал адекватно работать с делителем 1024)))
Сам пользую аналогичный чип,но уже в новом форм факторе и с немного другими адресами и функционалом регистров.
хм… хоть лсм-ка у меня есть но пока ее еще не подключал… но… есть отличия настройки лсм-ки от виевских.
а какие?
#define LSM330ACC_DEVICE_SIGNATURE 0xE0
.......................................................................
bool lsm330accDetect(sensor_t *acc)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(LSM330ACC_ADDRESS, LSM330_CTRL_REG1_A , 1, &sig);
sig = sig <<5;
if (!ack || sig != LSM330ACC_DEVICE_SIGNATURE)
return false;
acc->init = LSM330ACCInit;
acc->read = LSM330ACCRead;
acc->align = LSM330ACCAlign;
return true;
}
это я подсунул младшие биты первого регистра (они не меняются) для автоопределения датчика, остальное по сути как в вие, только поддержки вием лсм-ки нет, или уже есть?
У моего знакомого аналогичный датчик стал адекватно работать с делителем 1024)))
а я пробовал с 1023 может в этом ошибался?
Сам пользую аналогичный чип,но уже в новом форм факторе и с немного другими адресами и функционалом регистров.
а я не успел купить - лавочку прикрыли 😦
temp[] - это что? коэффициенты меняют длину вектора от акселя, ВИЮ важна длина?
temp - это я ворочал датчик так и осталось, а да делители важна для калибровки -
acc_1G = 256; а потом делителем подгоняется к этой длинне вектора
accelData[0] = temp[1]/32;
accelData[1] = -temp[0]/32;
accelData[2] = accelData[2]/32;
можно попробовать 1g и там 8192 обозвать тогда делитель не нужен, но чёт сомневаюсь что что-то изменится…
acc_1G = 256 и делитель 32 в принципе дают векторы правильные по всем осям, если ставить что-нибудь другое то не досчитывает по осям X,Y…
Выход один - нужна калибровка по всем осям наверно…
Только я не пойму, почему за полную шкалу берётся 16 бит, ведь показания в дополнительном коде и старший бит отвечает за знак, т.е. реальные показания это младших 15 бит, или я что-то не понимаю или сам запутался?
Так вот тараканы зашевелились: полная шкала 15бит(у ДУСов так и в Wii тоже
#if defined(ITG3200)
////////////////////
GYRO_ORIENTATION( ((rawADC[0]<<8) | rawADC[1])/4 , // range: +/- 8192; +/- 2000 deg/sec
8192*4 = 32768 ну если дотошно то должно быть 32767 ибо 32768 уже -1 получается)
так почемууу? аксель считается от 16 бит т.е. 65535 ??? они чё на бивнях считали что-ли? 😃
8192*4 = 32768 ну если дотошно то должно быть 32767 ибо 32768 уже -1 получается) так почемууу? аксель считается от 16 бит т.е. 65535 ??? они чё на бивнях считали что-ли?
погоди, не горячись, что uint16_t, что int16_t всё это одно и тоже 16-битное целое, только знаковое как бы “переломлено в минус” через нуль. Например, если счетчик 8 бит считает в минус …0х02, 0х01, 0х00, а дальше отрицательное переполнение 0xff=-1, 0xfe = -2… 0x80 максимальное отрицательное -128 ( для 16 бит -32768). Т.е ничего неизменилось, диапазон значений тот же 256 или 65536, меняется только “середина” диапазна не 0х80, а0х00.
i2cWrite(LSM330ACC_ADDRESS, LSM330_CTRL_REG4_A, 0x98) вот где кака - должно быть 0Х28, вылезла другая бяка - теряю ДУСы блин…
вылезла другая бяка - теряю ДУСы блин…
В телеметрию выведи сырые данные, а лучше отлачиком “проутюж”
в расчётах разобрался, датчик в 16g включался вот и считал что в два раза больше получаются делители, а следовательно и максимальное число, ДУСы рубятся где-то кальманом я и забыл совсем, что обновлял алгоритм…
Бугага, ДУСы душит файлсейв 😃
теперь у меня две версии с Кальманом и без, разницы пока не вижу…
а ЛСМ-ка по одной оси Y не досчитывает! вот и увод вправо!!!
В общем как только закажу платы для большОй, займусь переделкой Мелкоплаты - нравится она мне, только 6050 ставить не буду, а то NAZE32 получится 😃
Вот интересно, а почему правда в NAZE32 4-я ревизия два акселя, а никто не фильтрует их вместе? хватило бы и комплиментарного фльтра?
Вот интересно, а почему правда в NAZE32 4-я ревизия два акселя, а никто не фильтрует их вместе?
Я тут для побочного проекта считал “векторы” вибраций отдельно акселей и отдельно гироскопов (датчик - пресловутый 6050). Дак несовпадение полное получил, что расстроило, понятное дело.
Причем вибрация - явно ниже 100 Гц, однако суммы квадратов (x^2+y^2) вращались относительно друг друга с изменяющейся частотой! То ли фазовая разница, то ли сам дурак…
Я к чему это все - насколько можно доверять данным с двух разных датчиков?
Буду пробовать калибровку по всем осям делать, делать то всё равно нечего, и посмотрю - нафига ФС душить ДУСы…
Я к чему это все - насколько можно доверять данным с двух разных датчиков?
да доверять то нечему и так, а если векторы вибраций разные то в резонанс не должны по идее попасть?
Причем вибрация - явно ниже 100 Гц, однако суммы квадратов (x^2+y^2) вращались относительно друг друга с изменяющейся частотой! То ли фазовая разница, то ли сам дурак…
Аксель и ДУС мериют разные физвеличины. Как они могли одинаковые (симфазные) данные показать?
Я к чему это все - насколько можно доверять данным с двух разных датчиков?
Если датчики мериют одно и тоже, либо их приводят к одним величинам, но сами дачики разные ( и исправные), то можно повысить отношение сигнал/шум.
Копаюсь в акселе дальше - второй регистр:
#define LSM330_A_HPCF1 0x10 /* High-pass filter cutoff frequency selection */
#define LSM330_A_HPCF2 0x20 /* High-pass filter cutoff frequency selection */
#define LSM330_A_HPCF3 0x30 /* High-pass filter cutoff frequency selection */
выбор частоты среза, но нигде не написано какой?
выбор частоты среза, но нигде не написано какой?
см. стр. 39, там правда для гиры, но вроде как одинаково с акселем.
у гиры 3 бита у акселя 2?
Вечером попробовать надо…
у гиры 3 бита у акселя 2?
Частота среза и там и там 2 бита, частота дескретизации у акселя 4 бита.
Не на 39 странице для гиры задаётся и частота дискретизации и частота среза, у акселя в этом регистре только частота дискретизации 29стр. меня же интересует ВЧ фильтр акселя 30стр. у гиры 40 стр. у гиры я не использую хватает первого регистра 39стр. а у акселя такого нет…
Я сейчас могу задействовать лапу прерывания готовности, BMP выкинул и входы (аппаратные) прерываний свободны, к тому же дорожки около ЛСМ-ки проходят, было бы классно задействовать, хотя тоже самое и программным сделается…
А Вы не пробовали разобраться, что за данные умеет выдавать MPU6000/6050? Там же на борту какой-то onboard Digital Motion Processor заявлен, судя по описанию.
onboard Digital Motion Processor™ (DMP™) capable of processing complex 9-axis MotionFusion algorithms. The parts’ integrated 9-axis MotionFusion algorithms access external magnetometers or other sensors through an auxiliary master I²C bus, allowing the devices to gather a full set of sensor data without intervention from the system processor
А я разобрался куда деваются ДУСы при ФС - на этой платке PPM все каналы уходят при выключении передатчика в 1000, что как раз соответствует калибровке их любимых, в Wii по идее тоже самое будет происходить, значит либо переделывать ПО платки, а лучше всё-таки купить нормальные передатчик с приёмником…