Вопросы по iNav
задержка фазы в данных приводит к болтанке и необходимости ослабления ПИД-ов в итоге… проверено…
А привнесение своей задержки может в итоге привести к положительной обратной связи и колебаниях уже в расчетной позиции, тоже проверено.
это как? для разных скоростей разный лаг?
Нет, для разных GPS-модулей разный лаг. В координатах на модели Pedestrian около 200-500 мс обычно, на модели Airborn<4G менее 100мс.
У вас в коде ГПС лаг выставлен на 1.5 сек. Это на глазок я так понимаю? ИНС уже работает на удержние позиции в полевых условиях?
0.2 сек работает в полевых условиях, причем работает неплохо. Отсутствие задержки баро колебаний высоты не дает.
На сколько тяжелый BiQuad фильтр? В чем его плюс? Не дает задержки фазы на нужных частотах?
Легкий, не сложнее FIR. Плюс в том, что параметры можно легко посчитать для любой частоты среза, в отличие от FIR. Фазовую задержку вносит больше, чем FIR или примитивный RC-фильтр, но это с лихвой окупается настраиваемой АЧХ.
А привнесение своей задержки может в итоге привести к положительной обратной связи и колебаниях уже в расчетной позиции, тоже проверено.
так не будет же своей задержки… т.е. ошибка будет высчитана из значений баро и актуальных расчетных данных (по акселю) из хистори, т.е. данные одной временной шкалы…
Нет, для разных GPS-модулей разный лаг. В координатах на модели Pedestrian около 200-500 мс обычно, на модели Airborn<4G менее 100мс.
это понятно что лаг разный для разных модулей… а как меряли? по логам?
0.2 сек работает в полевых условиях, причем работает неплохо
лаг баро ~0.3 сек… по вашей логике для шустрых гпс модулей хистори не нужна 😃
так не будет же своей задержки… т.е. ошибка будет высчитана из значений баро и актуальных расчетных данных (по акселю) из хистори, т.е. данные одной временной шкалы…
Это если угадаешь. А если попадется более шустрый сенсор то расчетные данные могут оказаться из “прошлого” по отношению к баро, что скорее всего приведет к нарушению сходимости (осцилляция расчетной координаты).
это понятно что лаг разный для разных модулей… а как меряли? по логам?
По логам, сравнивал данные, выдаваемые GPS и наблюдаемые по акселерометру. Меньше 200мс значение ни разу не получалось, так что 200мс - вполне безопасное значение по умолчанию.
лаг баро ~0.3 сек… по вашей логике для шустрых гпс модулей хистори не нужна
Нужна, с этим я не спорю, проблема в том, что мои попытки ее внести приводили только к ухудшению сходимости.
Всё усугубляется еще и тем, что дискретность данных разная. У интеграторов ИНС - 500Гц-1кГц, смотря какой looptime выставлен, у GPS - 5-10Гц, у баро - 20Гц, а у хистори (и у выходных данных ИНС) - 50Гц.
Это если угадаешь. А если попадется более шустрый сенсор то расчетные данные могут оказаться из “прошлого” по отношению к баро, что скорее всего приведет к нарушению сходимости (осцилляция расчетной координаты).
Если ориентироваться на mpu6050/6000 (где внутренний фильтр на 42 или 20гц) и ms5611, то картина предельно ясная. Выводим расчетные высоту по акселю из хистори и сырую высоту по баро на графики и подбираем задержку буфера для хистори по схождению этих двух кривых.
У меня вышло ~325мс. Сам удивился, т.к. ожидал ~150мс макс… Получилось довольно точно, т.к. сейчас даже при обильной болтанке по вертикали, расхождение минимально…
По логам, сравнивал данные, выдаваемые GPS и наблюдаемые по акселерометру. Меньше 200мс значение ни разу не получалось, так что 200мс - вполне безопасное значение по умолчанию.
Это вы наверное на довольно современных модулях катались 😃
Пристарелые народные mtk3329/3339 и ublox-ы определенно от 500мс и выше. Хотя я пытался бегать стометровки по полю уже полтора-два года назад 😃
В прошлом году Алексей Козин придарил мне парочку на mtk3333 с поддержкой Глонасс. Этот уже пошустрее, может там и будет 200-300мс. У вас на этом чипсете нет модулей? Не меряли?
Если ориентироваться на mpu6050/6000 (где внутренний фильтр на 42 или 20гц) и ms5611, то картина предельно ясная. Выводим расчетные высоту по акселю из хистори и сырую высоту по баро на графики и подбираем задержку буфера для хистори по схождению этих двух кривых.
У вас же это уже сделано и работает? Можно глянуть код? Вдруг я где-то делаю элементарную ошибку из-за чего у меня оно и не работает.
Пристарелые народные mtk3329/3339 и ublox-ы определенно от 500мс и выше. Хотя я пытался бегать стометровки по полю уже полтора-два года назад
Странно, у меня на Neo-M8N получилось 200-300 мс.
В прошлом году Алексей Козин придарил мне парочку на mtk3333 с поддержкой Глонасс. Этот уже пошустрее, может там и будет 200-300мс. У вас на этом чипсете нет модулей? Не меряли?
Нету, увы.
Дрейф атмосферного давления и турбулентность фиг отфильтруешь, лучше резкие порывы воздуха к барометру не подпускать - паралон наше все
Свет тоже лучше не подпускать, например два слоя черного двухстороннего скотча с основой плюс черная изолента, немного черного паралона, например от амбушюры наушника и строить домик вокруг барометра ))
Свет тоже лучше не подпускать
Да, кстати, совсем про это забыл. Барометр темноту любит 😁
Насчёт света шутка или всерьёз?
Насчёт света шутка или всерьёз?
Абсолютно серьезно. Кроме шуток, мне барометр прикрытый белым паралоном стоил километров нервов - я никак не мог понять почему он работает дома (относительно темно) и скачет по высоте в яркий солнечный идеальный для полетов день.
Насчёт света шутка или всерьёз?
Тоже думал шутка, пока фонариком не посветил. Черный но тонкий паралон тоже пропускает свет…
Господа, может не будем зацикливаться на старых GPS модулях? Кто то же должен поддерживать товарооборот в Китае!!! 😃))
По поводу PID, я очень доволен LuxFloat. Так что всё остальное можно убрать для освобождения места.
Господа, может не будем зацикливаться на старых GPS модулях? Кто то же должен поддерживать товарооборот в Китае!!! 😃))
О да, к тому же я очень доволен тем, как летает Ublox Neo-M8N.
По поводу PID, я очень доволен LuxFloat. Так что всё остальное можно убрать для освобождения места.
У меня в разработке как раз переделанный LuxFloat. Сейчас тестируется и дорабатывается, после того как доведу его до ума, он станет единственным.
Прошу не забывать про то что самолетчики так же питают надежды на ваш код.😃
У вас же это уже сделано и работает? Можно глянуть код?
debug[1] и debug[2], в correctZStateWithBaro() функции, в идеале должны конвёржиться/совпадать на графике…
#define HIST_Z_POSITION_POINTS 13 // delay = HIST_Z_POSITION_POINTS * INS_UPDATE_INTERVAL ~= 325ms
uint8_t histZPositionCount;
int32_t histZPosition[HIST_Z_POSITION_POINTS];
uint8_t calculateZ_INS() {
static uint16_t previousT = micros(); // set first time
uint16_t currentT = micros();
uint16_t dTime = currentT - previousT;
if (dTime < INS_UPDATE_INTERVAL) {
return 0;
}
previousT = currentT;
static bool isArmed = false;
calculateRAWAltitude();
updateAccelEF_Filtered(ALT);
if (f.ARMED) {
if (!isArmed) {
isArmed = true;
resetZState();
}
float dt = dTime * 0.000001f; // dt in sec
correctZStateWithBaro(&dt);
updateZState(&dt);
saveZPositionToHistory();
return 1;
} else {
if (isArmed) {
isArmed = false;
}
}
return 0;
}
// Set the Acc weight for Acc/Baro complementary filter (CF)
// Increasing this value will reduce and delay Baro influence on the output of the filter.
#ifdef KILL_VIBRO
#define INS_Z_CONVERGE_FACTOR 3.0f
#else
#define INS_Z_CONVERGE_FACTOR 4.4f
#endif
#define INS_Z_POS_FACTOR (3.0f / INS_Z_CONVERGE_FACTOR)
#define INS_Z_VEL_FACTOR (3.0f / (INS_Z_CONVERGE_FACTOR * INS_Z_CONVERGE_FACTOR))
//#define INS_Z_ACC_FACTOR (1.0f / (INS_Z_CONVERGE_FACTOR * INS_Z_CONVERGE_FACTOR * INS_Z_CONVERGE_FACTOR))
void correctZStateWithBaro(float* dt) {
// calculate error in position from baro with our estimate
// reduce effect of air-cushion, i.e. influence of alt.baroAlt on take off and landing,
// because baro altitude value is dropped for 3-5m near the ground (i.e. not correct)
float posError = ((isTakeOffInProgress() && (alt.baroAlt < minTakeOffBaroAlt)) ? minTakeOffBaroAlt : alt.baroAlt)
- histZPosition[histZPositionCount];
/* !!! historic Z positions and baro should be at the same phase
* i.e. looks the same on chart and depends on HIST_Z_POSITION_POINTS !!! */
debug[1] = histZPosition[histZPositionCount];
debug[2] = alt.baroAlt;
//accCorrection[ALT] += altError * INS_Z_ACC_FACTOR * *dt;
ins.velocityEF[ALT] += posError * (INS_Z_VEL_FACTOR * *dt);
ins.positionEF[ALT] += posError * (INS_Z_POS_FACTOR * *dt);
}
void updateZState(float* dt) {
// calculate velocity increase adding new acceleration from accelerometers
//float velocityIncrease = (ins.accelEF_Filtered[ALT] + accCorrection[ALT]) * *dt;
float velocityIncrease = ins.accelEF_Filtered[ALT] * *dt;
// calculate new estimate of position
ins.positionEF[ALT] += (ins.velocityEF[ALT] + velocityIncrease * 0.5) * *dt;
alt.estAlt = ins.positionEF[ALT];
// calculate new velocity
ins.velocityEF[ALT] += velocityIncrease;
alt.vario = ins.velocityEF[ALT];
//debug[0] = alt.vario; // Positive when moving up
//debug[0] = ins.accelEF_Filtered[ALT];
debug[3] = alt.estAlt; // Positive when moving up
}
void saveZPositionToHistory() {
// store 3rd order estimate (i.e. estimated vertical position) for future use at 40hz (25ms)
histZPosition[histZPositionCount] = alt.estAlt; [ALT];
histZPositionCount++;
if (histZPositionCount >= HIST_Z_POSITION_POINTS) {
histZPositionCount = 0;
}
}
void resetZState() {
// reset ins Z params at ARM event
ins.positionEF[ALT] = 0.0f;
ins.velocityEF[ALT] = 0.0f;
alt.vario = 0;
histZPositionCount = 0;
for (uint8_t i = 0; i < HIST_Z_POSITION_POINTS; i++) {
//histZPosition[i] = alt.EstAlt; // reset history to current averaged raw alt
histZPosition[i] = 0;
}
}
Странно, у меня на Neo-M8N получилось 200-300 мс.
ну так это вовсе и не старый модуль…
Еще посмотрел у вас в baroCalculateAltitude() функции (стр. 175) остался фильтр. Для данной ИНС имплементации он не нужен по идее.
github.com/iNavFlight/inav/blob/…/barometer.c
BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
Глянул в multiwii код. Это наследие из IMU.cpp и судя по комменту это я его добавил когда то 😃 Эта доп. фильтрация нужна была т.к. по raw baro velocity осуществлялась коррекция в компл. фильтре…
// baroGroundPressureSum is not supposed to be 0 here
// see:
BaroAlt = ( logBaroGroundPressureSum - log(baroPressureSum) ) * baroGroundTemperatureScale;
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
//P
int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >>7), -150, +150);
//I
errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += errorAltitudeI>>9; //I in range +/-60
applyDeadband(accZ, ACC_Z_DEADBAND);
static int32_t lastBaroAlt;
// could only overflow with a difference of 320m, which is highly improbable here
int16_t baroVel = mul((alt.EstAlt - lastBaroAlt) , (1000000 / UPDATE_INTERVAL));
lastBaroAlt = alt.EstAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
// Integrator - velocity, cm/sec
vel += accZ * ACC_VelScale * dTime;
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * 0.985f + baroVel * 0.015f;
//D
alt.vario = vel;
applyDeadband(alt.vario, 5);
BaroPID -= constrain(conf.pid[PIDALT].D8 * alt.vario >>4, -150, 150);
#endif
Абсолютно серьезно. Кроме шуток, мне барометр прикрытый белым паралоном стоил километров нервов - я никак не мог понять почему он работает дома (относительно темно) и скачет по высоте в яркий солнечный идеальный для полетов день.
ловил подобные глюки 2-3 года назад… перепад высоты был до 6-8 метров!
debug[1] и debug[2], в correctZStateWithBaro() функции, в идеале должны конвёржиться/совпадать на графике…
Благодарю, поковыряю на досуге. Фокус в том что в вашем коде частота обновления ИНС совпадает с частотой обновления барометра. У меня не так, по-идее мне компенсацию задержки нужно переделывать…
Еще посмотрел у вас в baroCalculateAltitude() функции (стр. 175) остался фильтр. Для данной ИНС имплементации он не нужен по идее.
Да, действительно, это остался рудимент от МультиВия, мне он в общем-то и не нужен (только лишнюю задержку вносит). Уберу, спасибо 😃
Вопрос по калибровке магнитометра.
Калибрую с аппаратуры, сохраняю параметры.
Потом наклоняю коптер и смотрю, как меняется Heading. Так вот при наклонах по ролу - можно наклонять хоть до 45 градусов - Heading почти не меняется, при наклоне по питч вперед - все отлично, назад - при наклоне на 15 градусов начинает уходить heading, а при наклоне в 20 градусов уже уезжает до 50 градусов.
вот результаты калибровки:
set magzero_x = 27
set magzero_y = -89
set magzero_z = -24
Похожи ли они на правду?
- Как правильно калибровать магнитометр? На это дается всего 30 секунд, это довольно мало. В начало калибровки можно было бы и пикнуть - сейчас тишина, а пикает только в конце калибровки.
- Может и так сойдет? Наклон назад при навигации почти не используется, а вперед и в стороны компас не уходит.
- Может это проблема моего внешнего магнитометра? Кристалл внутри микросхемы расположен криво и никак его не откалибруешь хорошо. 😃
- Вспомнил насчет бипера,- куда подевалась команда set beeper_off_flags?
Калибрую с аппаратуры, сохраняю параметры.
Потом наклоняю коптер и смотрю, как меняется Heading. Так вот при наклонах по ролу - можно наклонять хоть до 45 градусов - Heading почти не меняется, при наклоне по питч вперед - все отлично, назад - при наклоне на 15 градусов начинает уходить heading, а при наклоне в 20 градусов уже уезжает до 50 градусов.
вот результаты калибровки:
set magzero_x = 27
set magzero_y = -89
set magzero_z = -24
Похожи ли они на правду?
Магнитометр смотнирован в той же плоскости, что и сама плата ПК или под углом? Рядом точно ничего металлического или магнитного нет? Пищалка - тоже магниит.
Как правильно калибровать магнитометр? На это дается всего 30 секунд, это довольно мало. В начало калибровки можно было бы и пикнуть - сейчас тишина, а пикает только в конце калибровки.
Включить калибровку и 30 секунд махать коптером в воздухе 😃
Может и так сойдет? Наклон назад при навигации почти не используется, а вперед и в стороны компас не уходит.
Не думаю, что сойдет. При борьбе с ветром или при торможении коптер может и назад наклониться.
Вспомнил насчет бипера,- куда подевалась команда set beeper_off_flags?
Ее заменит команда “beeper” по аналогии с “feature”. Код живет в ветке beeper-rework, войдет в 1.1 после тестов.
Сегодня немного поснимал …
@jShadow Като сранно он себя ведет послe быстрых флипов, уходит по Yaw, приходится доводить. Константин, вы случайно не на базе Cleanflight 1.8 все это писали. Там помойму был Bug…
Начал настраивать квадрик. Это первый опыт общения с Cleanflight Configurator, так что много не понятно. Квадрик не армится (в комнате тестирую). Это из-за GPS? Ещё не понял насчёт светодиодной ленты, её поддержку убирали, а вернули или нет? В версии iNav 1.0.1 она поддерживается?
Без GPS FIX вы не сможете за армить, или надо в CLI написать “set nav_extra_arming_safety = off”