Создание собственной системы стабилизации

alexmos

Выше Александр написал уже: “если разность больше 180, вычитай или прибавляй 360 (т.е. полный оборот)”. Только ещё можно учесть, что полных оборотов может быть больше одного, и нужно в начале инициализировать фильтр сырыми данными без этой проверки (начальный угол может быть любым и не надо его сравнивать с нулем). Пример кода не буду давать чтоб не запутать, но там пара строк всего.

rual
alexmos:

И однозначно сказать какой “правильный”, может только тот, кто команду дает.

Это верно, особо для навигации, но в случае стабилизации не могу представить случай, когда нужно будет идти по большому кругу. К тому же использовать углы для получения сигналов рассогласования, как мне кажется, не правильно, лучше применять матрицы, кватернионы, вектора.

Публикую свой “велосипед”, может кому пригодиться.
Чтоб работал нужно сделать:

В проект включить 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 /* бит вызова прерывания для переключения обработчиков */

ROSA.rarROSA_AVRMega_Exp.rarCortex.rar

alexeykozin

нахаляву досталась платка flip32 по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях

rual
alexeykozin:

по случаю затестил горизонт cleanflight визуально вроде бы не валится в ускорениях

Каким образом испытывал? Каруселькой?

rual
alexeykozin:

линейно перемещал вдоль поверхности

Линейно перемещать с ускорением долго нельзя, если постоянная времени коррекции большая, то завал будет не заметен. Лучше все таки крутить.

alexeykozin

ну в арду даже при резких линейных перемещениях можно зафиксировать приличные отклонения
сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить

oleg70
alexeykozin:

линейно перемещал вдоль поверхности

Только “каруселькой”…

alexeykozin:

сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить

У кого сложилось ? если можно поподробнее… как то, пока не встречал рабочего решения этой проблемы…

rual
alexeykozin:

сложилось мнение что нужно интегральную составляющую корекции гироскопов добавлять а пропорциональную от акселя уменьшить

Не понял, интегральную коррекцию гиры от акселя или чего?
У меня например аксель управляет ТОЛЬКО смещением ДУСа, т.е. исправляет интегральную ошибку, никакой пропорциональной составляющей.

alexeykozin

смещение ДУС - через пропорциональный коэф между нормалью и горизонтом системы,
а постоянная составляющая - долгосрочное среднее значение от этой компенсации, источником которой является характерное уплывание гироскопов

в результате за счет уменьшения влияния от моментальной нормали можно достичь мньшей пропорции смещения чтоз пропорциональную и как следствие меньшее отклонение в перегрузках

serg2557

Борис запилил raceflight под F4 проц. интересно а cleanflight-подобные прошивки пойдут на F4BY? просто по набору датчиков она более продвинута чем KISSFC)))

SergDoc

пойдут - драйвера для датчиков можно взять тут github.com/SergDoc/…/Release

serg2557:

Борис запилил raceflight под F4 проц

там всё есть только подправить прошивку под железо…

oleg70
rual:

Публикую свой “велосипед”, может кому пригодиться.

А нельзя коротко пояснить - как ЭТО работает и что может ? (в чём “изюминка”, так сказать… )

rual
oleg70:

А нельзя коротко пояснить

Коротко уже объяснил, это движок программных прерываний с приоритетами, по сути замена замена надстройка над штатной системой прерываний. Т.е. у меня есть набор функций-обработчиков с набором вычислений, которые должны быть выполнены по какому-то событию. Приведу свой пример, нужно сделать следующее:
Получить данные ДУС - Рассчитать новое положение ИНС - Рассчитать стабилизатор моментов - Рассчитать микшер - Вывалить в ШИМ.
Можно сделать так:

ISR_DUS()
{
Получить данные ДУС();
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}

Всё хорошо, если больше ничего делать не нужно, но если еще параллельно идет опрос других устройств, то это недопустимо.
Поэтому лучше сделать так:

