Создание собственной системы стабилизации
Не совсем, Олег Syberian Кальман использует rcopen.com/blogs/39391/15187
Он же не СТшный калман юзает
У китайцев с чувством юмора плохо goodluckbuy.com/invensence-mpu-6000-chip-six-axis-…
Он же не СТшный калман юзает
а фильтр Кальмана бывает ST-шный или чей-то ещё? не я конечно понимаю, что каждый изобретает свой адаптивный фильтр, но в основе с 60-х годов, таки у всех фильтр Калмана (Или Кальмана) 😃
но не многим, и я тому не исключение, удалось в нем разобраться 😦
Я с Александром обсуждал CTшную либу iNemo имеющую реализацию ЕКФ - просто стшный калман звучит короче =)))
Я прекрасно понимаю, но не факт что он будет работать правильно, там же-ж куева хуча коэффициентов которые надо подбирать самому )))
блин надеюсь скоро закажу платки, буду догонять 😃 выяснилось вот что флешку новую докупать надо 😦 , а барометр хоть и паяный но рабочий 😃 , надеюсь завтра погода и свободное время совпадут, полетаю 😃
А я дурак с мелкоплатой - пропы 12-е вместо 10-х, а пиды загнал как для 10-х и аппарат легче на 150гр., а я хочу чтобы полетело😵😃😠😃 у меня ж тяги на каждой ВМГ в ДВА раза больше😠 злой сам на себя, чуть аппарат не угробил…
Как-то вы странно рассуждаете, DCM это МЕТОД ОПИСАНИЯ ПОВОРОТА, так же как кватернионы, или углы Эйлера. Он не лучше и не хуже, ничего он не фильтрует.
ну или фильтрует также как любой другой метод - путем интегрирования угловых скоростей.
Говорить что метод описания поворота чувствителен к вибрациям, это кажется, неправильно…
Если вибрации влияют на датчики, кажется, надо устранять вибрации, ибо другой информации у нас нет, (я не рассматриваю пока компас и GPS…)
Как-то вы странно рассуждаете,
То что ДЦМ описывает поворот понятно, выше имелся ввиду весь алгоритм ИМУ ардупилота, в т.ч. и коррекция по акселю. В деталях его не разбирался, но АПМ1.0 видел как работает - немного покрутил в руках (без вибраций и тряски) и горизонт уплывает градусов на 25.
Отлетал до пищалки (мелкоплата с кальманутым фильтром 😃 ), но в Акро, в Горизонт и Angle аксель вечно пытается куда-то увести аппарат, как и раньше 😦 , положительных два аспекта - отсутствовал дома 20мин, значит 15 стабильно летал ( трёха 1100гр., DT700, 12Х4.5 Gefman, 3000мА/ч 20с), в акро висит как прибитый, т.к. я пилот не очень и путаю педали то, погулял за ним по всему двору - аппарат сбежать не пытался 😃
выводы: надо добивать аксель (или менять) ибо дальше АХ с сонаром и т.п. даже пробовать нечего пока не добьюсь стабильности хотя-бы как в Акро.
P.S. серва быстрая цыфровая не дёргается как припадошная в отличии от стандартного wii - немного уводит по курсу, но и погода не безветренная и yawdeadband можно увеличить т.к. пока работаешь газом всё равно немного руддером подрабатываешь даже незаметно для себя, а т.к. это трёха то и чувствительность высокая, немного есть расколбас PPM платки но терпимо 😃
Ха а писчалка сработала на 3.74 В на банку…
Отлетал до пищалки
Спасибо за опыт! Одномерный калмане не решает.
пока не добьюсь стабильности хотя-бы как в Акро.
Видимо аксель надо менять, фильтры сами посебе вытянуть не способны…
Можно попробовать вием “зажать” acc_lpf_factor=100 сделать например, но что-то мне подсказывает что ничего это не даст, т.к. тянет в сторону как будто магнитом каким - вот ровно-ровно а потом поплыл с ускорением, хорошо хоть не переворачивается, надо бы ещё на квадрике(без сервы- она довольно сильные вибрации даёт (в мет. корпусе, мотор наружу) ) попробовать , но пока не могу…
серва быстрая цыфровая не дёргается как припадошная в отличии от стандартного 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 ??? они чё на бивнях считали что-ли? 😃