Создание собственной системы стабилизации
Я думаю, даже уверен, что смысла ждать неделю нет. Лучше ее потратить на начало работы с FreeRTOS.
Таки изобрёл велосипед))) По сути обработчик программных прерываний, обработчик событий. Особо если портировать на АВР, там аппаратных прерываний мало.
Не нужно в прерываниях ничего считать. Только забрать данные которые “вдруг” “привалили” и выставить флаг (семафор) для задачи которая все это обсчитает.
Вот у меня всё так теперь, только не семафор, а вызов обработчика. При этом вызывающее прерывание освобождается, можно дальше “слушать” датчик.
Кста, кто может " прокатить" под отладчиком FreeRTOS, сколько циклов проца натикает до старта вычисления в самой приоритетной задаче? У меня 230 )
Подскажите , кто может, - как сделать чтоб коптер не разворачивало по “Yaw” на 360 гр. при переходе через “0-360” град. (всю голову сломал, не могу придумать, прям “ручник” какой то)))),
т.е. на ПИД по “Yaw” подаем 0-360 гр. с ИМУ и уставку 0-360 гр… получаем в районе перехода 0-360 сигнал управления на целый оборот…(?), как у людей то сделано ??.
получаем в районе перехода 0-360 сигнал управления на целый оборот…(?), как у людей то сделано ??.
Ну тут вроде всё просто: если разность больше 180, вычитай или прибавляй 360 (т.е. полный оборот). Хотя математически правильно на выходе иму получить угол ±Пи, а преобразование в азимутальный 0-360 только для индикации.
Хотя математически правильно на выходе иму получить угол ±Пи
Это ничем не лучше 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 можно было использовать незадействованные “железные” вектора, и вызывать их поднятием бита в регистре прерываний соответствующего устройства. Но хотелось получить более универсальный, простой и управляемый механизм. Собственно в этом и изюминка.
)) уже не очень коротко. Работает просто, запрос вызывает планировщик и ставит флажок, вызывается прерывание, которое последовательно в соответствии с приоритетами выбирает флажки и запускает обработчики. Всё.
запрос вызывает планировщик
А “запрос” кто, когда и как формирует ?? Это “конечный автомат на прерываниях” получается что ль ??