flybrain. передатчик + приемник + автопилот. powered by stm32
Ну не получится так. Если я плату изначально криво в самолет пихаю, откуда она узнает где какие стороны света относительно самолета?
Стоп. А нахрена её криво в самолет пихать? Трудно ровно поставить типа ось x вдоль самолета, ось y - поперёк, z-вверх…
Начальные ролл и питч по акселю определяем, яв - по магнетометру.
В чем великая проблема?
Стоп. А нахрена её криво в самолет пихать?
Наверно по тому, что платка не в каждый самик влезет плашмя, но вполне себе стоймя залезает?
Как задавать начальное референсное значение платформы.
В существующих проектах предпологается неизменяемая установка платы, с возможностью ручного задания углов установки. Точное тримирование акселей делается по команде пользователя: модель должна стоять ровно. Гиры при инициализации (явной или неявной - при подаче питания) калибруют свои нули. Процедура инициализации, как и тримирования, обычно занимает секунд 15-30.
В существующих проектах предпологается неизменяемая установка платы
А если у меня плата встает только боком, или только с наклоном градусов 30? Все пропало?
Не ну я понимаю, текущие решения не умеют референс ресетить на старте. Но я то хочу, чтобы было максимально гибкое решение.
Сейчас в голову приходит такой вариант начальной установки:
- Держим кнопку, подаем питание. Плата спустя пару секунд переходит в режим каллибровки. моргает светодиодами.
- Ставит руль высоты на максимум вниз, типа просит самолет носом наклонить к земле и держать. дает на установку наклона 5 сек, затем собирает статистику
- Ставит руль высоты вверх, просит поставить самоль носом вверх
- Элероны влево, наклоняем самоль влево
- Элероны вправо, наклоняем вправо.
- Машет всеми рулями, просит зависнуть точно в горизонте, это будет начальным референсным положением с нулевыми рулями.
- Еще раз машет всеми рулями, просит наклонить самолет так, чтобы придать правильное триммерное положение рулям. в этот момент рули отслеживают наклоны и повторяют углы. Пользователь смотрит, где достаточно. Затем нажимает кнопку. Плата запоминает положение наклонных триммеров.
- Выход из режима каллибровки.
В те моменты когда самоль будет наклонятся, можно автоматом калибровать компас и аксель.
А если у меня плата встает только боком, или только с наклоном градусов 30? Все пропало?
Ничего не пропало.
Вот например у мну, можно девайс хоть как поставить. Просто нужно использовать в качестве референсных значений для определения горизонта показания акселя и магнетометра именно в таком положении. Фишку 30-ю аналогично ставить можно как угодно, но обязательно обучив ее при этом горизонту.
А гиры калибруются вне зависимости от положения девайса, главное чтоб он в покое оставался на протяжении всей калибровки.
Вот например у мну, можно девайс хоть как поставить.
Что значит как угодно? Можно задом наперед поставить? Как она узнает, где направление вперед?
Что значит как угодно? М
В разумных пределах по крену и тангажу
В разумных пределах по крену и тангажу
ну вот, а я хочу в неразумных пределах. Поставить задом наперед, вверх ногами. Ну или ось YAW с Roll поменять, например.
Рассказываю. Задаем т.н. референсный фрейм. В нормальном положении по акселю x=0g y=0g z=1g. Хотите перевернуть девайс z=-1g.
И крутитесь вокруг этого фрейма.
С магнето тоже самое.
если у меня плата встает только боком, или только с наклоном градусов 30?
Под неизменяемой установкой, имеется в виду не засовывать каждый раз как угодно.
Но сама установка - может быть любой. Просто программно вводятся коэффициенты поворота в нормаль по каждой из осей.
Плюс тонкая тримирование… Плюс автокалибровка. Ничего изобретатьь не надо .Просто посмотрите, как это у других сделано. 😃
В нормальном положении по акселю x=0g y=0g z=1g.
кто-то из нас тормозит 😃
И что для платы, которая знает только, что z=1g будет означать y>0 в этом случае? Это будет лево или право по отношению к самолету?
Дайте мне ссылку на инструкцию к FY-XX, я прочитаю что там пишут, а то вы все тут меня совсем запутали.
Это будет лево или право по отношению к самолету?
Вот жеж эти электронщики-теоретики…
при z=1g y>0 быть не может, увы. Это чё, там где вы находитесь g какоето другое?
Т.к расположение акселерометров относительно гироскопов неизменно, можно не наклонять, а попросить один раз повернуть на 1 оборот блинчиком и один раз - сделать “петлю” ( носом от горизонта вверх), потом поставить в покое в горизонте. Этих данных должно быть достаточно для калибровки направлений осей и горизонта.
Хотя, кажется, достаточно попросить сделать один известный по направлению оборот вокруг любой из осей и оставить в горизонте.
Вот жеж эти электронщики-теоретики
хорошо, мы такие. А вы, практики, никак объяснить не можете.
z=1
y=-0.789E-4
x=+0.45215
Как из этого понять где у самолета левый борт, а где правый?
Я вот о чем талдычу? Сама плата конечно знает, где у нее право, а где лево. Но, плата не знает, совпадает ли направление ее осей, с осями самолета (reference body frame). Если мы договариваемся, что плата может быть внутри самоля ориентирована по любому, то нужно учить плату ориентации внутри самолета и только после этого учить самолет горизонту.
Хотя, кажется, достаточно
неа, не будет этого достаточно.
Я вот сейчас подумал. должно быть так:
- поставить в горизонт, зсечь вектор тяжести.
- Наклонить влево, пауза
- Наклонить вперед, пауза
- Поставить в горизонт
Вот теперь все. Тогда направление pitch и roll будет известно, а yaw уже векторным прозведением можно расчитать да и GPS будет, если что откорректирует на ходу
хорошо, мы такие. А вы, практики, никак объяснить не можете. z=1 y=-0.789E-4 x=+0.45215
Этого в покое быть НЕ МОЖЕТ!
Суммарный вектор = 1, а тут уже только по z единица!
Такое может быть если z вертикально вниз смотрит, а по оси x дёрнули плату.
Ну или сдвинули по всем осям
Да, и еще такое может выть если оси у вас не перпендикулярны. Но это уже совсем сложный случай. 😃
Найти середину - это вариант,
И второе действие: “выровнять оси” - не забудьте,
иначе не сойдется с гирой.
Лога с сырыми датчиками посмотреть еще нет?
(время отсчета и три оси, по трем датчикам: аксель,гира,маг. ;
минут пять покрутить, чтобы конец маг-а заполнил(зачертил) сферу)
Передумал…
Лога с сырыми датчиками посмотреть еще нет?
да я чо-то как-то в основную работу погрузился. Платку пока отложил. До субботы буду занят на основной работе. Прежде чем с компасом ковыряться хочу еще сингулярность на pitch угле запатчить. Надо как-то вырубить искусственно установку этого угла точно в 90 градусов. Я все не решу на каком этапе алгоритма это получше сделать.
Я кстати реализовал кватернионы, только чо-то пака не уверен, что ошибок нет. Они субъективно хуже горизонт на перегрузках держат. Может ошибся где, надо еще покопаться получше в этой теме.
А какой толк на сырые данные смотреть? Тебе компас интересен или все в куче? Могу дать лог, только какой с него толк?
такое может выть если оси у вас не перпендикулярны
Ну я ж условно, для примера. Ясен пень, что не может.
Алекс, это какой-то фейк. Кватернионы не могут держать или не держать горизонт, что это за выдумка такая? Они просто описывают поворот.
Они просто описывают поворот
Поворот на хлеб не намажешь, и на экран в виде roll/pitch/yaw не визуализуешь. Соответственно кватернион текущего состоняния переводится в углы эйлера, чтобы можно было горизонт воспринять. Соответственно я вижу, что это работает у меня почему-то хуже, чем EKF. И я, похоже, понимаю почему - у меня хорошо подобрана system state model для ekf, а в кватернионы я сунул то, что везде предлагают. Либо, возможно, где-то еще какой-то косяк. Я бы может и не парился с кватернионами, то EKF падает, когда pitch 90 и я не вижу другого решения, кроме как в этот момент отключать EKF а руководствоваться только прямым интегрированием гиры.
То есть, перед шагом предсказания проверять. Если |cos(pitch)| < 0.0001 например, то после интеграции считать, что углы и так достоверны. Это добавит дрожания едва заметного в окрестности pitch 90, но зато сингулярность отпадет. Ну не часто, наверно, нужно летать свечой вверх под 90 градусов.
Надо еще японско-корейские патенты почитать, какие они там нелинейные модели состояний твердых тел предлагают. Я же вижу, что есть видеоролики с очень качественным поведением EKF на кватернионах. Правда, эти хитрецы всегда до кучи магнитометр используют, а хотелось бы не очень доверять этому устройству.
Алекс, можно узнать полный формат пакета RFM-ки ( сколько байт преамбула итп.)?
Сорри, только сейчас нашлось время залезть в исходники.
Вот настройка с которой у меня сейчас работает
// конфиг для случая 9.6 kbps, Dev: 15kHz, BW: 42.7 kHz
const uint8_t rfm_init_table_9_6__15[][2]={
{0x05,0x00}, // disable all 1-st interrupts
{0x06,0x00}, // disable all 2-nd interrupts
{0x07,0x01}, // ready IDLE mode xton=1, потом посмотрим как сократиться время если pllon =1 держать
// {0x09,0x7f}, // cap = 12.5pf не знаю, так везде ставят
{0x09,0x6d}, // cap load не знаю, так поставили в EZradio примере
{0x0a,0x05}, //clk output is 2MHz, в принципе по фиг какая там частота, это не используется
// {0x0b,0xf5}, // RX_EN on gpio0 1111 0100, uses RX State signal
{0x0b,0xea}, // RX_EN on gpio0 1110 1010, uses Direct Digital Output
// {0x0c,0xf2}, // TX_EN on gpio1 1111 0010, uses TX State signal
{0x0c,0xea}, // TX_EN on gpio1 1110 1010, uses Direct Digital Output
{0x0d,0x00}, // GPIO2 for MCLK output
{0x0e,0x00}, // GPIO port use default value, RX_ANT=0, TX_ANT=0
{0x0f,0x70}, // NO ADC used
{0x10,0x00}, // NO ADC used
{0x12,0x20}, // no temperature sensor used
{0x13,0x00}, // no temperature sensor used
// IF settings
{0x1c,0x2c}, // 9.6 kbps, Dev: 15kHz, BW: 42.7 kHz ********
{0x1d,0x44}, // AFC on ?? or 0x40
{0x1e,0x0a}, // AFC timing
{0x20,0x68}, //clock recovery **********
{0x21,0x01}, //clock recovery **********
{0x22,0x3a}, //clock recovery **********
{0x23,0x93}, //clock recovery **********
{0x24,0x01}, //clock recovery **********
{0x25,0x95}, //clock recovery **********
{0x2a,0x1e}, //AFC limiter **********
//TX/RX general packet settings
{0x30,0x8d}, //data acess control: MSB, CRC=1, CRC over data only = 0, crc type = crc16-ibm
{0x32,0x08}, // broadcast = disable, header3 compare only
{0x33,0x10}, // 1 byte header3, variable pkt len, 1 byte sync word = syncword3
{0x34,0x08}, // preamble = 32bits
{0x35,0x20}, // preamble detect = 16bit, rssi_offset = 0
{0x36,0x2d}, // preamble 1
{0x37,0xd4}, // preamble 2
{0x38,0x00}, // preamble 3
{0x39,0x00}, // preamble 4
{0x3a,0x54}, // tx header 3
{0x3b,0x53}, // tx header 2
{0x3c,0x52}, // tx header 1
{0x3d,0x51}, // tx header 0
{0x3e,0x00}, // pkt len = 0, not used
{0x3f,0x54}, // rx header 3
{0x40,0x53}, // rx header 2
{0x41,0x52}, // rx header 1
{0x42,0x51}, // rx header 0
{0x43,0xff}, // header 3 check mask
{0x44,0xff}, // header 2 check mask
{0x45,0xff}, // header 1 check mask
{0x46,0xff}, // header 0 check mask
//endpoint FIFO handler settings
{0x69,0x60}, // automatic Automatic Gain Control Enable for preamble time
{0x6e,0x4e}, // tx data rate 1 0x4ea5*1000/2097152=9.6 kbit **************
{0x6f,0xa5}, // tx data rate 2, всего получится 9.6 кбит/с если пересчитать по формуле ***************
{0x6d,0x1c}, // 0001 1100 = +11dbm transmit power
{0x70,0x2c}, // no Manchester Coding, no Data Whitening, txdtrtscale = 1 for < 30 kbps
{0x71,0x23}, // No TX Data CLK is available, FIFO Mode, GFSK
{0x72,0x18}, // deviation 10kHz **************
{0x73,0x00}, // frequency offset = 0, возможно потом порегулируем, чтобы точно совместить приемник и передатчик. Шаг 125 Гц
{0x74,0x00}, // frequency offset = 0
{0x75,0x53}, // band selction: 430 - 439.9 MHz
{0x76,0x4B}, // carrier freq ( 430 + (0x6400/64000) ) (*2 if hbsel=1)=430.030 mhz
{0x77,0x00}, // carrier freq
{0x79,0x00}, // req channel = 0. by default
{0x7a,0x02}, // freq hopping size = 20 kHz
// {0x07,0x02}, // pllon=1, IDLE - проверить потом как на скорость перехода в режим передачи повлияет
{0,0}
};
1 секунда делится на 50 таймслотов. 25 слотов в одну сторону, 25 слотов в другую сторону
Вот процедура, которая формирует пакет и шлет его. Из кода все читаемо.
void PreparePacketData(uint8_t send_params){
// засылаем данные в буфер модуля
static uint8_t digit_keys;
if(send_params == 1) {
// точно выясним, будем ли слать параметры или стандартный пакет
if(packet_number == 2 || packet_number == 6 || packet_number == 10 || packet_number == 14 || packet_number == 18 || packet_number == 22){
// да, шлем стандартный пакет
} else {
send_params = 0; // нет, шлем стандартный пакет
}
}
if(send_params==1) {
TimeoutAfterLastReceivedPacket = 0; // чтобы не свалиться в failsafe
// параметры ручек и джойстиков
if(packet_number == 2) {
send_data[0] = 0x82; //передать тангаж
send_data[1] = 0; // резерв
// минимум
send_data[2] = (uint8_t)(Settings.tangazh_up_ugol >> 8); // старший байт
send_data[3] = (uint8_t)(Settings.tangazh_up_ugol & 0xff); // младший байт
// максимум
send_data[4] = (uint8_t)(Settings.tangazh_down_ugol >> 8); // старший байт
send_data[5] = (uint8_t)(Settings.tangazh_down_ugol & 0xff); // младший байт
// центр на серве
send_data[6] = (uint8_t)(Settings.tangazh_center_ugol >> 8); // старший байт
send_data[7] = (uint8_t)(Settings.tangazh_center_ugol & 0xff); // младший байт
send_data_len = 8;
}
if(packet_number == 6) {
send_data[0] = 0x83; //передать крен
send_data[1] = 0; // резерв
// минимум
send_data[2] = (uint8_t)(Settings.kren_left_ugol >> 8); // старший байт
send_data[3] = (uint8_t)(Settings.kren_left_ugol & 0xff); // младший байт
// максимум
send_data[4] = (uint8_t)(Settings.kren_right_ugol >> 8); // старший байт
send_data[5] = (uint8_t)(Settings.kren_right_ugol & 0xff); // младший байт
// центр
send_data[6] = (uint8_t)(Settings.kren_center_ugol >> 8); // старший байт
send_data[7] = (uint8_t)(Settings.kren_center_ugol & 0xff); // младший байт
send_data_len = 8;
}
if(packet_number == 10) {
send_data[0] = 0x84; //передать газ
send_data[1] = 0; // резерв
// минимум
send_data[2] = (uint8_t)(Settings.gaz_up_ugol >> 8); // старший байт
send_data[3] = (uint8_t)(Settings.gaz_up_ugol & 0xff); // младший байт
// максимум
send_data[4] = (uint8_t)(Settings.gaz_down_ugol >> 8); // старший байт
send_data[5] = (uint8_t)(Settings.gaz_down_ugol & 0xff); // младший байт
// центр
send_data[6] = (uint8_t)(Settings.gaz_center_ugol >> 8); // старший байт
send_data[7] = (uint8_t)(Settings.gaz_center_ugol & 0xff); // младший байт
send_data_len = 8;
}
if(packet_number == 14) {
send_data[0] = 0x85; //передать курс
send_data[1] = 0; // резерв
// минимум
send_data[2] = (uint8_t)(Settings.kurs_left_ugol >> 8); // старший байт
send_data[3] = (uint8_t)(Settings.kurs_left_ugol & 0xff); // младший байт
// максимум
send_data[4] = (uint8_t)(Settings.kurs_right_ugol >> 8); // старший байт
send_data[5] = (uint8_t)(Settings.kurs_right_ugol & 0xff); // младший байт
// центр
send_data[6] = (uint8_t)(Settings.kurs_center_ugol >> 8); // старший байт
send_data[7] = (uint8_t)(Settings.kurs_center_ugol & 0xff); // младший байт
send_data_len = 8;
}
if(packet_number == 18) {
send_data_len = 0;
send_data[send_data_len++] = 0x89; //передать частоты канала
send_data[send_data_len++] = Settings.main_channel; // основной канал
send_data[send_data_len++] = Settings.backup_channel_1; // запасной канал 1
send_data[send_data_len++] = Settings.backup_channel_2; // запасной канал 2
send_data[send_data_len++] = Settings.tx_power; // мощность передатчика
}
if(packet_number == 22) {
send_data_len = 0;
send_data[send_data_len++] = 0x8a; //передать failsafe позиции по всем рулям
send_data[send_data_len++] = Settings.tangazh_failsave_position; // 0 failsafe position, рули веиз до упора
send_data[send_data_len++] = Settings.kren_failsave_position_1; // failsafe position kren1 вверх до упора =0= ПравЭлер
send_data[send_data_len++] = Settings.kren_failsave_position_2; // failsafe position kren2 вверх до упора =255= ЛевЭлер
send_data[send_data_len++] = Settings.gaz_failsave_position; // failsafe position, выключить газ в ноль =0=
send_data[send_data_len++] = Settings.kurs_failsave_position; // failsafe position kurs1 руль направления по центру = 128 =
}
} else {
// увеличиваем счетчик времени непринятого ответа от приемника
TimeoutAfterLastReceivedPacket ++;
// текущие положения органов управления
send_data_len = 0;
if(HoppingMode_Enable == 0) {
send_data[send_data_len++] = packet_number & 0x3f; // режим пачки даных, + номер пакета
} else {
// режим пачки даных, + номер пакета + бит признак того, что следующий пакет пойдет в режиме FHSS
send_data[send_data_len++] = (packet_number & 0x3f) | 0x40; // режим пачки даных, + номер пакета + бит признак того, что следующий пакет пойдет в режиме FHSS
}
send_data[send_data_len++] = HoppingPool[(packet_number>>1)]; // частота канала для FHSS для данного пакета
send_data[send_data_len++] = adc_ch[CHANNEL_GAZ]; // газ
send_data[send_data_len++] = adc_ch[CHANNEL_KURS]; // курс
send_data[send_data_len++] = adc_ch[CHANNEL_TANGAZH]; // тангаж
send_data[send_data_len++] = adc_ch[CHANNEL_KREN]; // крен 1
if(Settings.kren_difference==1) // крен 2
send_data[send_data_len++] = ~adc_ch[CHANNEL_KREN];
else
send_data[send_data_len++] = adc_ch[CHANNEL_KREN];
digit_keys = (PORT_MENU->IDR >> PIN_RESERVE) & 1; // пока есть только один переключатель. Первый бит - половинный расход
send_data[send_data_len++] = digit_keys; // цифровые переключатели, пока не используются
}
}
void TransmitPacket(){
// _falisafe_test_count++;
// запускаем передачу
TIM2->CNT = 0; // запускаем таймер статистики
// if(_falisafe_test_count>1000 && _falisafe_test_count<1200);
// else
rfm_TransmitData2(send_data_len, send_data);
}