Создание собственной системы стабилизации
а да, трёхэтажная - ИМУ и GPS можно менять без замены всего, думаю проц на долго останется и ИМУ ближе к ЦТ аппарата…
причина понятна.
Ещё один момент, почему на SPI выделил только один CS? Если стремишся к универсальности, то возможно будешь ставить датчики с несколькими ус-вами SPI.
В принципе да, можно ещё по две лапы задействовать на одном и на втором:)
Вобщем, дабы не плодить ненужных файлов, отпишусь на словах:
Сонар переехал на PE7 - эхо, PE8 - триггер, тем самым отжал ИМУ 4 резервных вывода(PE0,PE1,PC4, PC5), 2 из которых могут также быть и входами АЦП и входами прерываний PC4, PC5…
также добавил к выносному SPI два резервных вывода PB10, PB11, которые также могут быть второй шиной I2C…
Ну и добавил питатель на аналоговую часть, а также отделил дросселями аналоговую землю и аналоговые 5В от остального…
Вроде всё, а ещё, у F4 нет VREF- вмесо неё просто аналоговая земля, а то что у F1XX было VSSA у 4-го VDD ! чуть не напоролся 😃
Буду разводить, проц и большинство деталей будут со стороны пайки (внизу), чтобы с разъёмами проблем не было и ИМУ ниже ляжет…
Приветствую Вас, единомышленники.
прочитал вашу ветку на форуме и сделал вывод что можете мне помочь советом (или подсказкой).
Также как и вы хочу написать собственный софт для квадрокоптера.
Изучил в инете темы по алгоритмам и принципам стабилизации, решил для начала просто повторить рабочий мультивийный алгоритм, но тут столкнулся с проблемой.
Проблема в том что программирую в CVavr на Си, попытки разобраться с листингом проекта мультивии не увенчались успехом (уж очень он универсальный и “оброс” доп. функциями).
Понял только, что в нем допускаются некоторые упрощения в отличии от базовой теории.
Хочу всего лишь понять основной математический прием для гироскопа и акселерометра.
Постановка вопроса:
имеем последовательно считанные в основном цикле программы данные с гиры и акса (магнетометр о др. не интересует)
типа INT, назовем их условно Gx,Gy,Gz,Ax,Ay,Az.
Можете просто подставить их в формулу чтобы получить текущую переменную воздействия для скармливания ПИДу?
Или хотя бы объяснить мне бестолковому последовательность работы с этими величинами для получения результата стабилизации. Понятно что еще есть данные со стиков пульта, но допустим, чтоб не усложнять ответ примем их за ноль.
Надеюсь не утомил Вас длинным изложением, подскажите хоть что то, кто может…
ну если смотреть комплиментарный фильтр то
уголX = (0.8*(уголX+(угловая_скорость_с_гиры*dt)))+(0.2*(угол_X_с_акселя));
где уголX+угловая_скорость_с_гиры*dt интегрирование угловой скорости по времени - угол из показаний гироскопа, угол_X_с_акселя - угол, высчитанный из показаний акселя, ну это если по простому…
А вот тоже пытаюсь прикрутить AHRS 9DOF но что то туговато все идет. Датчики HMC5883L и MPU6050. Может кто то поделится исходниками или поможет консультацией?
на прошлой странице rcopen.com/forum/f134/topic224458/845, а также MultiWii, и ещё куча примеров…
Надеюсь не утомил Вас длинным изложением, подскажите хоть что то, кто может…
готовых рецептов никто не даст, точнее эти “рецепты” Вас не устроили (я про исходники МВ). А SergDoc чётко обрисовал основную формулу
, правда я её немного дополню:
уголX += 0.8*угловая_скорость_с_гиры*dt;
уголX -= 0.2*(уголX -угол_X_с_акселя);
Так как последний член есть поправка интеграла по абсолютному датчику (в том смысле что гиро датчик относительной величины).
И ещё момент, если Вам это нужно чтобы получить приемлемый результат и забросить, то пойдёт и АВР; если же есть желание двигаться дальше в части развития функционала и качества системы, то советую смотреть в сторону 32х разрядных процов.
Тааак, спасибо, поправьте если не так понял:
Угол_Х слева равенства это искомое значение (отклонение ЛА)
Угол_Х внутри скобок это предыдущее значение гиры за время цикла опроса dT?
dT в условных единицах ? (видимо мСек., или как оно вообще там реализовано)
0.8 и 0.2 - что это?
и с акселя данные уже как то обсчитанные или “голые”?
Александр, а можете сформулировать перспективность 32-х разрядов для данной задачи или если хотите тупиковось AVR?
Очень интересно понять, глобально эту проблему…
dt это дельта t - отрезок времени между измерениями вообще в системе СИ в секундах меряется…
0.8, 0.2 - коэффициенты фильтра (легко найти - комплиментарный или альфа-бета фильтр)
угол считается из проекций вектора ускорения свободного падения на оси акселерометра…
То есть все данные в формуле приводятся в системе Си: град/сек, градусы, и секунды соответственно.
dT как то придется считать аппаратно или программно между двумя обращениями к гироскопу…
понятно, буду пробовать…
С шумами датчиков МультиВии видимо не борятся ни как, чем точнее датчик тем лучше?
Не ну можно считать и в попугаях, только гироскопы выдают угловую скорость в град/сек, тогда и эти величины придётся переводить в град/попугай 😃
Понял, понял 😃, просто мучаю гиры Murata(аналоговые), поэтому и сомневался
С муратовскими гирами не всё так просто, у них 0.67мВ/град/сек и 0 у них 1.35В, 10-ти битного АЦП маловато, по сей причине надо для начала на Aref АВРки подать 1.65В (не забыв analogReference(EXTERNAL); сделать) - это максимум что могут выдать гиры, тоесть 1.65В будет равно 1024 для АЦП и тогда уже пытатся считывать с них какую-то информацию 😃
Я использую следующий алгоритм:
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
}
Углы с кватерионов получаю так:
void getEuler(void) {
angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI; // psi
angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180/M_PI; // theta
angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 180/M_PI; // phi
}
Но значения углов скачут от фанаря. Что я не так делаю?
Это, насколько я понимаю, уже готовый фильтр на квартенионах, а что на входе?
Извиняюсь, вопрос на вопрос, а recipnorm() это встроенная функция матлаба ?
в коде должно встретится что-то на подобии
time=micros();
Dt=time-oldtime;
oldtime=time;
это и есть расчёт dt потом уже его делим (зависит от того в чём запущен отсчёт) чтобы получить секунды…
а recipnorm() это встроенная функция матлаба ?
на сколько вижу из приведённого вами кусочка кода это float переменная
Это, насколько я понимаю, уже готовый фильтр на квартенионах, а что на входе?
На входе данные с гиро и акселя.
recipNorm да это переменная.
не ну понятно, вопрос какие, для муратовских, и если соблюсти условия что я приводил выше, то данные с гир будут выглядеть так:
gyroXrate = (gyroXadc-gyroZeroX)*2.4; где gyroXadc - цифирки снятые с АЦП, gyroZeroX нулевые показания гиры с АЦП, 2.4 - коэффициент приведения к градусам в секунду из показаний АЦП, а какой аксель я не знаю…
С муратовскими гирами не всё так просто, у них 0.67мВ/град/сек и 0 у них 1.35В, 10-ти битного АЦП маловато, по сей причине надо для начала на Aref АВРки подать 1.65В (не забыв analogReference(EXTERNAL); сделать) - это максимум что могут выдать гиры, тоесть 1.65В будет равно 1024 для АЦП и тогда уже пытатся считывать с них какую-то информацию 😃
Сергей спасибо за поддержку!
Как считаете, у меня сейчас аксель (на пробу взял) с диапазоном 1G=64 единицы, дубовый видимо да?
Можно наверно с ним даже не пробовать что либо получить дееспособное?