ISR_DUS()
{
Получить данные ДУС();
ВыставимФлаг(ДУСГотов);
}

Loop{
if (ДУСГотов) {
Рассчитать новое положение ИНС();
Рассчитать стабилизатор моментов();
Рассчитать микшер ();
Вывалить в ШИМ();
}
}

Это способ разделить “железные” прерывания, с “быстрыми” обработчиками, и “медленные” обработчики-вычислители. Собственно для СТМ32 можно было использовать незадействованные “железные” вектора, и вызывать их поднятием бита в регистре прерываний соответствующего устройства. Но хотелось получить более универсальный, простой и управляемый механизм. Собственно в этом и изюминка.

)) уже не очень коротко. Работает просто, запрос вызывает планировщик и ставит флажок, вызывается прерывание, которое последовательно в соответствии с приоритетами выбирает флажки и запускает обработчики. Всё.

oleg70
rual:

запрос вызывает планировщик

А “запрос” кто, когда и как формирует ?? Это “конечный автомат на прерываниях” получается что ль ??

rual
oleg70:

А “запрос” кто, когда и как формирует ??

ROSA_CallEvent(0);

У меня запросы формируют обработчики “железных” прерываний, а дальше, допустим, обработчик расчета ИНС, вызывает обработчик микширов ШИМ, которые имееют более низкий приоритет относительно ИНС и стабилизатора. Так что если микшеры еще не досчитаются, а ДУС даст новые данные, то ИНС их не пропустит, прервет обсчет микширов.

Собственно я пробные проекты выкладывал, под АВРстудио можно “поиграться” в софтовом симуляторе не прошивая кристал, как впрочем и в проекте под СТМ32Ф1 и кейл

ROSA (1).zip

oleg70
rual:

можно “поиграться” в софтовом симуляторе

крайний вопрос - реальные испытания были ?? А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…

rual
oleg70:

крайний вопрос - реальные испытания были ??

Да, “виртуалка” СТМ32Ф4 и полетник на Ф3. На Ф1 испытывал только на демопроекте, но с ним всё прощще, не нужно контекст ФПУ сохранять. Так что будет работать.

oleg70:

А то у меня щас “КоОС” начала внезапно безбожно виснуть по непонятным причинам и надежды нарыть глюк мало… короче вопрос выкинуть ее нафиг остро назрел…

Роса как бы не заменяет типовую ртос, она не может выполнять независимые ЦИКЛИЧЕСКИЕ задачи (хотя возможно зациклить две задачи, которые будут вызывать друг друга).

rual

2oleg70
Дополню кусками из своего проекта:
hw_config.с - массив ссылок на обработчики и массив приоритетов (макс 254).

const Event_TypeDef _Event_List[] =  {
	DUSCalc,
	AccCalc,
	Rdy_PWM1,
	Rdy_PWM2,
	MagmCalc,
	BaroCalc,
	GNSSCalc,
	Rdy_RC,
	FrSky_Process,
};

const Event_Priorety_TypeDef _Ev_Prior[] = {
	0,1,2,2,3,4,4,5,6 };

обработчики вызываются функцией ROSA_CallEvent(<номер обработчика в массиве 0-254>); чтоб не запоминать
вносим в хедер hw_config.h перечислитель обработчиков,


typedef enum {
	evDUS,
	evAcc,
	evPWM1,
	evPWM2,
	evMag,
	evBaro,
	evGNSS,
	evRC,
	evFRSKY,
	evRXTerm,
} ROSA_Events;

после чего используем вместо номеров названия событий 😉

/*--------- вызовы методов по готовности данных датчиков ---------- */
/* Аксель */
void AccDataRdy(void)
{
	AP.ГотовДанныхАкселя();
	ROSA_CallEvent(evAcc);

#ifndef 	ACC_FILTER
	Mag_Zapros();
#endif

}
void AccCalc(void)
{
	AP.РасчетПоАкселю();
}

