Premetto non essere farina del mio sacco...l'ha proposto Mis su rc group quando ho postato sul topic che la modifica era ferma alla vecchia versione e non funzionava su MW2.3...
* #if(SONAR)* * * * * * * // BARO+SONAR Fusion* * if(sonarAlt > 0 && f.SMALL_ANGLES_25) {* * * // use sonar only if valid sonar data, and with inclinations below 25 degres* * * float ratio = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);* // baro/sonar ratio* * * ratio = constrain(ratio, 0.0f, 0.90f);* * * * * * * * // min sonar ratio = 0, max sonar ratio = 0.9* * * BaroAlt = BaroAlt * (1.0f-ratio) + sonarAlt * ratio;* * }* #endif* alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)Need declaration for one "float scale;" variable at begin of "etEstimatedAttitude" function.Then on config.h we need two more decalrations:#define SONAR_BARO_FUSION_LC 100 // bellow 100cm use only sonar#define SONAR_BARO_FUSION_HC 500 // above 500cm use only baroThat's all. Bettwen 1 and 5m althold use baro+sonar fusion proportional to sonar height (lower -> more sonar usage), below 1m only sonar is used, and above 5m only baro is used.Additionally sonar is used only if inclination of copter is lower than 25deg, because in higher inclinations reading of sonar can have too high error.
Inviato dal mio GT-I9300 utilizzando Tapatalk
* #if(SONAR)* * * * * * * // BARO+SONAR Fusion* * if(sonarAlt > 0 && f.SMALL_ANGLES_25) {* * * // use sonar only if valid sonar data, and with inclinations below 25 degres* * * float ratio = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);* // baro/sonar ratio* * * ratio = constrain(ratio, 0.0f, 0.90f);* * * * * * * * // min sonar ratio = 0, max sonar ratio = 0.9* * * BaroAlt = BaroAlt * (1.0f-ratio) + sonarAlt * ratio;* * }* #endif* alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)Need declaration for one "float scale;" variable at begin of "etEstimatedAttitude" function.Then on config.h we need two more decalrations:#define SONAR_BARO_FUSION_LC 100 // bellow 100cm use only sonar#define SONAR_BARO_FUSION_HC 500 // above 500cm use only baroThat's all. Bettwen 1 and 5m althold use baro+sonar fusion proportional to sonar height (lower -> more sonar usage), below 1m only sonar is used, and above 5m only baro is used.Additionally sonar is used only if inclination of copter is lower than 25deg, because in higher inclinations reading of sonar can have too high error.
Inviato dal mio GT-I9300 utilizzando Tapatalk
Commenta