Создание собственной системы стабилизации
Надеюсь не утомил Вас длинным изложением, подскажите хоть что то, кто может…
готовых рецептов никто не даст, точнее эти “рецепты” Вас не устроили (я про исходники МВ). А 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 единицы, дубовый видимо да?
Можно наверно с ним даже не пробовать что либо получить дееспособное?
у меня сейчас аксель
маркировку пожалуйста…
MMA7455
включайте в 10-ти битном режиме иначе из-за шума самого датчика показаний толком не видно будет…
так себе датчик, только большие углы покажет…
10-битный в нем только при шкале 8G работает (шило на мыло) , короче ясно - мягко говоря не очень, но попробую конечно для начала хоть что то нащупать…
Да к тому же он в дополнительном коде выдаёт показания, т.е. при 8-ми битном режиме в char забирать надо, а потом с ней мучатся 😦 , а почему в другую переменную не запихнуть, всё просто- знак (±)теряется 😦
Мне б сначала хоть какой нибудь результат стабилизации получить, зацепиться за тему, уж очень хочу свой понятный от А до Я код написать без коммерческой универсальности, просто чтоб летало…
Чужой код, для меня , читать а тем более править трудновато, а хочется чтоб увлечение имело какое то развитие в перспективе, из-за этого и связался… (многие скажут еще один велосипед изобретает), хочу и все…
Угол_Х слева равенства это искомое значение (отклонение ЛА) Угол_Х внутри скобок это предыдущее значение гиры за время цикла опроса dT?
нет, это уже обновлённое значение угла по ДУС(датчик угловых скоростей, гироскоп - неправильное название для МЕМС ДУС,импортный жаргонизм или упрощенное представлние), высчитаное в цикле по первой формуле.
dT в условных единицах ? (видимо мСек., или как оно вообще там реализовано)
в единицах приведённых к весу разряда датчика, т.е. если вес разряда дус 1рад/с, то dt в секундах, если 0,0001 рад/с, то dt в миллисекундах.
0.8 и 0.2 - что это?
вес влияния датчика на итоговое значение угла.
и с акселя данные уже как то обсчитанные или “голые”?
Аксель даёт вектор суммы действующих ускорений, чтобы получить угол надо посчитать обратную тригонометрию от координат вектора. На АВРе обычны приближенные вычисления…
Александр, а можете сформулировать перспективность 32-х разрядов для данной задачи или если хотите тупиковось AVR?
Если Вам интересно выжимать такты из АВР и Вы хорошо знакомы с методами приближенных или алгоритмами быстрых вычислений, то вперёд - сделаете стабильно летающую платформу с минимумом доп функций, дальнейший рост функций только добавлением доп процов. И по Вашим же словам у Вас пока нет представления опринцыпах работы подобных систем, посему будет лучше сосредоточится над решением вопросов связанных с системой управления, а не над оптимизацией кода. Т.е. АВР свяжет по рукам и ногам ваши эксперименты…