/* ДУС */
void DUSDataRdy(void)
{
	AP.ГотовДанныхДУС();

	ROSA_CallEvent(evDUS);
}
void DUSCalc(void)
{
	AP.РасчетПоДУС();

	ROSA_CallEvent(evPWM1); /* обновим приводы */
}

/* магнитометр */
void MagmDataRdy(void)
{
	AP.ГотовДанныхМагнит();

	AP.инс.Барометр.ЗапросДанных();

	ROSA_CallEvent(evMag);
}

void MagmCalc(void)
{
	AP.РасчетПоМагнит();
}

/* барометр */
void BaroDataRdy(void)
{
	AP.ГотовДанныхБаро();

	ROSA_CallEvent(evBaro);
}

void BaroCalc(void)
{
	AP.РасчетПоБаро();
}
/* ГНСС */
void GNSSCalc(void)
{
	glns.FrameRdy();
}

/* ППМ получен*/
void Rdy_RC(void)
{
	ppm.rasch();
}

/* вывод кадра ШИМ завершен */
void Rdy_PWM1(void)
{
	/* суммируем управление на приводы */
	mixINS1.rasch(AP.инс.Крен*1024.0f,AP.инс.Тангаж*1024.0f,AP.инс.Рыск*1024.0f,AP.цУпрТяга);
	mixPID1.rasch(AP.цУпрКрен,AP.цУпрТанг,AP.цУпрРыск,AP.цУпрТяга);

	ROSA_CallEvent(evPWM2);
}

/* вывод кадра ШИМ завершен */
void Rdy_PWM2(void)
{
	/* суммируем управление на приводы */
	mixINS2.rasch(AP.инс.Крен*1024.0f,AP.инс.Тангаж*1024.0f,AP.инс.Рыск*1024.0f,AP.инс.векРасстоянияИНС.z);
	mixPID2.rasch(AP.цУпрКрен,AP.цУпрТанг,AP.цУпрРыск,AP.цУпрТяга);
}

В консоли для контроля пропусков обработки использую :


const char* str_hndlr[] = { "ДУС","Аксель","ШИМ1","ШИМ2","Магн.","Баро","ГНСС","Р/У","Телем.","Связь"};

.........


					t = millis()+1002;
					while (getchar() == EOF){
						if (millis()-t > 1000){
							printf("\r\nСистемное время: %d",millis());
							printf("\r\nОбработчик \tСост.  \tСч.пропусков\r\n");
							for (Event_Number_TypeDef i = 0;i<Event_Count;i++)
							{
								//char c*[] = {"Стп","Раб","Жд"};
								char s;
								switch(ROSA_GetStatEvent(i)){
									case EVENT_WAIT:				s= 'W'; break;

									case EVENT_READY:
									case EVENT_RUN:					s= 'R'; break;

									case EVENT_BLOCK:
									case EVENT_BLOCK_WAIT:	s= 'B'; break;

									default: s= 'S'; break;
								}
								ROSA_InterruptDisable();
								int c = ROSA_GetFailCount(i);
								ROSA_ResetFailCount(i);
								ROSA_InterruptEnable();

								printf("%s:\t\t%c\t%d\r\n",str_hndlr[i],s,c);
							}
							printf("Обработчиков в ожидании: %d\r\n",ROSA_GetWaitingEvent());

							t = millis();
						}
				  }

Вывод:


Системное время: 412922264
Обработчик      Сост.   Сч.пропусков
ДУС:            S       0
Аксель:         S       0
ШИМ1:           S       0
ШИМ2:           S       0
Магн.:          S       0
Баро:           S       0
ГНСС:           S       0
Р/У:            S       0
Телем.:         S       40
Обработчиков в ожидании: 0
rual

2SergDoc Сергей, поделись кодом для настройки МПУ9150, ну и особенностями, если не жалко )
Решил тут попробовать новый кодогенератор для СТМ32, низы под ТауЛавс Спарки переделываю, ибо на эроплан у меня Ф4Бы не лезет.