Вопросы по iNav
Прошу не забывать про то что самолетчики так же питают надежды на ваш код.😃
У вас же это уже сделано и работает? Можно глянуть код?
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”
@jShadow Като сранно он себя ведет послe быстрых флипов, уходит по Yaw, приходится доводить. Константин, вы случайно не на базе Cleanflight 1.8 все это писали. Там помойму был Bug…
Изначальная база была 1.10, после этого куча изменений. Логи черного ящика с этой проблемой есть?
Лога к сожалению нет. На видео (0.13) видно как он прокручиваеться
Лога к сожалению нет. На видео (0.13) видно как он прокручиваеться
На видео я это тоже заметил, причина непонятна, без лога не разберешься 😦
Надо будет BlackBox подкупить. Какой посоветуете? Кстати, Bluetooth и OSD подлучаются паралельно или только одно из двух можно подключать?
Надо будет BlackBox подкупить. Какой посоветуете? Кстати, Bluetooth и OSD подлучаются паралельно или только одно из двух можно подключать?
Я летаю с контроллерами, у которых Blackbox на борту. Еще у меня есть вот этот - тоже работает.
Bluetooth и OSD можно параллельно, если OSD использует LTM или MAVLink. Если это ScarabOSD, они с Bluetooth подерутся за линию данных “к контроллеру”.
А у каких контроллеров он на борту? У меня MinimOSD KV Team, значт нет…
А у каких контроллеров он на борту?
Naze Rev6, CC3D, SPRacingF3, RMRC DoDo
У меня MinimOSD KV Team, значт нет…
Нет, она работает по протоколу MSP, соединять параллельно с Bluetooth - лотерея, если не повезет можно и пожечь либо Bluetooth либо OSD. Хотя, резисторы по 220 Ом в цепь TX OSD или Bluetooth (до соединения) - и вперед. Должно работать.
Ухххх, не знал что у них столько внутренней памяти.
Надо будет BlackBox подкупить. Какой посоветуете?
Вот такой
Ухххх, не знал что у них столько внутренней памяти.
Память не внутренняя, просто распаян дополнительный чип
Без GPS FIX вы не сможете за армить, или надо в CLI написать “set nav_extra_arming_safety = off”
А параметр такой есть для отключения невозможности арминга, если летательный аппарат находится не в горизонте (например на боку или перевернут)?
А параметр такой есть для отключения невозможности арминга, если летательный аппарат находится не в горизонте (например на боку или перевернут)?
В CLI:
small_angle - максимальный угол крена, при котором арминг разрешен. Если сделать set small_angle = 180 можно заармиться даже вверх ногами.
Подскажите пожалуйста новичку, что нужно докупить и как потом это подключить для такого контроллера CC3D от Eachine Racer 250, чтобы работал RTH и GPS Hold? 😃 Или лучше сразу перейти на naze32 DOF 10?