Создание собственной системы стабилизации
Дело в том, что отрицательные числа в ЭВМ хранятся в т.н. дополнительном коде. Он получается из нормального кода инвертированием всех разрядов и добавлением единицы.
Т.е. получается код
int acc;
int res;
if (acc > 127)
res = ~acc + 1; // если реальная разрядность переменной acc не 8 бит, то res = ~(acc - 128) + 1;
else
res = acc;
Хотя при получении отрицательного числа все вышеперечисленное автоматом делается, так что вроде как достаточно самого простого:
if (acc > 127)
res = acc - 128;
else
res = acc;
C char тоже всё работает без всяких извращений, это я лох, полез дальше разбираться где напортачил
timer=millis(); - оно надеюсь берёт время от начала программы?
Ввёл для гироскопов дополнительный фильтр:
#define FI 0.07 - коэффициент
//gyros
int gyroZeroX;//x-axis - откалиброванное значение
float gyroXadc; // отфильтрованное значение показаний
float gyronoXadc; // нефильтрованное значение
float gyroXrate; // реальная я надеюсь угловая скорость
float gyroXangle; // ну и угол
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gyroXadc=gyroZeroX; // пришлось приравнять перед loop , а то если будет в 0 то полный бред
}
void loop()
{
timer=millis();//start timing
dtime=timer-oldtime;
gyronoXadc = analogRead(gX);
gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI));
gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec
gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование
показания стали стабильнее, но углы всёравно плывут, надо чёт придумывать с интегрированием…
Наверно будет лучше поступить так с gyroXrate, а не с gyroXadc ?
Показания и будут плыть.Это абсолютно нормально, супер результат - 1 градус в минуту.Почти не достижимо. Чуть улучшает ситуацию оверсэмплинг, то есть многократное повторное чтение ADC и последующее усреднение значений, можно увеличить таким образом разрешение. Но для нормальной стабилизации по углу нужно делать фьюжн нескольких сенсоров.
И не стоит делать так dtime/1000 , dtime/1000.0 позволит избежать трудновылавливаемых багов.
соскакивает именно первый просчёт угла - время большое, может попробовать так:
если dt>100 то угол=угол (тоесть 0) иначе gyroXangle=gyroXangle+gyroXrate*dtime/1000.0; ?
я просто пока притормозил выполнение перед снятием показаний, время > 600
далее время <20
Ну вот, я опять в тупике, расчёты вроде верные, даже на выходе показания приемлимые, программа выполняется за 20 милисекунд, но вот реакция на повороты в пространстве слишком медленная примерно 1-1.5 секунды, придётся пересмотреть концепцию, боюсь что полностью…
То есть Вы уже интегрируете и у изменения угла такая большая задержка?А чем меряли? У нас с Вами очень близкие проекты, меня всегда можно найти в
Skype : ivereninov
Я слегка погорячился, это у меня так по всей видимости просто Com с Serial monitor подтормаживают было также пару глюков, мог фильтр не стартонуть, сейчас вроде всё более менее, платка лежит уже полчаса, углы считает пока тьфу-тьфу без сбоев:) вот на данный момент последняя версия расчётов
Кстати возникла идея на счёт КУКа и акселерометра , считаем угол из показаний акселерометра делим на Dt получаем якобы угловую скорость ну естественно PID для сопоставления данных, применяем альфа-бета фильтр(довольно простой и маленький) - подставляем далее в куковские расчёты и наблюдаем за аппаратом😁 ?
с куком есть одна проблема все таймеры заняты а при чтении по SPI нужно прерывание таймера:(
Cергей а если упростить задачу- сделать стабилизацию по след алгоритму- приемник- аксель- кук, то есть сигнал с приемника подмешивает данные с акселя, а уже туда подмешивает данные с платы кука, то есть у вас получается обычный кук, но с акселем, , да получится громоздко, но это на первое время
давно такое предлагал сделать для трикоптера, без платы кука
приемник- аксель- гира на каждый луч. аксель независимо от гироскопов высчитывает уплывание горизонта и подмешивает сигнал, а гироскопы отрабатывают мгновенные колебания. здесь так же можно сделать только вместо гир на кждый луч- будет плата кука.
система в будущем получится модульной с удобно настройкой
А как реализуется определение углов от акселя при наличии линейных ускорений? Просто при сумме по всем осям = g ?
а как напряжение на выходах у акселя и гир перевести в углы? в даташитах же написано какое напряжение на выходах акселя при определенных углах, у гир напряжение зависит от ускорения, как все это перевести в математику я не знаю
ну вопервых, напряжение на аналоговых выходах - индикатор измеряемого параметра который можно померять и перевести в понятные для математики цифирки, а вот дальше считать, считать и считать
гироскопы:
gyronoXadc = analogRead(gX); //считали данные с выхода гироскопа
gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI)); // ну тут слегка фильтронули
gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec собственно угловая скорость= (показания - нулевыепоказания)*2.4
gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование угол = угол предыдущий + угловая скорость * делта t (время между двумя цыклами расчёта)
акселерометр:
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//расчёт длинны вектора)
accXangle = acos(accXval/R)*RAD_TO_DEG-90; // расчёт угла между вектором и осью X акселерометра arccos(проекция вектора ускорения свободного падения/ длинна вектора)* перевод из радиан в градусы
accYangle = acos(accYval/R)*RAD_TO_DEG-90;
о как раз в программном лажу обнаружил пойду переделывать, а то блин если оба угла с акселерометра равны нулю то все расчёты летят к чертям …
Слабо гироскопы местами перепутать😁
так а как у Вас реализована защита от наличия линейных ускорений?Они же будут “портить” угол.
Основные расчёты берутся именно с гироскопов, а для коррекции показания с акселерометра именно через альфа-бета фильтр,
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
его ещё комплиментарным называют,
если же имеются именно линейные ускорения это даже хорошо допустим аппарат смещается по какой-то оси без крена и тангажа, в любом случае на данное смещение нужно противодействие в данном случае опять же изменение угла которого на самом деле то и нет, тоесть для противостояния порыву ветра аппарат же надо наклонить в противоположном направлении, как-то так, всё равно бьюсь над съёмом показаний ненравятся они мне (именно сами показания)…
надо наверно гироскопы запитать всё-таки 5ю вольтами и переделать общение по i2c с акселерометром
о а нафига я два раза угод с гир расчитываю😵
попробова считать углы с акселя через atan2
accXangle = atan2(accZval,accXval)*RAD_TO_DEG+135;
accYangle = atan2(accZval,accYval)*RAD_TO_DEG;
почему-то так только получается
похоже аксель я доканал - глючит
чёт с переменными нето знак я всётаки гдето теряю
Ну вот последнее на чём остановился, по крайней мере углы правдоподобные
accXangle = -atan2(-accXval,-accZval)*RAD_TO_DEG;
accYangle = -atan2(-accYval,-accZval)*RAD_TO_DEG;
теперь буду каким-то образом прошивку добивать (управление, шымы, настройки и т.п.)…
а теперь представьте, что коптер резко начинает изменять высоту.Что будет с углом от акселя?
перевернётся, я уже модуль ввожу для Z, остаётся только -90 +90 сейчас воюю с ним
У меня оказывается и стол шатается и переворачивается даже😵
Сейчас решаю проблему
либо коэффициент силы трения лыж с асфальтьм слишком высокий, либо коэффициент моего интеллекта слишком низкий😁
покрайней мере уже в чужом коде разбиратся начал…
Мне кажется разумным следующий аглоритм.
1.Проверяем что сумма ускорений по всем осям=g.
2.Если так, то обнуляем ошибку интегрирования от гироскопов.
Вопрос лишь в вероятности ситуации, когда сумма ускорений будет g , но коптер будет ускоряться…надо посчитать…