Создание собственной системы стабилизации
Хотя математически правильно на выходе иму получить угол ±Пи
Это ничем не лучше 0-360. Если угол используется для управления (в ПИД-контроллере, задание цели, и т.д.) то он должен быть непрерывным. Можно завести специальный тип данных, но проще всего во float преобразовать и не париться. Методика называется “phase unwrapping”. Обратно в ±180 также просто преобразовать. Ни и эту особенность углов надо всегда помнить. Если угол 10, а команда идет “повернуть на 380” - то это можно сделать двумя способами, длинным и коротким. И однозначно сказать какой “правильный”, может только тот, кто команду дает.
он должен быть непрерывным
но проще всего во float преобразовать и не париться
Непонятно как это сделать практически… , может формулу или пример дадите ? Если я правильно понял, то как бы угол +/- бесконечность нужно сделать и считать еще и обороты относительно “нуля” ?
Выше Александр написал уже: “если разность больше 180, вычитай или прибавляй 360 (т.е. полный оборот)”. Только ещё можно учесть, что полных оборотов может быть больше одного, и нужно в начале инициализировать фильтр сырыми данными без этой проверки (начальный угол может быть любым и не надо его сравнивать с нулем). Пример кода не буду давать чтоб не запутать, но там пара строк всего.
И однозначно сказать какой “правильный”, может только тот, кто команду дает.
Это верно, особо для навигации, но в случае стабилизации не могу представить случай, когда нужно будет идти по большому кругу. К тому же использовать углы для получения сигналов рассогласования, как мне кажется, не правильно, лучше применять матрицы, кватернионы, вектора.
Публикую свой “велосипед”, может кому пригодиться.
Чтоб работал нужно сделать:
В проект включить ROSA.c и соответствующие файлы порта для АВР или СТМ32
в main.c или другом ц,цпп-файле
#include “ROSA.h”
void hend0(void)
{
ROSA_CallEvent(3);
}void hend1(void)
{
//ROSA_CallEvent(2);
}
void hend2(void)
{
//ROSA_CallEvent(3);
}void hend3(void)
{
ROSA_CallEvent(4);
}void hend4(void)
{
ROSA_CallEvent(1);
}/* массив обработчиков */
Event_TypeDef Event_List [] = {hend0,hend1,hend2,hend3,hend4
};/* массив приоритетов */
const Event_Priorety_TypeDef _Ev_Prior[] = {
0,1,2,3,4 };int main(void)
{
ROSA_Init(0); /* инициализация РОСА*/for(;😉 {
ROSA_CallEvent(0);
}
}
Ну и в конфиге “ROSA_config.h” прописать определение регистров проца, количество обработчиков, не\использование фиксированных приоритетов, использование механизма очереди. В основной дериктории есть “рыба” “_ROSA_config.h”, надо только оставить нужно, лишнее удалить/закомметировать.
#include “stm32f10x.h” /* заголовочный файл для контроллера STM32 */
#include <avr/io.h>
#include <avr/interrupt.h> /* определение внутренних регистров МК */#define Event_Count 4 /* количество обработчиков */
#define ROSA_ConstPriority /* массив приоритетов определяется в пользовательском файле */
#define ROSA_WaitEvent_QUEUE /* использование очереди, если нет, то простой перебор */
/* это только для AVR на другое не влияет */
#define ROSA_AVR_SW_PENDBIT_PORTD3 /* бит вызова прерывания для переключения обработчиков */
нахаляву досталась платка flip32 по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях
по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях
Каким образом испытывал? Каруселькой?
линейно перемещал вдоль поверхности
линейно перемещал вдоль поверхности
Линейно перемещать с ускорением долго нельзя, если постоянная времени коррекции большая, то завал будет не заметен. Лучше все таки крутить.
ну в арду даже при резких линейных перемещениях можно зафиксировать приличные отклонения
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
линейно перемещал вдоль поверхности
Только “каруселькой”…
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
У кого сложилось ? если можно поподробнее… как то, пока не встречал рабочего решения этой проблемы…
тема длинная но интересная github.com/cleanflight/cleanflight/issues/791
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить
Не понял, интегральную коррекцию гиры от акселя или чего?
У меня например аксель управляет ТОЛЬКО смещением ДУСа, т.е. исправляет интегральную ошибку, никакой пропорциональной составляющей.
смещение ДУС - через пропорциональный коэф между нормалью и горизонтом системы,
а постоянная составляющая - долгосрочное среднее значение от этой компенсации, источником которой является характерное уплывание гироскопов
в результате за счет уменьшения влияния от моментальной нормали можно достичь мньшей пропорции смещения чтоз пропорциональную и как следствие меньшее отклонение в перегрузках
Борис запилил raceflight под F4 проц. интересно а cleanflight-подобные прошивки пойдут на F4BY? просто по набору датчиков она более продвинута чем KISSFC)))
пойдут - драйвера для датчиков можно взять тут github.com/SergDoc/…/Release
Борис запилил raceflight под F4 проц
там всё есть только подправить прошивку под железо…
Публикую свой “велосипед”, может кому пригодиться.
А нельзя коротко пояснить - как ЭТО работает и что может ? (в чём “изюминка”, так сказать… )
А нельзя коротко пояснить
Коротко уже объяснил, это движок программных прерываний с приоритетами, по сути замена замена надстройка над штатной системой прерываний. Т.е. у меня есть набор функций-обработчиков с набором вычислений, которые должны быть выполнены по какому-то событию. Приведу свой пример, нужно сделать следующее:
Получить данные ДУС - Рассчитать новое положение ИНС - Рассчитать стабилизатор моментов - Рассчитать микшер - Вывалить в ШИМ.
Можно сделать так:
ISR_DUS()
{
Получить данные ДУС();
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}
Всё хорошо, если больше ничего делать не нужно, но если еще параллельно идет опрос других устройств, то это недопустимо.
Поэтому лучше сделать так:
ISR_DUS()
{
Получить данные ДУС();
ВыставимФлаг(ДУСГотов);
}Loop{
if (ДУСГотов) {
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}
}
Это способ разделить “железные” прерывания, с “быстрыми” обработчиками, и “медленные” обработчики-вычислители. Собственно для СТМ32 можно было использовать незадействованные “железные” вектора, и вызывать их поднятием бита в регистре прерываний соответствующего устройства. Но хотелось получить более универсальный, простой и управляемый механизм. Собственно в этом и изюминка.
)) уже не очень коротко. Работает просто, запрос вызывает планировщик и ставит флажок, вызывается прерывание, которое последовательно в соответствии с приоритетами выбирает флажки и запускает обработчики. Всё.
запрос вызывает планировщик
А “запрос” кто, когда и как формирует ?? Это “конечный автомат на прерываниях” получается что ль ??
А “запрос” кто, когда и как формирует ??
ROSA_CallEvent(0);
У меня запросы формируют обработчики “железных” прерываний, а дальше, допустим, обработчик расчета ИНС, вызывает обработчик микширов ШИМ, которые имееют более низкий приоритет относительно ИНС и стабилизатора. Так что если микшеры еще не досчитаются, а ДУС даст новые данные, то ИНС их не пропустит, прервет обсчет микширов.
Собственно я пробные проекты выкладывал, под АВРстудио можно “поиграться” в софтовом симуляторе не прошивая кристал, как впрочем и в проекте под СТМ32Ф1 и кейл
можно “поиграться” в софтовом симуляторе
крайний вопрос - реальные испытания были ?? А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…
крайний вопрос - реальные испытания были ??
Да, “виртуалка” СТМ32Ф4 и полетник на Ф3. На Ф1 испытывал только на демопроекте, но с ним всё прощще, не нужно контекст ФПУ сохранять. Так что будет работать.
А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…
Роса как бы не заменяет типовую ртос, она не может выполнять независимые ЦИКЛИЧЕСКИЕ задачи (хотя возможно зациклить две задачи, которые будут вызывать друг друга).