Создание собственной системы стабилизации
А вот тоже пытаюсь прикрутить 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 единицы, дубовый видимо да?
Можно наверно с ним даже не пробовать что либо получить дееспособное?
у меня сейчас аксель
маркировку пожалуйста…
MMA7455
включайте в 10-ти битном режиме иначе из-за шума самого датчика показаний толком не видно будет…
так себе датчик, только большие углы покажет…
10-битный в нем только при шкале 8G работает (шило на мыло) , короче ясно - мягко говоря не очень, но попробую конечно для начала хоть что то нащупать…
Да к тому же он в дополнительном коде выдаёт показания, т.е. при 8-ми битном режиме в char забирать надо, а потом с ней мучатся 😦 , а почему в другую переменную не запихнуть, всё просто- знак (±)теряется 😦