Создание собственной системы стабилизации
Александр mahowik меня тут озадачил, новым вийным ПИД-ом , полез я сравнивать и короче наткнулся вот на что:
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500;
ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500;
} else {
if (f.ANGLE_MODE && axis < 2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}
и хде тут время цикла? - короче я как всегда запутался 😃
Таймкоп по ходу готовит 5-ю ревизию платы - обзавёлся SPI и кварц уже на 12мГц (интересно что за проц) и не понятно из каких побуждений использует РС13, РС14 для прерываний датчиков? на 4-й ревизии у него было PB12, PB13 и ничего путного из этого не вышло…
догнал оно же само в петле 😃
впихнул новый ПИД на выходных проверю…
Там в сообщении старый
новый сдёрнул отсюда code.google.com/p/afrodevices/source/…/mw.c?r=363
и хде тут время цикла? - короче я как всегда запутался
В старом нет, а в новом явно присутствует
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
Старый в петле самой…
а в этом могу задать время цикла магическое 2048…
подождём вечера/ночи, может mahowik чего умного подскажет 😃
смущает, в отличии от стандартного wii дифференциальная составляющая с плюсом, наверно надо P занизить изначально, короче пропы есть надо экспериментировать 😃
вот этот кусок:
static void pidRewrite(void)
{
int32_t errorAngle = 0;
int axis;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4;
}
}
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * cfg.P8[axis]) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * cfg.D8[axis]) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
А мне тут пришел наконец HMC5983. Буду эксперементировать с вынесением датчика на шелдвоном проводе
Буду эксперементировать с вынесением датчика
Что хотите выяснить? Что не нравилось когда компас на плате?
Да в принцепе устраивает более мене, плывет немного при перегазовках. Просто хочу узнать на своем опыте какая разница.
Вообщем чисто академический интерес.
Так как mahowik отдыхает в бане, приходится общаться через личку, он мне объяснил про новый ПИД, ну и дабы не устраивать глухой телефон, разглашу тайную переписку:
вроде догнал, он не отвязан от времени цикла, а масштабируется по времени цикла - я правильно понял?
если ты про новую имплементацию, то да
в классическом пид регуле два параметра из трех зависимы от dt
I * dt - умножение т.к. интегрирование/сложение
D / dt - деление т.к. дифферинцированиевот по простому о непонятном написано 😉 pidcontrol.narod.ru
Я в принципе понял в чём камень преткновения, и почему сделано так, а не иначе.
Я представлял себе так: есть возмущение - которое нужно погасить D составляющей - завышающей пропорцию, потом пропорцией (от этого возмущения) вернуть в исходное положение - естественно сгладив/подтянув I составляющей, но такой метод не подходит, ибо надо практически предсказать какое возмущение будет в момент “запуска” D составляющей. По этой причине используется более простое представление, где пропорцией возвращаем в нужное положение, D составляющая служит как “тормоз” от переруливания самой P ну а интегральная составляющая дотягивает до нужного положения “медленно но верно” без переруливания. По сей причине и формула выглядит так P+I-D, вот и смутило что в новом ПИДе:// -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm;как если бы было как в примере который я привёл выше…
посмотрел код… короче и там и там знак минус 😉
просто в одном случае расчет D части ведется по угловой скорости гиры,
delta = imu.gyroData[axis] - lastGyro[axis];а в другом из ошибки по гире, а она в инверте
RateError = AngleRateTmp - imu.gyroData[axis]; .... delta = RateError - lastError[axis];т.е. и там и там D работает на торможение 😉
Нарисовал инвертор для фриски, может когда-нибудь разбогатею на неё, в архиве файлы для орла:
Ой чёт ошибся, на разъёме фриски RX, размеры 12Х7мм
ПИД работает 😃 и по моим ощущениям лучше старого, нашел источник вибраций - чуть аппарат не потерял - правый задний движек разболтался - думаю как быстрее починить и долетать батарейку…
нет не долетаю сегодня - он одну обмотку покоцал 😦 да блин… слов нет…
нашел в загашнике старый статор - надеюсь завтра починю, а нет - значит соберу обратно трёху…
а да подключил напряжометр (АЦП+DMA) работает…
Кто знает секрет: в каких единицах нужно подавать данные в FreeImu c акселя и магнетометра ??
С гирой все ясно - град./сек. переводим в радианы делим на “сампл.” и т.д…
А вот с акселем и магом мне непонятно…, по логике вещей вообще без разницы какой размерности (ведь считается отношение) но в готовом коде от ARDU присутствует “магический” коэфициент для акселя:
gains[0] = 0.00376390;
gains[1] = 0.00376009;
gains[2] = 0.00349265;
Сейчас активно кручу коэфициенты TwoKi и TwoKp, наблюдаю реакцию системы, пытаюсь понять воздействие/зависимости, а тут что то в тупик зашел…
Движек починил, мудрю с микшером к своей кривой раме, учитывая что цт посередине между моторами, получается картинка непривлекательная, надо ввести коэффициенты поправочные, кто владеет математикой, проверте пожалуйста:
Перезалил в гит прошивку и исходники с новым ПИД-ом (включен по умолчанию)…
получается картинка непривлекательная
Может я не прав…, а сместить центр тяжести в любое нужное место (например батареей) не проще?
Так как mahowik отдыхает в бане, приходится общаться через личку, он мне объяснил про новый ПИД, ну и дабы не устраивать глухой телефон, разглашу тайную переписку:
Вот замедленно что там с PD происходит
Синий столбец это Р зеленый D. Столбцы направлены в ту сторону в которую от них результирующая сила. При действии возмущающей силы D P тянут в одну и туже стороны. При снятии силы в противоположные. Т.е. сначала D сопротивляется возмущению, а потом препятствует перегулированию(это самая важная его задача) от Р.
т.е. Во всех системах D направлен одинаково иначе от него вред один.
Может я не прав…, а сместить центр тяжести в любое нужное место (например батареей) не проще?
ну пока так и летаю - батарея сзади 😃
на счёт ПИД, то что вчера казалось плавным и красивым, сегодня в ветер сыграло со мной злую шутку - жуткий расколбас (с увеличением, как будто перегулирование) как следствие - переворот и минус два пропа, остался один комплект 😦 сейчас смотрю в чём бяка…
сегодня в ветер сыграло со мной злую шутку - жуткий расколбас (с увеличением, как будто перегулирование) как следствие - переворот и минус два пропа, остался один комплект сейчас смотрю в чём бяка…
Кмк слишком сурово настроено. Это основной недостаток ПИД регулятора, если закрутить его для хорошей стабильности, в ветер или при пезких маневарпх - будет расколбас. Надо П уменьшать или Д увеличивать. Но если его потряхивает с увеличением Д может стать еще худе. Т.е. PD можно настроить только под конкретный импульс от возмущающей силы.
надо ввести коэффициенты поправочные, кто владеет математикой, проверте пожалуйста:
Самое простое и надежное(хоть и не самое быстрое). Это установить AutoCAD, 2000й в самый раз 😃) начертить в нем и померить расстояния - ошибок точно не будет. Да и при прикидывании рамы он очень полезен 😃
переворот и минус два пропа
Я решил, пока досконально не пойму работу алгоритма, не пробовать полеты…(движки по 33$ жалко)
Короче перекомпенсация валит и никакие танцы не помогли, может у меня бубен не тот? 😃 переключил на стандартный ПИД - проверить, может что с коптером не так, ну так и отлетал две батарейки - плавно и аккуратно(надо будет чуть Д добавить, а то небольшие подёргивания на спуске), или же я где-то напортачил с переносом, или же он (ПИД) в ветер не летабельный…
вернусь к нему позже, когда пропы приедут, а то последний комплект сейчас установлен…
Я решил, пока досконально не пойму работу алгоритма, не пробовать полеты…(движки по 33$ жалко)
ну так для экспериментов, чё нить подешевле на разбить надо 😃
Самое простое и надежное(хоть и не самое быстрое). Это установить AutoCAD, 2000й в самый раз ) начертить в нем и померить расстояния - ошибок точно не будет. Да и при прикидывании рамы он очень полезен
я дружу с автогадом 😉 все предыдущие рамы в нём начерчены, только эта покупная (желание было внутрь рамы батарейку запихнуть), следующая будет складная, пока в размышлениях и в эскизах на бумаге…
Короче перекомпенсация валит и никакие танцы не помогли, может у меня бубен не тот? переключил на стандартный ПИД - проверить, может что с коптером не так, ну так и отлетал две батарейки - плавно и аккуратно(надо будет чуть Д добавить, а то небольшие подёргивания на спуске), или же я где-то напортачил с переносом, или же он (ПИД) в ветер не летабельный… вернусь к нему позже, когда пропы приедут, а то последний комплект сейчас установлен…
По моему опыту, если расколбашивает в ветер, надо P убрать на четверть. Вот такое УГ этот PID 😃)
(надо будет чуть Д добавить, а то небольшие подёргивания на спуске)
Подергивания это не обязательно перегулирование. Может D шуметь. Увеличь усреднение до 5 - станет намного лучше
deltaSum = delta1[axis] + delta2[axis] + delta3[axis] +delta4[axis] + delta;
delta4[axis] = delta2[axis];
delta3[axis] = delta2[axis];
delta2[axis] = delta1[axis];
delta1[axis] = delta;
Все это D очень не срочно(усреднение же дает лаг), если интересно понять работу PID, могу EXEшником поделиться, что на видео. В уме представить колебательный процесс - анрыл 😃 Для этого я это “стенд” и написал.
И кстати если качает на спуске(мы же постоянно падаем в зону адовой турбуленции) - это еще один признак слишком злого Р.
ЗЫ D вообще зло для коптеров. т.к. мы его получаем дифференцируя гирик. Т.е. шум от вибрации возводим во 2ю степень. Плясать надо от того на сколько разогнались моторы. Площадь под кривой тяги(разнотяга) мотора - это сила импульса. Дальше пока подсказать не буду 😃 Надеюсь скоро на железо перенесу.
Поэкспериментирую, у меня есть непреодолимое желание петлю запускать по готовности гир а не с этими извращениями:
currentTime = micros();
if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + mcfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
а да расколбас только в angle, в acro и horizont летабельно…
И кстати если качает на спуске
не качает, слегка поддёргивает, p меньше 4-х не хочу опускать, а d я поставил 30 (обычно 35 но лень было с батарейками в карманах домой возвращаться) приедут меньшие пропы должно быть лучше (12-е слишком большие для этой рамы)
а да расколбас только в angle, в acro и horizont летабельно…
А мультивий-мутант или что? Скорее всего где-то ошибка в коде. Когда все правильно P вLEVEL режиме это I в ACRO. Просто не абстрактный как I а с привязкой к горизонту. Но такой же линейный коэффициент, если соблюдать все размерности - даже численно равны будут при прочих равных.
Я что бы ловить такие пакости такую хрень соорудил
Но пока к USB подключен и к акку, валиться по питанию видимо(атмега перегружается) 😦