From f87c081475006fdb2a2238be9b79a4e5f9678523 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 21 Aug 2022 22:06:53 +0300 Subject: [PATCH 01/61] gps fix estimation squashed commit --- docs/GPS_fix_estimation.md | 87 +++++++++++++ docs/Screenshots/gps_off_box.png | Bin 0 -> 10195 bytes src/main/fc/fc_msp_box.c | 6 +- src/main/fc/rc_modes.h | 1 + src/main/fc/runtime_config.h | 1 + src/main/fc/settings.yaml | 5 + src/main/flight/failsafe.c | 2 +- src/main/flight/wind_estimator.c | 3 +- src/main/io/gps.c | 123 ++++++++++++++++++ src/main/io/osd.c | 22 ++-- src/main/io/osd_dji_hd.c | 2 +- src/main/navigation/navigation.c | 13 +- src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 2 +- .../navigation/navigation_pos_estimator.c | 13 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/frsky_d.c | 6 +- src/main/telemetry/ghst.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/sim.c | 2 +- src/main/telemetry/smartport.c | 2 +- src/main/telemetry/srxl.c | 4 +- 22 files changed, 273 insertions(+), 29 deletions(-) create mode 100644 docs/GPS_fix_estimation.md create mode 100644 docs/Screenshots/gps_off_box.png diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md new file mode 100644 index 00000000000..4cf9f010837 --- /dev/null +++ b/docs/GPS_fix_estimation.md @@ -0,0 +1,87 @@ +# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing + +Video demonstration + +[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/wzvgRpXCS4U/0.jpg)](https://www.youtube.com/watch?v=wzvgRpXCS4U) + +There is possibility to allow plane to estimate it's position when GPS fix is lost. +The main purpose is RTH without GPS. +It works on fixed wing only. + +Plane should have the following sensors: +- acceleromenter, gyroscope +- barometer +- GPS +- magnethometer +- pitot (optional) + +By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above inreachable spaces, plane will be lost. + +GPS fix estimation allows to recover plane using magnetometer and baromener only. + +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. + +# How it works ? + +In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. + + +Without GPS fix, plane has nose heading from magnetometer only. + +To navigate without GPS fix, we make the following assumptions: +- plane is flying in the direction where nose is pointing +- plane is flying with constant speed, specified in settings + +It is posible to roughtly estimate position using these assumptions. To increase heading accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase speed estimation accuracy, plane will use pitot tube (if available). + +From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. + +It is assumed, that plane will fly in roughtly estimated direction to home position untill either GPS fix or RC signal is recovered. + +*Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. + +# Settings + +GPS Fix estimation is enabled with CLI command: + +```set inav_allow_gps_fix_estimation=ON``` + +Also you have to specify criuse airspeed of the plane. + +The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. + +Cruise airspeed is specified in m/s. + +To convert km/h to m/s, multiply by 27.77. + + +Example: 100 km/h = 100 * 27.77 = 2777 m/s + +```set fw_reference_airspeed=2777``` + +*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Icrease cruise speed with throttle" - do not use it without GPS Fix.* + +*If pitot is available, pitot sensor data will be used instead of constant.* + +*Note related command: to continue mission without RC signal, see command ```set failsafe_mission=OFF```.* + +**After entering CLI command, make sure that settings are saved:** + +```save``` + +# Disabling GPS sensor from RC controller + +![](Screenshots/gps_off_box.png) + +For testing purpoces, it is now possible to disable GPS sensor from RC controller: + +*GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* + +# Is it possible to implement this for multirotor ? + +There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing. + + +# Links + +INAV HITL https://github.com/RomanLut/INAV-X-Plane-HITL diff --git a/docs/Screenshots/gps_off_box.png b/docs/Screenshots/gps_off_box.png new file mode 100644 index 0000000000000000000000000000000000000000..1c912492d2eed99c27a623a2dcaa4735d4ed4f0e GIT binary patch literal 10195 zcmaKS2Q-{t6SowS5F|nb5rXKwtg=D$-d68XmMFW6U6vIs5;ce(J%}DekFtnf62j`e zx7DIX|5lR!|NYK+zxTVx;XcoqJ2Q7??lbcnH&|0$ftZk*5DyQJSV>V9gok&10ynq0 zc@_6N?K6!6?hh40&H$kevq2y&T)=oz)-X#jy%NO277PMgSbI2kf}i5yU1PA*F+doo zsft;_Alw$0G~DhGC){d0yr+`xP8L=UU|jv(OO6(nFjs^)15VRFTYxzI%N7d%YbKn*c-$?Vc=)(^FSqm$ zp_8YTg!1$NY_wm5wq^Y6d=*Y;6Q5k2pv&^vjy4{~f%Z?LB zvbQd|-e$?{TUoeu^}|&9)ION^0STRiBWfjC3ejOakx1k|_6Y%==XM?>!I`7Z2IbmU8)L{XD#MtlY>FIy0p6p<)N<&La`{>c5 zD_5?VY_&A_ky>!YJFM6m8`Fs6&w+5P(x0U>kALXEkcF0}Ig*Adj(WPZ5Ab?X;!M%# z%qg7e>gINQd^|Ssu8jt7b7_2m)fq*YG`@_dxuv;9@aD-i5 z2%!jVjk{8^SVIh4hX#I*O9!ZoKIb)vU@+(kQoOR?W3k@@nwN{n8|}ibS!a z&bhe|F)c*DzmoIa#~`}&qD)f^$n?O}yD!U4#j^js2Puns1Qi#Bc2CV%?EZ2%u~aeO@T_MJuPnPz5%M4`dkC9gdu6W5~9H|q68%r!Ih3QV~fp6tEK%*<3#R(^OV z1|*xW!=99sWNd7_>Vbdt%9uIsWHnioEY`9rT3at{Y-wO6vDOff0bDgpz=a^(b^#U8 z^H5s*ff7Y=l|9u_vR22ugi>|ymHqHRA(c5ORU$!B3G7W?etxNJI^Zw{B=RH8;Q1GS zP;tk{H=$81if_i;vP#YDW?t8Wf+OBx*6Lcwf{P=Hs?A0h*@y`f1cACul&6gmD#>IF zj*w3U80S<8kOp7QiJg1_lZ|& z(A&nH-5e6l!YyHJ@60cwOiM)2H`IK%6*w-uvAy^CN8;4f46UBM-k__#cbh@+zn+G_ z0$u5q+V80;C3Ql57NC*zerapVVr(*OGs)Opxk#jvBE}l4=0VS`%CGoEMG(A|V^kh@uIMj6w(^~Dx9^*wult50SayM^hA|359J*?%v zN{62L*$Quy)kq>Qwjr0*2zPj~MuR702t60$1=DKaC}-rc|d1D}=jN9h!4Gw-?Vpgp~u=%V== zd0}Q&o3_)y+KBsl?5;x%#J8jHRYVGF4lF5%fYn1BwgqcBQ6 zsLE+hKv5c|?1XE|WHTqw?QlFG-^uPl(Afq}sVhHB`84%5&5`|tp6wIumwC6+ACwd!Dl#QL4@rm; z%Ed9CiC3`;R$Nr;zT@a0I4vX4rmEWYUc^CN_b8Y#(8qE8mYO(IJ;URiE&B$GThCl$ zTyp%PLs}Vj#RH!N0b@C5j?5-+(G zWvH?LPd1G8tD21jD-YNx5fB#N68^1PvXop^%fUvqd6NaqZS>@|^kW8GwsT6y8{BL} z&Nyzt1#>xDg;iaCFNLF|J~=R9wbYJ@d^7O$*~6p z#}gxbW_v7s7Jg*uQq!ZOqXM&)h39jNDB|UNqT)j0qI!T35^q5dP|)WBL+?E04Jdxj zms7?nPMZlxxVumvN~@&0cnsJ^-hO`IlDp~~HgCUWxwfUkgBJ8UMi5G|-O)`d35s!( z*!v-qtB=zZ5ATOYbKNo2*0poivh!8Ts+Hv#fmK&lZY}Bri&_wdV4!$kkL( z<~KwjI}hB>XNxUXIZGk@VfwG=)~xbCxsT=1rXqi;EhoPZ1A>%?Cq6IrJY3e!qNTmQ z@yJi-)tQ*-VJHNpP<5swZdpe;)wNu<^`n@g_YcLD9YYj>i3=<~pBn6JUD@?N2G|RfS>7}fXS3OEx><}2zX&(`HshN9 zAqGD>EJuY}W4cbmBYk1}jf6s_PTEsFuRz|zwS;;7-jQ*S-J;61Qs9%tZ)B8polowQ zH^lwYzU^_!WyXN~_GQXq8fzJ>Vn=iLPfhy8QIqiSc~9ARTwsqWUs1a4m8rQZGIP^P z^emJCgbAsIPmiB}V#9AeICRukU-L2P0%$V-_MPts#KJrA3XBN!8c$4Bl(=`<$QRF^ z$tRJ&jG8Mc(#6^8oWK1VLz`H;MmY`awMwGUnyf9VxA5?Mlc-J{@!pp2P6zGMNwM9I zZ5e*sHoU%W2J!8|SWNT6*BG0Fb|*z)l#! zZhUnT!Qu*o#dU(Lr;N5UxGZSvv=ASm32y&DF2(ki-m=7iJvM(ZyZHR)-rmCQk-G-2 z47=5=g{yo3>8tc4W)T0WP4%9}%q!KR59Kro83kE|&OXjQPOE$po{Pj)$MPSvGUZ*} z67?*hQ0M_J7u>x5n}?l@_Q7mkTAKN^)JEaZ5-lx*)d?4koA%}j$9xzd(@%hUh4Sfv z%G(r+7CtTsHHrvZ!NI4&*Vbt1SzTqwEnSgF?N5s?#mQ!sR&z4;%586{81IMrzk2n9 z8)q6!K&IomgLnvdJg!^l@a1Y-%}JU1h2(rzo!NWUo4LsFJmkZrO$Jt1Nl;#Sem=XH z*jU9JC`54;478zQ@?X`XQfH2yrN;SIWOrtS-(_$Wfh(qmnir*1Cr|-S*49kM`0v6Z z_>>6-fHww!dc&HNcVcQ{fBg6n5D*X*MTR!plIBU8lfiLp1um_!a)SljaiAyJ!|B^G z=YeznzAFv3dDD&40e;qw3YzHa`zl;^J4Wk{FG_e|pM-)~DsQ|kizIXt7a`ACM<%M| z{WA8QM)^DAYKP^tkHg<`GApsx%72Y`P78Dspudpt2{ z6m47nouFMXxuW*Eici4gkQI~&+s9i^M(}?Lc`0cinOc)l|S)#A2Om_>V6dmq$vhV6isSkNJgt zUG+5G+{FffE_*9l$D%&xFWU!d?}kQHUE2Khuia`I0L_fMSG-pBptv0X=otV-ihbJ3 zXMaF-?Zea`IF}Qoc%e1iT4{I9d38HHLbM>k8^EZa8Vk&}I_wzFA^nf>1Ei$<4*;YV z1|u&^T0Y#a<+?@EI?VeUQ4-@%XybOt%Yax3tMa(n+_cnIcK$Zv1b(}o`Yzr+i4>b} z5`3iV3gcts<{5$&7(?q4_zHV*$Ky^KR5PHJ75Ef~ezuuKGM*pfBL8yGv%aa-Oo)!F zTxl*%f)dzw8K=cndN*{H7qWMZX?~~~*V)gHx-dh(Fiq`ZmvRU_H?osZ)!I8xe6qLQ zxnIk@SASiu?*>oeL7_V&zU~_S9}s@_X0^7+P99|s)=B_yOAXLaJmUG$^S*zVmHr8~ zqE_{@Og)!Fw41lRBV;ReX0K`o<4N~|L(q5N2v13~QGJuUFwk3zBNMHk3wcy4x&9$DkURB-GvT~N=0`{jd z?^5Y2ktRR03pcujy<*cE=3}>ug%=0m$@#26cxxq*Fdg&Fo22)g3kj>yg5(#cA;yX9 z0+4+Bu4cwpo(F*nZMtT5EK){>v^OWN$?>H97A(fQo;`oHb zP!4=NCkeBwQ;k#Pf9h^o|Y!fm%iOk&`k@0jkZLj9IU>{6D(PfjREtj>bt!I12hFDCqmf5>KhN-T;N9nT; z8@>B(o4(3T!*Hi8dl0#M&aHCB>Wg#R+|VYwh8ft%OBkmAVy*p3t|1|?ebx^z9_iRJc>$L&w9R36?0VR zliYtu{b}H`o24%G+x>sLS+{4!vn&qZR8Ip};u0>~On7(?xMd#1)U>xuM!x&o=@P#E zPp>SQgB6XcIA6_4J738pB_{Bip02FSH-Gu_LPsZNe7(23yW1OBTTp;m>M*oPNJvQY zIj`P_;f!-Rr1EGfC?pk`qViSZscDLdpH_TNx^S6m2}VwN*j}8U;o}EtCI2BUYE4Hf zx*-rTadDhxbzd?v=GLpraP6aV%nQLs;;6m(2uUbx*xK3};dWrM?PzCrw3{Y*e9-9k z`q!`RZR4W3?)jfh&F$*%Mbhi*Qy&3@Ip8iXJERHsJI-3~X`HU&nW;9mXNHCRJY% z6eMWS*bk?Q{o{Qq0h@0xpD*QYY1q|PES1gnFManp+JIe%3kxH^jg&0&n0!bf@IMdb zspqAxKJ)NsI62!YC@6r%GPjls2~|+?n0)a$KHJk|h~zY_;e<`sZ;V%_iRtPY=yZ?( z-FbbMzJ0-=XY-5hV&wWrx(@rQM-^l|F*a5%*1pdwbg0mCvF}ZUnM2Ki`Bp;%Vzr+! zkQ8T*SZO`Nk0}K$>dPOqxQ=H$3-=`k&-d-@u)B!_aiwP3)xw@?HbzEcQ9@gkY@C>a ze{;)Pj!eZ)3Rd6xV-~pRXeKw~kXNqn#3AlSsHe+W5z$zBhuFSZdjwt15|4mD^jOP1 z7In>}4!3YxV4PdO)5r4in2OjvV=-n%Mt>t!dxhkN=*qX0hW8AOjcc7|rR@oOlSP}o z<*Gl_X@+xpAgdi~usKX|5&bK(St{|lGb@fC)kC{B^?v)YowN)gmh#=3a3L2kse9?b zJ|)HCgq}`S0YElla>6hmAP&`TB;7@=E66TYOI2cg&%zxDWXESGI>cfoZ^d?0!^Ur`gYe?6y+jz+q6kDoU7Hp9pr>AVkytSfV*N0BqHd5;E=$;_Ar_a2n1qTA|j}>gvHWG z08Fy%)2CB`o_dYT?QL!Sp$Ve9SF#eyVhtyn1_b25dKPIJyyi5*9*wK{UA^v~I;IlR zE;fd2>ng3c1Qe2JcOckOc1Dm@p>ggOeCYgr`D(%GDXl|ugK1S^*D1q43H<0p;yht( zBTTN)KkX;9?B{$t#?8RseSG|Ig~RZOdV*@3TmGQEJ-Hxtm?rB(aa6e?6CojChV(~* z0lTCVnI)o=A-d0|4c;l1Fgm(495m1ZP!MVx{-9FvNPnnRzw&r-@=fV#k=FOG{x>o+ zGIF0-c; zlEV8Yv~*-3rqGrt0ztXOlgv7N1#HMF&}oUYr3=GC#uXPuD? zhm%lk*wt_Hka_%&9OI?FjY4y`n6$NYcaK98lY}o88^2zgZPetbcK3TKj-jm>?Xp7glrpi}Alf3a zFW&nk)n^F*=s2FYkvB!u>4TG!G)JG;^Rp8wnw9-bu*(1xT>h-Jx(`ry5W_4$GS0fR9I8{p?)6d3nhp;0b! zac(Pq?Q>=r8FEj}%SP?TP_J@@i9=tzPlRDKmDO=UN`qmH>Bkz~?{!1(?m+7k^zPhw z#5`w2q38EWS&N2p%2QW^{Lwv>Vfw6&(Bkafuj0UF#(DN)lJh`|PbIbPKN)Q67vBp( z25ugt_%8QvTyCqS z#%^ZxJ9+(qMXK6h73lb95=8LvI; z&r*qax)T%;5plA=y7`)&GAZ^)uVv|YkBCpP_&YzUFV8gX1*zGthlNltFAu}zco-Nm z&esNOYHHrEtSs3qR+rhb?Qn7c<(6NsVkqg-l%knq<6@@1^r<(TZH|w@YnQL%W39vKGMJ(^^~z8x3ht~ zEn<00)>w8nGfN#v*_xDlc85sqah92x3+LmrvOL>df|3`1t0{8=au_ z9&~*UtN6Nc3vXSnf@Z;BG>b-kVB*T@i#tcZ<|8mcY5Wuv6mp_jvoYff;hGJ-0a1?3 zj|acjQrtH^`#jY8y78ipTr zG~PC$XT{|U+?Ol?0HIFfS-DkUUE0ynagRm0JuDP;FuFYDxup?G$&;<<0?k-G&=Ok@ zW=Wm-`Q^>(>Q>!u&%nsYIMe1bHa@kU7v^Jz-N&lLbDr&Vi`x<1e<)${E-KL_sbS~v zFg~2R!f};9#Evfc;`zb&Xkp>niS(A|YF7Ng<|K_I&>PoZt*)*{p}$gff0sO$Dz0ln zg#$fvnS?~;pYL<%j#hpe%wDavvuhYH94vi5#=(7-UOJK8x*EMzKJr`v+++MCuUJwb zE3JTRO|dwiov*S5F?!T$hziWpi8**|j!>gq^A)?D1HL zB8sc~Th#crKIL(T5*bU#(t+jVpoqP^K@rUSRvDi__O0q}50sw!aS1)k3s&#hX(iYzk84EeD5>O8}TR7TXW@}1+%w=VAS_ zhe-4Z#!Pj)lrGZypi3VaqNrxMlnpy&fc;!RB-Ar+6-32qG$;b^Jz5yrWK;15sAsTs zp44*gXZlL@@+Z|Bo0z;@8(=#CBR&}?3fPhmGq9{K3+jB8a09TlcmX1z2a?9+%)!@@Lr&Y%qIzSPqU@*>gVYDh?|94UJ1&S+c z-z62gv?MGieXeYRxG3=O<4}zkkU^IZfX0RE@sR8T`2l}p<<|l}SD1~ZM{%emUf$w? zt39q>{d8p()sSDFc3J#P7NQ`+niaM$lNghZIG(7AS8LvjVX1YQNvZ zDwEKp^rw@=YY=ddszEqQ;9Sb_2ULHdl9>9l7}sX{-O|Lv`}Y>izgJiOxdnswU$ 0 ADD_ACTIVE_BOX(BOXOSDALT1); @@ -400,6 +403,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)), BOXPLANWPMISSION); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGPSOFF)), BOXGPSOFF); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index e72e18a84e6..d8819ad75c1 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -75,6 +75,7 @@ typedef enum { BOXPLANWPMISSION = 46, BOXSOARING = 47, BOXUSER3 = 48, + BOXGPSOFF = 49, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3a04e192f74..01fc555c42b 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -122,6 +122,7 @@ typedef enum { NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED = (1 << 9), + GPS_ESTIMATED_FIX = (1 << 10), NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f264763e688..be17f963250 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2280,6 +2280,11 @@ groups: default_value: OFF field: allow_dead_reckoning type: bool + - name: inav_allow_gps_fix_estimation + description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS." + default_value: OFF + field: allow_gps_fix_estimation + type: bool - name: inav_reset_altitude description: "Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming)" default_value: "FIRST_ARM" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 0e761f3bd08..9a1cf2e1944 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -343,7 +343,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 7fe49a45949..c58aeb99a94 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -82,7 +82,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (!STATE(FIXED_WING_LEGACY) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || - !gpsSol.flags.validVelD) { + !gpsSol.flags.validVelD || + STATE(GPS_ESTIMATED_FIX)){ return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e05260ff7e0..23fd9254a8e 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -45,14 +45,19 @@ #include "fc/runtime_config.h" #endif +#include "fc/rc_modes.h" + #include "sensors/sensors.h" #include "sensors/compass.h" +#include "sensors/barometer.h" +#include "sensors/pitotmeter.h" #include "io/serial.h" #include "io/gps.h" #include "io/gps_private.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -60,6 +65,11 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/imu.h" +#include "flight/wind_estimator.h" +#include "flight/pid.h" +#include "flight/mixer.h" + typedef struct { bool isDriverBased; portMode_t portMode; // Port mode RX/TX (only for serial based) @@ -137,6 +147,117 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) gpsState.timeoutMs = timeoutMs; } + +bool canEstimateGPSFix(void) +{ + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && sensors(SENSOR_MAG) && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); +} + +void updateEstimatedGPSFix(void) { + + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; + + static int32_t last_lat = 0; + static int32_t last_lon = 0; + static int32_t last_alt = 0; + + if (IS_RC_MODE_ACTIVE(BOXGPSOFF)) + { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; + + //freeze coordinates + gpsSol.llh.lat = last_lat; + gpsSol.llh.lon = last_lon; + gpsSol.llh.alt = last_alt; + + DISABLE_STATE(GPS_FIX); + } + else + { + last_lat = gpsSol.llh.lat; + last_lon = gpsSol.llh.lon; + last_alt = gpsSol.llh.alt; + } + + if (STATE(GPS_FIX) || !canEstimateGPSFix()) { + DISABLE_STATE(GPS_ESTIMATED_FIX); + estimated_lat = gpsSol.llh.lat; + estimated_lon = gpsSol.llh.lon; + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + return; + } + + ENABLE_STATE(GPS_ESTIMATED_FIX); + + gpsSol.fixType = GPS_FIX_3D; + gpsSol.hdop = 99; + gpsSol.flags.hasNewData = true; + gpsSol.numSat = 99; + + gpsSol.eph = 100; + gpsSol.epv = 100; + + gpsSol.flags.validVelNE = 1; + gpsSol.flags.validVelD = 0; //do not provide velocity.z + gpsSol.flags.validEPE = 1; + + float speed = pidProfile()->fixedWingReferenceAirspeed; + +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) + { + speed = pitotCalculateAirSpeed(); + } +#endif + + float velX = rMat[0][0] * speed; + float velY = -rMat[1][0] * speed; + // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame + + if (isEstimatedWindSpeedValid()) { + velX += getEstimatedWindSpeed(X); + velY += getEstimatedWindSpeed(Y); + } + // here (velX, velY) is estimated horizontal speed with wind influence = ground speed + + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) + { + velX = 0; + velY = 0; + } + + estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); + estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + + gpsSol.llh.lat = estimated_lat; + gpsSol.llh.lon = estimated_lon; + gpsSol.llh.alt = estimated_alt; + + gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); + + float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction + if (groundCourse < 0) { + groundCourse += 2 * M_PIf; + } + gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); + + gpsSol.velNED[X] = (int16_t)(velX); + gpsSol.velNED[Y] = (int16_t)(velY); + gpsSol.velNED[Z] = 0; +} + + + void gpsProcessNewSolutionData(void) { // Set GPS fix flag only if we have 3D fix @@ -154,6 +275,8 @@ void gpsProcessNewSolutionData(void) // Set sensor as ready and available sensorsSet(SENSOR_GPS); + updateEstimatedGPSFix(); + // Pass on GPS update to NAV and IMU onNewGPSData(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7198262a229..e53668bad38 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1227,7 +1227,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); float poiAngle = DEGREES_TO_RADIANS(directionToPoi); @@ -1341,7 +1341,7 @@ static void osdDisplayTelemetry(void) static uint16_t trk_bearing = 0; if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX)){ + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ if (GPS_distanceToHome > 5) { trk_bearing = GPS_directionToHome; trk_bearing += 360 + 180; @@ -1662,8 +1662,10 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); - if (!STATE(GPS_FIX)) { - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + if (STATE(GPS_ESTIMATED_FIX)) { + if (!STATE(GPS_FIX)) { + strcpy(buff + 2, "ES"); + } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { strcpy(buff + 2, "X!"); } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1717,7 +1719,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } @@ -2178,11 +2180,11 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { osdHudDrawHoming(elemPosX, elemPosY); } - if (STATE(GPS_FIX) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { osdHudClear(); @@ -2721,7 +2723,7 @@ static bool osdDrawSingleElement(uint8_t item) bool moreThanAh = false; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -2792,7 +2794,7 @@ static bool osdDrawSingleElement(uint8_t item) int32_t value = 0; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -2953,7 +2955,7 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; int digitsRemoved = osdConfig()->plus_code_short * 2; - if (STATE(GPS_FIX)) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { // +codes with > 8 digits have a + at the 9th digit diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 269ea0cd7bb..bc71851b549 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -784,7 +784,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 906db2881f0..5917ed16cb8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2371,8 +2371,17 @@ bool validateRTHSanityChecker(void) return true; } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; + //schedule check 5 seconds after apperange of real GPS fix, when position estimation coords stabilise after jump + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + return true; + } + // Check at 10Hz rate - if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { + if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); posControl.rthSanityChecker.lastCheckTime = currentTimeMs; @@ -3889,7 +3898,7 @@ void updateWpMissionPlanner(void) { static timeMs_t resetTimerStart = 0; if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { - const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX); + const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); posControl.flags.wpMissionPlannerActive = true; if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d67d679992c..0b0a393a460 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -209,6 +209,8 @@ typedef struct positionEstimationConfig_s { float baro_epv; // Baro position error uint8_t use_gps_no_baro; + + uint8_t allow_gps_fix_estimation; } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 511d92ef72d..6cfc48f5c59 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -697,7 +697,7 @@ bool isFixedWingLandingDetected(void) timeMs_t currentTimeMs = millis(); // Check horizontal and vertical volocities are low (cm/s) - bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && posControl.actualState.velXY < 100.0f; + bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && ((posControl.actualState.velXY < 100.0f) || STATE(GPS_ESTIMATED_FIX)); // Check angular rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < 2.0f; DEBUG_SET(DEBUG_LANDING, 2, velCondition); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index bd082da8085..0d6f7f28c15 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -91,7 +91,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, - .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT + .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, + + .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -150,6 +152,13 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + previousTime = 0; + return true; + } + if (previousTime == 0) { isGlitching = false; } @@ -202,7 +211,7 @@ void onNewGPSData(void) newLLH.alt = gpsSol.llh.alt; if (sensors(SENSOR_GPS)) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { isFirstGPSUpdate = true; return; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 0896f412de6..5c0a1a73143 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -76,7 +76,7 @@ uint16_t frskyGetGPSState(void) tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; // thousands column (GPS fix status) - if (STATE(GPS_FIX)) + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) tmpi += 1000; if (STATE(GPS_FIX_HOME)) tmpi += 2000; diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 0bc65d3457d..6d44ea7cf56 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -181,7 +181,7 @@ static void sendGpsAltitude(void) { uint16_t altitude = gpsSol.llh.alt / 100; // meters //Send real GPS altitude only if it's reliable (there's a GPS fix) - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { altitude = 0; } sendDataHead(ID_GPS_ALTIDUTE_BP); @@ -221,7 +221,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) static void sendSpeed(void) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { return; } //Speed should be sent in knots (GPS speed is in cm/s) @@ -234,7 +234,7 @@ static void sendSpeed(void) static void sendHomeDistance(void) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { return; } sendDataHead(ID_HOME_DIST); diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index 85f6f85b962..a9630efa317 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,7 +147,7 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { gpsFlags |= GPS_FLAGS_FIX; } if (STATE(GPS_FIX_HOME)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 9e1f9f36226..92a84c15a06 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,7 +234,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 7fda28cd08b..a7c3776e4ee 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,7 @@ static void sendSMS(void) memset(pluscode_url, 0, sizeof(pluscode_url)); - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 766dea19010..3b4e0277413 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -346,7 +346,7 @@ static bool smartPortShouldSendGPSData(void) // or the craft has never been armed yet. This way if GPS stops working // while in flight, the user will easily notice because the sensor will stop // updating. - return feature(FEATURE_GPS) && (STATE(GPS_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); + return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); } void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 8e01e47ef8e..d8a8c15c33a 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -303,7 +303,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; - if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { return false; } @@ -371,7 +371,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; - if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { return false; } From 00b50852fc3a52b02de01df5e93972571a4c54a2 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 21 Aug 2022 23:32:13 +0300 Subject: [PATCH 02/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4cf9f010837..cfe943fda5f 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -6,7 +6,7 @@ Video demonstration There is possibility to allow plane to estimate it's position when GPS fix is lost. The main purpose is RTH without GPS. -It works on fixed wing only. +It works for fixed wing only. Plane should have the following sensors: - acceleromenter, gyroscope @@ -59,7 +59,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 m/s ```set fw_reference_airspeed=2777``` -*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Icrease cruise speed with throttle" - do not use it without GPS Fix.* +*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.* *If pitot is available, pitot sensor data will be used instead of constant.* From ccfa58eec6d318cba2e3cafca97fd6af7ba64905 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 00:35:36 +0300 Subject: [PATCH 03/61] included fixed USER3 BOX ID collision --- src/main/fc/fc_msp_box.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 3d5459e9d9b..7fb1cd9be0f 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -87,14 +87,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 }, { .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 }, { .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 }, - { .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 57 }, - { .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 50 }, - { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 51 }, - { .boxId = BOXTURTLE, .boxName = "TURTLE", .permanentId = 52 }, - { .boxId = BOXNAVCRUISE, .boxName = "NAV CRUISE", .permanentId = 53 }, - { .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 }, - { .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 }, - { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 }, + { .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 50 }, + { .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 51 }, + { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 52 }, + { .boxId = BOXTURTLE, .boxName = "TURTLE", .permanentId = 53 }, + { .boxId = BOXNAVCRUISE, .boxName = "NAV CRUISE", .permanentId = 54 }, + { .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 55 }, + { .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 56 }, + { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 57 }, { .boxId = BOXGPSOFF, .boxName = "GPS OFF", .permanentId = 58 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; From ffa28d89ffd3a213103f3c0811b5c4c119804501 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:01:45 +0300 Subject: [PATCH 04/61] updated documentation --- docs/Settings.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index ca0d7b90d8f..b28c5a1e25a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1842,6 +1842,16 @@ Defines if inav will dead-reckon over short GPS outages. May also be useful for --- +### inav_allow_gps_fix_estimation + +Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### inav_auto_mag_decl Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. From 7de16ffb3f27c59540b19dd0b8f6ebea9048a38a Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:10:20 +0300 Subject: [PATCH 05/61] retrigger checks From 2f6a1927d7135ee99db3b4dd9718ba791c9f38f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 20:10:08 +0300 Subject: [PATCH 06/61] retrigger checks From 9f0489e82da3c94a539787432b325065720b3153 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:19:15 +0300 Subject: [PATCH 07/61] retrigger checks From e4ff863bdc8ca7cbb85c2589f999c3e1ae9755cc Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:31:27 +0300 Subject: [PATCH 08/61] fixed rename getAirspeedEstimate() --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 23fd9254a8e..abc44c601fa 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -215,7 +215,7 @@ void updateEstimatedGPSFix(void) { #ifdef USE_PITOT if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - speed = pitotCalculateAirSpeed(); + speed = getAirspeedEstimate(); } #endif From ccad410979abc887b3c0ecfe6ac78c53dcdfc439 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:41:16 +0300 Subject: [PATCH 09/61] retrigger checks From bd5a92cf005bb72b0ec83ca35623f9ebcaa61bcd Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:41:31 +0300 Subject: [PATCH 10/61] retrigger checks From 2b83b16a89d04328e933ee8fb9c87c18f8b2d72d Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 26 Sep 2022 18:15:45 +0300 Subject: [PATCH 11/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index cfe943fda5f..60ea7818e82 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -50,12 +50,12 @@ Also you have to specify criuse airspeed of the plane. The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. -Cruise airspeed is specified in m/s. +Cruise airspeed is specified in cm/s. To convert km/h to m/s, multiply by 27.77. -Example: 100 km/h = 100 * 27.77 = 2777 m/s +Example: 100 km/h = 100 * 27.77 = 2777 cm/s ```set fw_reference_airspeed=2777``` From a439e2925cb710e98fa30850ccc0c5dd082deeff Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 10 Dec 2022 19:52:18 +0200 Subject: [PATCH 12/61] adjusted docs --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index baa3338c368..d34e684d345 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -30,9 +30,9 @@ Without GPS fix, plane has nose heading from magnetometer only. To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing -- plane is flying with constant speed, specified in settings +- (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using these assumptions. To increase heading accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase speed estimation accuracy, plane will use pitot tube (if available). +It is posible to roughtly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. From 3ade7e218dcc63a9930f6cc20a621dda7273fb46 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 28 Dec 2022 03:07:26 +0200 Subject: [PATCH 13/61] fixed typo in comment --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6eaf8cec538..5fa68da903a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2385,7 +2385,7 @@ bool validateRTHSanityChecker(void) //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; - //schedule check 5 seconds after apperange of real GPS fix, when position estimation coords stabilise after jump + //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; return true; } From 01ada270cd0c015fbdfe6af38a0bc351ae580455 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 28 Dec 2022 03:25:00 +0200 Subject: [PATCH 14/61] added failsafe_gps_fix_estimation_delay --- docs/Settings.md | 12 +++++++++++- src/main/fc/settings.yaml | 7 ++++++- src/main/flight/failsafe.c | 13 +++++++++++++ src/main/flight/failsafe.h | 2 ++ 4 files changed, 32 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d1d339bfd6f..46f0f8ea43b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -852,6 +852,16 @@ Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active --- +### failsafe_gps_fix_estimation_delay + +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. + +| Default | Min | Max | +| --- | --- | --- | +| 7 | -1 | 600 | + +--- + ### failsafe_lights Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. @@ -1714,7 +1724,7 @@ Defines if INAV will dead-reckon over short GPS outages. May also be useful for ### inav_allow_gps_fix_estimation -Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS. +Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15b05bf3c6d..92f0b109ebc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -840,6 +840,11 @@ groups: default_value: 0 min: -1 max: 600 + - name: failsafe_gps_fix_estimation_delay + description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." + default_value: 7 + min: -1 + max: 600 - name: PG_LIGHTS_CONFIG type: lightsConfig_t @@ -2189,7 +2194,7 @@ groups: field: allow_dead_reckoning type: bool - name: inav_allow_gps_fix_estimation - description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS." + description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." default_value: OFF field: allow_gps_fix_estimation type: bool diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 79ea091af3d..646cf92b3ec 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -82,6 +82,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) + .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied ); typedef enum { @@ -403,6 +404,18 @@ void failsafeUpdateState(void) } reprocessState = true; } + else { + if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { + if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); + } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + } + } else + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7cd88acd591..0cbeffb8198 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,6 +43,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) + int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -149,6 +150,7 @@ typedef struct failsafeState_s { timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time + timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation failsafeProcedure_e activeProcedure; failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; From dfd9dff9e265dd33b45e61e6c83d443254ff2fa8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 30 Dec 2022 01:36:46 +0200 Subject: [PATCH 15/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index d34e684d345..1972245e75b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ GPS Fix estimation is enabled with CLI command: Also you have to specify criuse airspeed of the plane. -The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. +To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. Cruise airspeed is specified in cm/s. @@ -77,6 +77,12 @@ For testing purpoces, it is possible to disable GPS sensor fix from RC controlle *GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* +# Allowing wp missions with GPS Fix estimation + +```failsafe_gps_fix_estimation_delay``` + +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. + # Is it possible to implement this for multirotor ? There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing. From bf3a85700bc28fec2ade9b0a15405850f39cde17 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 15 Feb 2023 16:42:48 +0200 Subject: [PATCH 16/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 1972245e75b..f93b6fec3fc 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -63,7 +63,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s *If pitot is available, pitot sensor data will be used instead of constant.* -*Note related command: to continue mission without RC signal, see command ```set failsafe_mission=OFF```.* +*Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.* **After entering CLI command, make sure that settings are saved:** From dc7119474354a8ddc103d63816d6d8f3ac738878 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Feb 2023 13:39:26 +0100 Subject: [PATCH 17/61] show pitot failure status on osd --- src/main/io/osd.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1dd3ed4be82..c75c0c97051 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2678,12 +2678,20 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_SPEED: { #ifdef USE_PITOT - const float airspeed_estimate = getAirspeedEstimate(); buff[0] = SYM_AIR; - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); - if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || - (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + if (pitotIsHealthy()) + { + const float airspeed_estimate = getAirspeedEstimate(); + osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || + (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + else + { + strcpy(buff + 1, " X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } #else From 7591ed550a3bfc58a8c4bdf653527e26e9074031 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Feb 2023 00:03:01 +0100 Subject: [PATCH 18/61] enable possibility to simulate GPS sensor timeout, mag failure and pitot failure with HITL --- src/main/fc/fc_msp.c | 6 +++++- src/main/fc/runtime_config.h | 5 ++++- src/main/io/gps.c | 19 +++++++++++++++---- src/main/sensors/diagnostics.c | 9 +++++++-- src/main/sensors/pitotmeter.c | 9 ++++++++- 5 files changed, 39 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ee5c8f03976..a5a634b4fa6 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3545,7 +3545,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (sensors(SENSOR_MAG)) { mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); @@ -3560,6 +3560,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); } + + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; + } } else { DISABLE_STATE(GPS_FIX); } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 779935b1277..522fcf73b6e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -181,7 +181,10 @@ typedef enum { HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) HITL_HAS_NEW_GPS_DATA = (1 << 4), HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 - HITL_AIRSPEED = (1 << 6) + HITL_AIRSPEED = (1 << 6), + HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 + HITL_GPS_TIMEOUT = (1 << 8), + HITL_PITOT_FAILURE = (1 << 9) } simulatorFlags_t; typedef struct { diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4976b797bea..0696ae0b62d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -474,10 +474,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE)) { - gpsUpdateTime(); - gpsSetState(GPS_RUNNING); - sensorsSet(SENSOR_GPS); - return gpsSol.flags.hasNewData; + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) + { + gpsSetState(GPS_LOST_COMMUNICATION); + sensorsClear(SENSOR_GPS); + gpsStats.timeouts = 5; + return false; + } + else + { + gpsSetState(GPS_RUNNING); + sensorsSet(SENSOR_GPS); + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; + } } #endif #ifdef USE_FAKE_GPS diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 0702f78991e..d0ae8324fb2 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -62,12 +62,17 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void) hardwareSensorStatus_e getHwCompassStatus(void) { +#if defined(USE_MAG) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { - return HW_SENSOR_OK; + if (compassIsHealthy()) { + return HW_SENSOR_OK; + } + else { + return HW_SENSOR_UNHEALTHY; + } } #endif -#if defined(USE_MAG) if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index eb2cad13f03..7c8fb7ad29b 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -193,6 +193,13 @@ STATIC_PROTOTHREAD(pitotThread) pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); while(1) { +#ifdef USE_SIMULATOR + while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) + { + ptDelayUs(10000); + } +#endif + // Start measurement if (pitot.dev.start(&pitot.dev)) { pitot.lastSeenHealthyMs = millis(); @@ -236,7 +243,7 @@ STATIC_PROTOTHREAD(pitotThread) } #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; + pitot.airSpeed = simulatorData.airSpeed; } #endif } From e6027076f994e32d5feaed449fcb8c1a53685831 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 5 Mar 2023 23:21:13 +0100 Subject: [PATCH 19/61] fix GPS timeout display on OSD --- src/main/io/osd.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c75c0c97051..850701573f0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1707,9 +1707,11 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_ESTIMATED_FIX)) { if (!STATE(GPS_FIX)) { strcpy(buff + 2, "ES"); - } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); - } + } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; From ce9cbd1f5d4ac94253719a9e8dcdfce41e9db5a0 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 5 Mar 2023 23:21:51 +0100 Subject: [PATCH 20/61] estimate GPS Fix only if compass and barometer are healthy --- src/main/io/gps.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0696ae0b62d..d26bb691a8b 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,7 +152,11 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { - return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && sensors(SENSOR_MAG) && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && + sensors(SENSOR_GPS) && + sensors(SENSOR_BARO) && baroIsHealthy() && + sensors(SENSOR_MAG) && compassIsHealthy() && + ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); } void updateEstimatedGPSFix(void) { From 12cb0c57571ea48a794a2aab89fb6b5a3e507267 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 17:56:36 +0100 Subject: [PATCH 21/61] support GPS Fix estimation on sensor timeout --- src/main/flight/failsafe.c | 2 +- src/main/flight/imu.c | 1 + src/main/io/gps.c | 67 +++++++++++++++---- .../navigation/navigation_pos_estimator.c | 6 +- src/main/telemetry/frsky_d.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ibus_shared.c | 32 ++++----- src/main/telemetry/jetiexbus.c | 2 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 4 +- src/main/telemetry/sim.c | 2 +- 11 files changed, 81 insertions(+), 41 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index d96730f78ab..35c10dc3153 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -355,7 +355,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { + ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 643773cebcc..5dbe717c18d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -321,6 +321,7 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c #endif } +//NOTE: checks if real GPS data is present, ignores any available GPS Fix estimation bool isGPSTrustworthy(void) { return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index d26bb691a8b..d63313099c6 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,23 +152,16 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { + //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: + //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once + //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && - sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); } -void updateEstimatedGPSFix(void) { - - static uint32_t lastUpdateMs = 0; - static int32_t estimated_lat = 0; - static int32_t estimated_lon = 0; - static int32_t estimated_alt = 0; - - uint32_t t = millis(); - int32_t dt = t - lastUpdateMs; - lastUpdateMs = t; +void processDisableGPSFix(void) { static int32_t last_lat = 0; static int32_t last_lon = 0; @@ -180,6 +173,11 @@ void updateEstimatedGPSFix(void) { gpsSol.hdop = 9999; gpsSol.numSat = 0; + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + gpsSol.flags.validTime = false; + //freeze coordinates gpsSol.llh.lat = last_lat; gpsSol.llh.lon = last_lon; @@ -193,6 +191,18 @@ void updateEstimatedGPSFix(void) { last_lon = gpsSol.llh.lon; last_alt = gpsSol.llh.alt; } +} + +void updateEstimatedGPSFix(void) { + + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; if (STATE(GPS_FIX) || !canEstimateGPSFix()) { DISABLE_STATE(GPS_ESTIMATED_FIX); @@ -212,9 +222,10 @@ void updateEstimatedGPSFix(void) { gpsSol.eph = 100; gpsSol.epv = 100; - gpsSol.flags.validVelNE = 1; - gpsSol.flags.validVelD = 0; //do not provide velocity.z - gpsSol.flags.validEPE = 1; + gpsSol.flags.validVelNE = true; + gpsSol.flags.validVelD = false; //do not provide velocity.z + gpsSol.flags.validEPE = true; + gpsSol.flags.validTime = false; float speed = pidProfile()->fixedWingReferenceAirspeed; @@ -281,6 +292,8 @@ void gpsProcessNewSolutionData(void) // Set sensor as ready and available sensorsSet(SENSOR_GPS); + processDisableGPSFix(); + updateEstimatedGPSFix(); // Pass on GPS update to NAV and IMU @@ -298,6 +311,8 @@ void gpsProcessNewSolutionData(void) // Toggle heartbeat gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + + debug[0]+=1; } static void gpsResetSolution(void) @@ -316,6 +331,23 @@ static void gpsResetSolution(void) gpsSol.flags.validTime = false; } +void gpsTryEstimateOnTimeout(void) +{ + gpsResetSolution(); + DISABLE_STATE(GPS_FIX); + + processDisableGPSFix(); + + updateEstimatedGPSFix(); + + if (STATE(GPS_ESTIMATED_FIX)) + { + onNewGPSData(); + gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + } + debug[1]+=1; +} + void gpsPreInit(void) { // Make sure gpsProvider is known when gpsMagDetect is called @@ -483,6 +515,7 @@ bool gpsUpdate(void) gpsSetState(GPS_LOST_COMMUNICATION); sensorsClear(SENSOR_GPS); gpsStats.timeouts = 5; + gpsTryEstimateOnTimeout(); return false; } else @@ -542,6 +575,11 @@ bool gpsUpdate(void) break; } + if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) + { + gpsTryEstimateOnTimeout(); + } + return gpsSol.flags.hasNewData; #endif } @@ -588,6 +626,7 @@ bool isGPSHealthy(void) return true; } +//NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 6363fc2a0f8..91cd96a85b2 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -210,7 +210,7 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { isFirstGPSUpdate = true; return; @@ -486,7 +486,7 @@ static bool navIsAccelerationUsable(void) static bool navIsHeadingUsable(void) { - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { // If we have GPS - we need true IMU north (valid heading) return isImuHeadingValid(); } @@ -501,7 +501,7 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; - if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid && + if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index e55aa0631a0..73e8e9801e8 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -540,7 +540,7 @@ void handleFrSkyTelemetry(void) } #ifdef USE_GPS - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { sendSpeed(); sendHomeDistance(); sendGpsAltitude(); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 92a84c15a06..a9810eb8f6d 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -476,7 +476,7 @@ static bool processBinaryModeRequest(uint8_t address) switch (address) { #ifdef USE_GPS case 0x8A: - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { hottPrepareGPSResponse(&hottGPSMessage); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); return true; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1e598dd2a76..a8d0cd82d0f 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -135,7 +135,7 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { #if defined(USE_GPS) uint8_t fix = 0; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (gpsSol.fixType == GPS_NO_FIX) fix = 1; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; @@ -195,7 +195,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { status += gpsSol.numSat * 1000; status += fix * 100; if (STATE(GPS_FIX_HOME)) status += 500; @@ -205,72 +205,72 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { return sendIbusMeasurement2(address, status); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t #endif return sendIbusMeasurement2(address, 0); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index c9b68fa27f7..53de96cc5d4 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -575,7 +575,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); if (!allSensorsActive) { - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { enableGpsTelemetry(true); allSensorsActive = true; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 282c9be4fd2..220623fc2c4 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -117,7 +117,7 @@ void ltm_gframe(sbuf_t *dst) uint8_t gps_fix_type = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (gpsSol.fixType == GPS_NO_FIX) gps_fix_type = 1; else if (gpsSol.fixType == GPS_FIX_2D) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 58a96a92d45..8be4aa72b21 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -523,7 +523,7 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; - if (!sensors(SENSOR_GPS)) + if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) return; if (gpsSol.fixType == GPS_NO_FIX) @@ -638,7 +638,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (sensors(SENSOR_GPS)) { + if (!( sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 85f1e740d6b..9be24ed7678 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,7 @@ static void sendSMS(void) ZERO_FARRAY(pluscode_url); - if (sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; From a62bc745ff6c28a5584eaedc2e5b95852a0be6f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 19:03:41 +0200 Subject: [PATCH 22/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index f93b6fec3fc..4f43eed353d 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -21,6 +21,8 @@ GPS fix estimation allows to recover plane using magnetometer and baromener only Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +GPS Fix is also estimated on GPS Sensor timeout (harware failure). + # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. @@ -40,6 +42,7 @@ It is assumed, that plane will fly in roughtly estimated direction to home posit *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. + # Settings GPS Fix estimation is enabled with CLI command: @@ -75,7 +78,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s For testing purpoces, it is possible to disable GPS sensor fix from RC controller in programming tab: -*GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* +*GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.* # Allowing wp missions with GPS Fix estimation From 6e7dafb96d12889af47226c4f578585b88a517cc Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 18:37:40 +0100 Subject: [PATCH 23/61] fixed compilation error --- src/main/io/gps.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index d63313099c6..71b9e964187 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,6 +152,8 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { +#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) + //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix @@ -159,6 +161,11 @@ bool canEstimateGPSFix(void) sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); + +#else + return false; +#endif + } void processDisableGPSFix(void) { From 7afaa0db34dd12132cf57f3e8ccf511b41ac44db Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 20:18:11 +0100 Subject: [PATCH 24/61] fixed indentation --- src/main/flight/failsafe.h | 2 +- src/main/flight/wind_estimator.c | 5 +- src/main/io/gps.c | 203 ++++++++++----------- src/main/io/osd.c | 6 +- src/main/navigation/navigation.c | 16 +- src/main/navigation/navigation_fixedwing.c | 6 +- 6 files changed, 115 insertions(+), 123 deletions(-) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 0cbeffb8198..d80ab650c78 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,7 +43,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) - int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) + int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index e4af1828190..30f15fac480 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -83,8 +83,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) static float lastValidEstimateAltitude = 0.0f; float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m - if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) - { + if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) { hasValidWindEstimate = false; } @@ -92,7 +91,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) !isGPSHeadingValid() || !gpsSol.flags.validVelNE || !gpsSol.flags.validVelD || - STATE(GPS_ESTIMATED_FIX)){ + STATE(GPS_ESTIMATED_FIX)) { return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2f954e4ba93..28ef5496517 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -163,121 +163,116 @@ bool canEstimateGPSFix(void) ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); #else - return false; + return false; #endif } -void processDisableGPSFix(void) { - - static int32_t last_lat = 0; - static int32_t last_lon = 0; - static int32_t last_alt = 0; +void processDisableGPSFix(void) +{ + static int32_t last_lat = 0; + static int32_t last_lon = 0; + static int32_t last_alt = 0; - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) - { - gpsSol.fixType = GPS_NO_FIX; - gpsSol.hdop = 9999; - gpsSol.numSat = 0; + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validEPE = false; + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; gpsSol.flags.validTime = false; - //freeze coordinates - gpsSol.llh.lat = last_lat; - gpsSol.llh.lon = last_lon; - gpsSol.llh.alt = last_alt; - - DISABLE_STATE(GPS_FIX); - } - else - { - last_lat = gpsSol.llh.lat; - last_lon = gpsSol.llh.lon; - last_alt = gpsSol.llh.alt; - } + //freeze coordinates + gpsSol.llh.lat = last_lat; + gpsSol.llh.lon = last_lon; + gpsSol.llh.alt = last_alt; + + DISABLE_STATE(GPS_FIX); + } else { + last_lat = gpsSol.llh.lat; + last_lon = gpsSol.llh.lon; + last_alt = gpsSol.llh.alt; + } } -void updateEstimatedGPSFix(void) { - - static uint32_t lastUpdateMs = 0; - static int32_t estimated_lat = 0; - static int32_t estimated_lon = 0; - static int32_t estimated_alt = 0; - - uint32_t t = millis(); - int32_t dt = t - lastUpdateMs; - lastUpdateMs = t; - - if (STATE(GPS_FIX) || !canEstimateGPSFix()) { - DISABLE_STATE(GPS_ESTIMATED_FIX); - estimated_lat = gpsSol.llh.lat; - estimated_lon = gpsSol.llh.lon; - estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; - return; - } - - ENABLE_STATE(GPS_ESTIMATED_FIX); - - gpsSol.fixType = GPS_FIX_3D; - gpsSol.hdop = 99; - gpsSol.flags.hasNewData = true; - gpsSol.numSat = 99; - - gpsSol.eph = 100; - gpsSol.epv = 100; - - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = false; //do not provide velocity.z - gpsSol.flags.validEPE = true; +void updateEstimatedGPSFix(void) +{ + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; + + if (STATE(GPS_FIX) || !canEstimateGPSFix()) { + DISABLE_STATE(GPS_ESTIMATED_FIX); + estimated_lat = gpsSol.llh.lat; + estimated_lon = gpsSol.llh.lon; + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + return; + } + + ENABLE_STATE(GPS_ESTIMATED_FIX); + + gpsSol.fixType = GPS_FIX_3D; + gpsSol.hdop = 99; + gpsSol.flags.hasNewData = true; + gpsSol.numSat = 99; + + gpsSol.eph = 100; + gpsSol.epv = 100; + + gpsSol.flags.validVelNE = true; + gpsSol.flags.validVelD = false; //do not provide velocity.z + gpsSol.flags.validEPE = true; gpsSol.flags.validTime = false; - float speed = pidProfile()->fixedWingReferenceAirspeed; + float speed = pidProfile()->fixedWingReferenceAirspeed; #ifdef USE_PITOT - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) - { - speed = getAirspeedEstimate(); - } + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + speed = getAirspeedEstimate(); + } #endif - float velX = rMat[0][0] * speed; - float velY = -rMat[1][0] * speed; - // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame - - if (isEstimatedWindSpeedValid()) { - velX += getEstimatedWindSpeed(X); - velY += getEstimatedWindSpeed(Y); - } - // here (velX, velY) is estimated horizontal speed with wind influence = ground speed - - if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) - { - velX = 0; - velY = 0; - } - - estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); - estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); - estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; - - gpsSol.llh.lat = estimated_lat; - gpsSol.llh.lon = estimated_lon; - gpsSol.llh.alt = estimated_alt; - - gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); - - float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction - if (groundCourse < 0) { - groundCourse += 2 * M_PIf; - } - gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); - - gpsSol.velNED[X] = (int16_t)(velX); - gpsSol.velNED[Y] = (int16_t)(velY); - gpsSol.velNED[Z] = 0; + float velX = rMat[0][0] * speed; + float velY = -rMat[1][0] * speed; + // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame + + if (isEstimatedWindSpeedValid()) { + velX += getEstimatedWindSpeed(X); + velY += getEstimatedWindSpeed(Y); + } + // here (velX, velY) is estimated horizontal speed with wind influence = ground speed + + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) { + velX = 0; + velY = 0; + } + + estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); + estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + + gpsSol.llh.lat = estimated_lat; + gpsSol.llh.lon = estimated_lon; + gpsSol.llh.alt = estimated_alt; + + gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); + + float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction + if (groundCourse < 0) { + groundCourse += 2 * M_PIf; + } + gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); + + gpsSol.velNED[X] = (int16_t)(velX); + gpsSol.velNED[Y] = (int16_t)(velY); + gpsSol.velNED[Z] = 0; } @@ -301,7 +296,7 @@ void gpsProcessNewSolutionData(void) processDisableGPSFix(); - updateEstimatedGPSFix(); + updateEstimatedGPSFix(); // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -345,10 +340,9 @@ void gpsTryEstimateOnTimeout(void) processDisableGPSFix(); - updateEstimatedGPSFix(); + updateEstimatedGPSFix(); - if (STATE(GPS_ESTIMATED_FIX)) - { + if (STATE(GPS_ESTIMATED_FIX)) { onNewGPSData(); gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } @@ -582,8 +576,7 @@ bool gpsUpdate(void) break; } - if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) - { + if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { gpsTryEstimateOnTimeout(); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 850701573f0..b1081a92a2b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1705,9 +1705,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (STATE(GPS_ESTIMATED_FIX)) { - if (!STATE(GPS_FIX)) { - strcpy(buff + 2, "ES"); - } + if (!STATE(GPS_FIX)) { + strcpy(buff + 2, "ES"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 58dda7f8086..40cc7989c71 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2376,14 +2376,14 @@ bool validateRTHSanityChecker(void) return true; } - if (STATE(GPS_ESTIMATED_FIX)) { - //disable sanity checks in GPS estimation mode - //when estimated GPS fix is replaced with real fix, coordinates may jump - posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; - //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump - posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; - return true; - } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; + //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + return true; + } // Check at 10Hz rate if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 34ca67d737c..09681047a0e 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -319,10 +319,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // turn direction to next waypoint loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = true; - } + needToCalculateCircularLoiter = true; + } posControl.flags.wpTurnSmoothingActive = true; - } + } } // We are closing in on a waypoint, calculate circular loiter if required From 69a122099226ac1287474b8784bd2c5ecdd3d34d Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 20:30:28 +0100 Subject: [PATCH 25/61] fixed indentation --- src/main/fc/fc_msp.c | 56 +++++++++++----------- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/sensors/diagnostics.c | 14 ++---- 3 files changed, 34 insertions(+), 38 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b2ebac8343f..04e55b4f503 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3222,8 +3222,8 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) #ifdef USE_SIMULATOR bool isOSDTypeSupportedBySimulator(void) { - displayPort_t *osdDisplayPort = osdGetDisplayPort(); - return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16)); + displayPort_t *osdDisplayPort = osdGetDisplayPort(); + return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16)); } void mspWriteSimulatorOSD(sbuf_t *dst) @@ -3434,51 +3434,51 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: - tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version + tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version // Check the MSP version of simulator - if (tmp_u8 != SIMULATOR_MSP_VERSION) { + if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; } - simulatorData.flags = sbufReadU8(src); + simulatorData.flags = sbufReadU8(src); if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { - if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE); + if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE); #ifdef USE_BARO - baroStartCalibration(); + baroStartCalibration(); #endif #ifdef USE_MAG - DISABLE_STATE(COMPASS_CALIBRATED); - compassInit(); + DISABLE_STATE(COMPASS_CALIBRATED); + compassInit(); #endif - simulatorData.flags = HITL_RESET_FLAGS; + simulatorData.flags = HITL_RESET_FLAGS; // Review: Many states were affected. Reboot? - disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } - } else if (!areSensorsCalibrating()) { - if (!ARMING_FLAG(SIMULATOR_MODE)) { // Just once + disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! + } + } else if (!areSensorsCalibrating()) { + if (!ARMING_FLAG(SIMULATOR_MODE)) { // Just once #ifdef USE_BARO - baroStartCalibration(); -#endif + baroStartCalibration(); +#endif #ifdef USE_MAG - if (compassConfig()->mag_hardware != MAG_NONE) { - sensorsSet(SENSOR_MAG); - ENABLE_STATE(COMPASS_CALIBRATED); - DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - mag.magADC[X] = 800; - mag.magADC[Y] = 0; - mag.magADC[Z] = 0; - } + if (compassConfig()->mag_hardware != MAG_NONE) { + sensorsSet(SENSOR_MAG); + ENABLE_STATE(COMPASS_CALIBRATED); + DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); + mag.magADC[X] = 800; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; + } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE); - LOG_DEBUG(SYSTEM, "Simulator enabled"); - } + ENABLE_ARMING_FLAG(SIMULATOR_MODE); + LOG_DEBUG(SYSTEM, "Simulator enabled"); + } if (dataSize >= 14) { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 09681047a0e..ca6e56bae25 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -406,7 +406,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // courseVirtualCorrection initially used to determine current position relative to course line for later use + // courseVirtualCorrection initially used to determine current position relative to course line for later use int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 351d5c6b5f8..583f852224b 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -64,11 +64,10 @@ hardwareSensorStatus_e getHwCompassStatus(void) { #if defined(USE_MAG) #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { + if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { if (compassIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -76,17 +75,14 @@ hardwareSensorStatus_e getHwCompassStatus(void) if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } - } - else { + } else { if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } From f9e078f9c145ddd12ee36e1d8c40d9ef10379fff Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:44:38 +0100 Subject: [PATCH 26/61] fixed indentation --- src/main/drivers/display.c | 16 ++++++++-------- src/main/drivers/sound_beeper.c | 10 +++++----- src/main/navigation/navigation_pos_estimator.c | 12 ++++++------ 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 28bbd881338..f3802638adf 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -179,14 +179,14 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) } #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE)) { - //some FCs do not power max7456 from USB power - //driver can not read font metadata - //chip assumed to not support second bank of font - //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) - //return dummy metadata to let all OSD elements to work in simulator mode - instance->maxChar = 512; - } + if (ARMING_FLAG(SIMULATOR_MODE)) { + //some FCs do not power max7456 from USB power + //driver can not read font metadata + //chip assumed to not support second bank of font + //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) + //return dummy metadata to let all OSD elements to work in simulator mode + instance->maxChar = 512; + } #endif if (c > instance->maxChar) { diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index bb089b4cb9e..4d7981b867f 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -47,11 +47,11 @@ void systemBeep(bool onoff) #else #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE)) { - if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { - onoff = false; - } - } + if (ARMING_FLAG(SIMULATOR_MODE)) { + if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { + onoff = false; + } + } #endif if (beeperConfig()->pwmMode) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 91cd96a85b2..4d5139479de 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -152,12 +152,12 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; - if (STATE(GPS_ESTIMATED_FIX)) { - //disable sanity checks in GPS estimation mode - //when estimated GPS fix is replaced with real fix, coordinates may jump - previousTime = 0; - return true; - } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + previousTime = 0; + return true; + } if (previousTime == 0) { isGlitching = false; From 4c39d77c45eb0df07a3c75ccd1e100c171652963 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:44:48 +0100 Subject: [PATCH 27/61] removed debug code --- src/main/io/gps.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 28ef5496517..34a5f915a77 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -157,7 +157,7 @@ bool canEstimateGPSFix(void) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix - return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); @@ -313,8 +313,6 @@ void gpsProcessNewSolutionData(void) // Toggle heartbeat gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; - - debug[0]+=1; } static void gpsResetSolution(void) @@ -346,7 +344,6 @@ void gpsTryEstimateOnTimeout(void) onNewGPSData(); gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } - debug[1]+=1; } void gpsPreInit(void) From 2ce2904d5f798dc0e385f6f11245420dc7c62b13 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 23:55:14 +0100 Subject: [PATCH 28/61] attempt to fix failsafe_gps_fix_estimation_delay bug --- src/main/flight/failsafe.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 35c10dc3153..8bf654a2f52 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -408,18 +408,19 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } - else { + } else { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { - failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); + if ( !FLIGHT_MODE(FAILSAFE_MODE) ) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + } } - } else - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } else + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) From a10b7ceaad3ab246a1114cfd1d704b5bfe813d96 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 08:29:54 +0100 Subject: [PATCH 29/61] enable ground course estimation --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4d5139479de..0f9e0582df3 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -698,7 +698,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { - if (STATE(GPS_FIX) && navIsHeadingUsable()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { static timeUs_t lastUpdateTimeUs = 0; if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate From 20cdfc8935173c3ea74ee79fb666ca5664701252 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 16:04:36 +0100 Subject: [PATCH 30/61] fixed landing detection with GPS fix estimation --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ca6e56bae25..691467e3210 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -715,7 +715,7 @@ bool isFixedWingLandingDetected(void) // Check horizontal and vertical velocities are low (cm/s) bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) && - ( posControl.actualState.velXY < (100.0f * sensitivity) || STATE(GPS_ESTIMATED_FIX)); + ( posControl.actualState.velXY < (100.0f * sensitivity)); // Check angular rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity); DEBUG_SET(DEBUG_LANDING, 2, velCondition); From ec5e0c8a73a96f4ba523568fbdb992d815f43442 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 23:53:12 +0100 Subject: [PATCH 31/61] fixed: forced RTH is not activated on GPS Fix estimation event during mission with RX loss --- src/main/flight/failsafe.c | 46 +++++++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 8bf654a2f52..e2cd71cc9d7 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -368,6 +368,27 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) return failsafeConfig()->failsafe_procedure; } + +bool checkGPSFixFailsafe(void) +{ + if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { + if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); + } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { + if ( !posControl.flags.forcedRTHActivated ) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + return true; + } + } + } else { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } + return false; +} + + void failsafeUpdateState(void) { if (!failsafeIsMonitoring() || failsafeIsSuspended()) { @@ -396,7 +417,10 @@ void failsafeUpdateState(void) if (!throttleStickIsLow()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } - if (!receivingRxDataAndNotFailsafeMode) { + + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } else if (!receivingRxDataAndNotFailsafeMode) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero @@ -408,20 +432,7 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } else { - if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { - if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); - } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { - if ( !FLIGHT_MODE(FAILSAFE_MODE) ) { - failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); - } - } - } else - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; - } + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { @@ -477,7 +488,12 @@ void failsafeUpdateState(void) } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; reprocessState = true; + } else { + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } } + break; case FAILSAFE_RETURN_TO_HOME: From 44fea0ece8a3ca1054e17c9d505f381f4d91b8f7 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 5 Apr 2023 13:02:06 +0200 Subject: [PATCH 32/61] fixed merge conflict --- src/main/io/osd.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e83f88a8291..bbf02b359d1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2916,7 +2916,6 @@ static bool osdDrawSingleElement(uint8_t item) bool moreThanAh = false; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { uint8_t digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values if (isBfCompatibleVideoSystem(osdConfig())) { @@ -2924,8 +2923,8 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); From cc3991298471103e380ab8cabd3f5152e25baaa6 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 25 Apr 2023 11:56:15 +0200 Subject: [PATCH 33/61] fixed: groundspeed is not sent in mavlink telemetry --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 892c1d3b71c..37a342b94f6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -638,7 +638,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (!( sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif From 9796bbbde55ee5705516c7d0857b7f870e042607 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 27 Apr 2023 14:46:59 +0200 Subject: [PATCH 34/61] use any wind estimator value with gps fix estimation --- src/main/flight/wind_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 30f15fac480..b6174611add 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -52,7 +52,7 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT]; bool isEstimatedWindSpeedValid(void) { - return hasValidWindEstimate; + return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. } float getEstimatedWindSpeed(int axis) From 55fdeae002c3f02bc87f5433ae030e5df4f84fa1 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 27 Apr 2023 15:56:11 +0200 Subject: [PATCH 35/61] improved course hold with GPS Fix estimation --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 283c0941bc8..e75193617ab 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -559,7 +559,7 @@ bool isGPSHealthy(void) //NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300; + return sensors(SENSOR_GPS) && ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; } #endif From 4d888e438b542eac3b0ac1206cd87e7894ca8618 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 18:15:09 +0200 Subject: [PATCH 36/61] fixed gpsSol access race conditions --- src/main/fc/fc_msp.c | 47 ++++++++-------- src/main/io/gps.c | 90 ++++++++++++++++++------------- src/main/io/gps.h | 2 - src/main/io/gps_fake.c | 52 +++++++++--------- src/main/io/gps_msp.c | 55 +++++++++---------- src/main/io/gps_nmea.c | 51 +++++++++--------- src/main/io/gps_private.h | 2 + src/main/io/gps_ublox.c | 109 +++++++++++++++++++------------------- 8 files changed, 216 insertions(+), 192 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e4b01fab966..432ad6e6a7a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3513,35 +3513,36 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (dataSize >= 14) { if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSol.fixType = sbufReadU8(src); - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.flags.hasNewData = true; - gpsSol.numSat = sbufReadU8(src); - - if (gpsSol.fixType != GPS_NO_FIX) { - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.validTime = false; - - gpsSol.llh.lat = sbufReadU32(src); - gpsSol.llh.lon = sbufReadU32(src); - gpsSol.llh.alt = sbufReadU32(src); - gpsSol.groundSpeed = (int16_t)sbufReadU16(src); - gpsSol.groundCourse = (int16_t)sbufReadU16(src); - - gpsSol.velNED[X] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Z] = (int16_t)sbufReadU16(src); - - gpsSol.eph = 100; - gpsSol.epv = 100; + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.flags.hasNewData = true; + gpsSolDRV.numSat = sbufReadU8(src); + + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; ENABLE_STATE(GPS_FIX); } else { sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } // Feed data to navigation + gpsProcessNewDriverData(); gpsProcessNewSolutionData(); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e75193617ab..10ade84f4df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -82,7 +82,13 @@ typedef struct { // GPS public data gpsReceiverData_t gpsState; gpsStatistics_t gpsStats; -gpsSolutionData_t gpsSol; + +//it is not safe to access gpsSolDRV which is filled in gps thread by driver. +//gpsSolDRV can be accessed only after driver signalled that data is ready +//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features +//and use it in the rest of code. +gpsSolutionData_t gpsSolDRV; //filled by driver +gpsSolutionData_t gpsSol; //used in the rest of the code // Map gpsBaudRate_e index to baudRate_e baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 }; @@ -195,8 +201,6 @@ void processDisableGPSFix(void) gpsSol.llh.lat = last_lat; gpsSol.llh.lon = last_lon; gpsSol.llh.alt = last_alt; - - DISABLE_STATE(GPS_FIX); } else { last_lat = gpsSol.llh.lat; last_lon = gpsSol.llh.lon; @@ -204,6 +208,7 @@ void processDisableGPSFix(void) } } +//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" void updateEstimatedGPSFix(void) { static uint32_t lastUpdateMs = 0; @@ -215,16 +220,15 @@ void updateEstimatedGPSFix(void) int32_t dt = t - lastUpdateMs; lastUpdateMs = t; - if (STATE(GPS_FIX) || !canEstimateGPSFix()) { - DISABLE_STATE(GPS_ESTIMATED_FIX); + bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats; + + if (sensorHasFix || !canEstimateGPSFix()) { estimated_lat = gpsSol.llh.lat; estimated_lon = gpsSol.llh.lon; estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; return; } - ENABLE_STATE(GPS_ESTIMATED_FIX); - gpsSol.fixType = GPS_FIX_3D; gpsSol.hdop = 99; gpsSol.flags.hasNewData = true; @@ -283,28 +287,42 @@ void updateEstimatedGPSFix(void) } +void gpsProcessNewDriverData(void) +{ + gpsSol = gpsSolDRV; + + processDisableGPSFix(); + updateEstimatedGPSFix(); +} +//called after: +//1)driver copies gpsSolDRV to gpsSol +//2)gpsSol is processed by "Disable GPS logical switch" +//3)gpsSol is processed by GPS Fix estimator void gpsProcessNewSolutionData(void) { - // Set GPS fix flag only if we have 3D fix - if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { - ENABLE_STATE(GPS_FIX); - } - else { - /* When no fix available - reset flags as well */ - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validEPE = false; + if ( gpsSol.numSat == 99 ) { + ENABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_FIX); + } else { + DISABLE_STATE(GPS_ESTIMATED_FIX); + + // Set GPS fix flag only if we have 3D fix + if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { + ENABLE_STATE(GPS_FIX); + } + else { + /* When no fix available - reset flags as well */ + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + DISABLE_STATE(GPS_FIX); + } } // Set sensor as ready and available sensorsSet(SENSOR_GPS); - processDisableGPSFix(); - - updateEstimatedGPSFix(); - // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -322,29 +340,27 @@ void gpsProcessNewSolutionData(void) gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } -static void gpsResetSolution(void) +static void gpsResetSolution(gpsSolutionData_t* gpsSol) { - gpsSol.eph = 9999; - gpsSol.epv = 9999; - gpsSol.numSat = 0; - gpsSol.hdop = 9999; + gpsSol->eph = 9999; + gpsSol->epv = 9999; + gpsSol->numSat = 0; + gpsSol->hdop = 9999; - gpsSol.fixType = GPS_NO_FIX; + gpsSol->fixType = GPS_NO_FIX; - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validMag = false; - gpsSol.flags.validEPE = false; - gpsSol.flags.validTime = false; + gpsSol->flags.validVelNE = false; + gpsSol->flags.validVelD = false; + gpsSol->flags.validEPE = false; + gpsSol->flags.validTime = false; } void gpsTryEstimateOnTimeout(void) { - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); - processDisableGPSFix(); - updateEstimatedGPSFix(); if (STATE(GPS_ESTIMATED_FIX)) { @@ -369,7 +385,8 @@ void gpsInit(void) gpsStats.timeouts = 0; // Reset solution, timeout and prepare to start - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); gpsSetProtocolTimeout(gpsState.baseTimeoutMs); gpsSetState(GPS_UNKNOWN); @@ -477,7 +494,8 @@ bool gpsUpdate(void) gpsSol.fixType = GPS_NO_FIX; // Reset solution - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); // Call GPS protocol reset handler gpsProviders[gpsState.gpsConfig->provider].restart(); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 92241c95602..a83662588c8 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -118,7 +118,6 @@ typedef struct gpsSolutionData_s { bool gpsHeartbeat; // Toggle each update bool validVelNE; bool validVelD; - bool validMag; bool validEPE; // EPH/EPV values are valid - actual accuracy bool validTime; } flags; @@ -127,7 +126,6 @@ typedef struct gpsSolutionData_s { uint8_t numSat; gpsLocation_t llh; - int16_t magData[3]; int16_t velNED[3]; int16_t groundSpeed; diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 6d574893436..30e8eeb94ab 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -62,37 +62,39 @@ void gpsFakeSet( int16_t velNED_Z, time_t time) { - gpsSol.fixType = fixType; - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.numSat = numSat; + gpsSolDRV.fixType = fixType; + gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = numSat; - gpsSol.llh.lat = lat; - gpsSol.llh.lon = lon; - gpsSol.llh.alt = alt; - gpsSol.groundSpeed = groundSpeed; - gpsSol.groundCourse = groundCourse; - gpsSol.velNED[X] = velNED_X; - gpsSol.velNED[Y] = velNED_Y; - gpsSol.velNED[Z] = velNED_Z; - gpsSol.eph = 100; - gpsSol.epv = 100; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.hasNewData = true; + gpsSolDRV.llh.lat = lat; + gpsSolDRV.llh.lon = lon; + gpsSolDRV.llh.alt = alt; + gpsSolDRV.groundSpeed = groundSpeed; + gpsSolDRV.groundCourse = groundCourse; + gpsSolDRV.velNED[X] = velNED_X; + gpsSolDRV.velNED[Y] = velNED_Y; + gpsSolDRV.velNED[Z] = velNED_Z; + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.hasNewData = true; if (time) { struct tm* gTime = gmtime(&time); - gpsSol.time.year = (uint16_t)(gTime->tm_year + 1900); - gpsSol.time.month = (uint16_t)(gTime->tm_mon + 1); - gpsSol.time.day = (uint8_t)gTime->tm_mday; - gpsSol.time.hours = (uint8_t)gTime->tm_hour; - gpsSol.time.minutes = (uint8_t)gTime->tm_min; - gpsSol.time.seconds = (uint8_t)gTime->tm_sec; - gpsSol.time.millis = 0; - gpsSol.flags.validTime = gpsSol.fixType >= 3; + gpsSolDRV.time.year = (uint16_t)(gTime->tm_year + 1900); + gpsSolDRV.time.month = (uint16_t)(gTime->tm_mon + 1); + gpsSolDRV.time.day = (uint8_t)gTime->tm_mday; + gpsSolDRV.time.hours = (uint8_t)gTime->tm_hour; + gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min; + gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec; + gpsSolDRV.time.millis = 0; + gpsSolDRV.flags.validTime = gpsSol.fixType >= 3; } + + gpsProcessNewDriverData(); } #endif diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index 8eafb2dff19..d4958117c92 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr) { const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; - gpsSol.fixType = gpsMapFixType(pkt->fixType); - gpsSol.numSat = pkt->satellitesInView; - gpsSol.llh.lon = pkt->longitude; - gpsSol.llh.lat = pkt->latitude; - gpsSol.llh.alt = pkt->mslAltitude; - gpsSol.velNED[X] = pkt->nedVelNorth; - gpsSol.velNED[Y] = pkt->nedVelEast; - gpsSol.velNED[Z] = pkt->nedVelDown; - gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); - gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 - gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); - gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(pkt->hdop); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - - gpsSol.time.year = pkt->year; - gpsSol.time.month = pkt->month; - gpsSol.time.day = pkt->day; - gpsSol.time.hours = pkt->hour; - gpsSol.time.minutes = pkt->min; - gpsSol.time.seconds = pkt->sec; - gpsSol.time.millis = 0; - - gpsSol.flags.validTime = (pkt->fixType >= 3); - + gpsSolDRV.fixType = gpsMapFixType(pkt->fixType); + gpsSolDRV.numSat = pkt->satellitesInView; + gpsSolDRV.llh.lon = pkt->longitude; + gpsSolDRV.llh.lat = pkt->latitude; + gpsSolDRV.llh.alt = pkt->mslAltitude; + gpsSolDRV.velNED[X] = pkt->nedVelNorth; + gpsSolDRV.velNED[Y] = pkt->nedVelEast; + gpsSolDRV.velNED[Z] = pkt->nedVelDown; + gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); + gpsSolDRV.groundCourse = pkt->groundCourse / 10; // in deg * 10 + gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + + gpsSolDRV.time.year = pkt->year; + gpsSolDRV.time.month = pkt->month; + gpsSolDRV.time.day = pkt->day; + gpsSolDRV.time.hours = pkt->hour; + gpsSolDRV.time.minutes = pkt->min; + gpsSolDRV.time.seconds = pkt->sec; + gpsSolDRV.time.millis = 0; + + gpsSolDRV.flags.validTime = (pkt->fixType >= 3); + + gpsProcessNewDriverData(); newDataReady = true; } #endif diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c index c5c3b420db7..7269b102fbd 100755 --- a/src/main/io/gps_nmea.c +++ b/src/main/io/gps_nmea.c @@ -200,45 +200,45 @@ static bool gpsNewFrameNMEA(char c) switch (gps_frame) { case FRAME_GGA: frameOK = 1; - gpsSol.numSat = gps_Msg.numSat; + gpsSolDRV.numSat = gps_Msg.numSat; if (gps_Msg.fix) { - gpsSol.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D + gpsSolDRV.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D - gpsSol.llh.lat = gps_Msg.latitude; - gpsSol.llh.lon = gps_Msg.longitude; - gpsSol.llh.alt = gps_Msg.altitude; + gpsSolDRV.llh.lat = gps_Msg.latitude; + gpsSolDRV.llh.lon = gps_Msg.longitude; + gpsSolDRV.llh.alt = gps_Msg.altitude; // EPH/EPV are unreliable for NMEA as they are not real accuracy - gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop); - gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.flags.validEPE = false; + gpsSolDRV.hdop = gpsConstrainHDOP(gps_Msg.hdop); + gpsSolDRV.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); + gpsSolDRV.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); + gpsSolDRV.flags.validEPE = false; } else { - gpsSol.fixType = GPS_NO_FIX; + gpsSolDRV.fixType = GPS_NO_FIX; } // NMEA does not report VELNED - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; + gpsSolDRV.flags.validVelNE = false; + gpsSolDRV.flags.validVelD = false; break; case FRAME_RMC: - gpsSol.groundSpeed = gps_Msg.speed; - gpsSol.groundCourse = gps_Msg.ground_course; + gpsSolDRV.groundSpeed = gps_Msg.speed; + gpsSolDRV.groundCourse = gps_Msg.ground_course; // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid if (gps_Msg.date != 0 && gps_Msg.time != 0) { - gpsSol.time.year = (gps_Msg.date % 100) + 2000; - gpsSol.time.month = (gps_Msg.date / 100) % 100; - gpsSol.time.day = (gps_Msg.date / 10000) % 100; - gpsSol.time.hours = (gps_Msg.time / 1000000) % 100; - gpsSol.time.minutes = (gps_Msg.time / 10000) % 100; - gpsSol.time.seconds = (gps_Msg.time / 100) % 100; - gpsSol.time.millis = (gps_Msg.time & 100) * 10; - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = (gps_Msg.date % 100) + 2000; + gpsSolDRV.time.month = (gps_Msg.date / 100) % 100; + gpsSolDRV.time.day = (gps_Msg.date / 10000) % 100; + gpsSolDRV.time.hours = (gps_Msg.time / 1000000) % 100; + gpsSolDRV.time.minutes = (gps_Msg.time / 10000) % 100; + gpsSolDRV.time.seconds = (gps_Msg.time / 100) % 100; + gpsSolDRV.time.millis = (gps_Msg.time & 100) * 10; + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } break; @@ -276,8 +276,9 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread) while (serialRxBytesWaiting(gpsState.gpsPort)) { uint8_t newChar = serialRead(gpsState.gpsPort); if (gpsNewFrameNMEA(newChar)) { - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; + gpsSolDRV.flags.validVelNE = false; + gpsSolDRV.flags.validVelD = false; + gpsProcessNewDriverData(); ptSemaphoreSignal(semNewDataReady); break; } diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 6e9abc72ffe..2a46c8670d2 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -57,6 +57,7 @@ typedef struct { } gpsReceiverData_t; extern gpsReceiverData_t gpsState; +extern gpsSolutionData_t gpsSolDRV; extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT]; @@ -66,6 +67,7 @@ extern void gpsFinalizeChangeBaud(void); extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainHDOP(uint32_t hdop); +void gpsProcessNewDriverData(void); void gpsProcessNewSolutionData(void); void gpsSetProtocolTimeout(timeMs_t timeoutMs); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 330cb2b75fc..aba2d930f92 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -604,84 +604,84 @@ static bool gpsParceFrameUBLOX(void) { switch (_msg_id) { case MSG_POSLLH: - gpsSol.llh.lon = _buffer.posllh.longitude; - gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm - gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); - gpsSol.flags.validEPE = true; + gpsSolDRV.llh.lon = _buffer.posllh.longitude; + gpsSolDRV.llh.lat = _buffer.posllh.latitude; + gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm + gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); + gpsSolDRV.flags.validEPE = true; if (next_fix_type != GPS_NO_FIX) - gpsSol.fixType = next_fix_type; + gpsSolDRV.fixType = next_fix_type; _new_position = true; break; case MSG_STATUS: next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; + gpsSolDRV.fixType = GPS_NO_FIX; break; case MSG_SOL: next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; - gpsSol.numSat = _buffer.solution.satellites; - gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); + gpsSolDRV.fixType = GPS_NO_FIX; + gpsSolDRV.numSat = _buffer.solution.satellites; + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); break; case MSG_VELNED: - gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.velNED[X] = _buffer.velned.ned_north; - gpsSol.velNED[Y] = _buffer.velned.ned_east; - gpsSol.velNED[Z] = _buffer.velned.ned_down; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; + gpsSolDRV.groundSpeed = _buffer.velned.speed_2d; // cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.velNED[X] = _buffer.velned.ned_north; + gpsSolDRV.velNED[Y] = _buffer.velned.ned_east; + gpsSolDRV.velNED[Z] = _buffer.velned.ned_down; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; _new_speed = true; break; case MSG_TIMEUTC: if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) { - gpsSol.time.year = _buffer.timeutc.year; - gpsSol.time.month = _buffer.timeutc.month; - gpsSol.time.day = _buffer.timeutc.day; - gpsSol.time.hours = _buffer.timeutc.hour; - gpsSol.time.minutes = _buffer.timeutc.min; - gpsSol.time.seconds = _buffer.timeutc.sec; - gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.timeutc.year; + gpsSolDRV.time.month = _buffer.timeutc.month; + gpsSolDRV.time.day = _buffer.timeutc.day; + gpsSolDRV.time.hours = _buffer.timeutc.hour; + gpsSolDRV.time.minutes = _buffer.timeutc.min; + gpsSolDRV.time.seconds = _buffer.timeutc.sec; + gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } break; case MSG_PVT: next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); - gpsSol.fixType = next_fix_type; - gpsSol.llh.lon = _buffer.pvt.longitude; - gpsSol.llh.lat = _buffer.pvt.latitude; - gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm - gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s - gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s - gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s - gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.numSat = _buffer.pvt.satellites; - gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; + gpsSolDRV.fixType = next_fix_type; + gpsSolDRV.llh.lon = _buffer.pvt.longitude; + gpsSolDRV.llh.lat = _buffer.pvt.latitude; + gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm + gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s + gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s + gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s + gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.numSat = _buffer.pvt.satellites; + gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) { - gpsSol.time.year = _buffer.pvt.year; - gpsSol.time.month = _buffer.pvt.month; - gpsSol.time.day = _buffer.pvt.day; - gpsSol.time.hours = _buffer.pvt.hour; - gpsSol.time.minutes = _buffer.pvt.min; - gpsSol.time.seconds = _buffer.pvt.sec; - gpsSol.time.millis = _buffer.pvt.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.pvt.year; + gpsSolDRV.time.month = _buffer.pvt.month; + gpsSolDRV.time.day = _buffer.pvt.day; + gpsSolDRV.time.hours = _buffer.pvt.hour; + gpsSolDRV.time.minutes = _buffer.pvt.min; + gpsSolDRV.time.seconds = _buffer.pvt.sec; + gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } _new_position = true; @@ -985,6 +985,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread) while (serialRxBytesWaiting(gpsState.gpsPort)) { uint8_t newChar = serialRead(gpsState.gpsPort); if (gpsNewFrameUBLOX(newChar)) { + gpsProcessNewDriverData(); ptSemaphoreSignal(semNewDataReady); break; } From 8c933edb2ca0b1e16466f6c95fe080b52811fffa Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 28 Apr 2023 22:33:18 +0200 Subject: [PATCH 37/61] fixed HITL HW sensors failure simulation --- src/main/fc/fc_msp.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 432ad6e6a7a..828fbe63ee8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3595,8 +3595,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #endif if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - simulatorData.airSpeed = sbufReadU16(src); - } + simulatorData.airSpeed = sbufReadU16(src); + } else { + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + sbufReadU16(src); + } + } if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; From 7d007a5c191c428ef8ab851c5399e8d05e825d35 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 21:14:34 +0200 Subject: [PATCH 38/61] use wind vector for course correction on estimation on gps timeout --- src/main/io/gps.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 10ade84f4df..6b94c12ee0a 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -574,10 +574,9 @@ bool isGPSHealthy(void) return true; } -//NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; + return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; } #endif From cdd299f045ef0d520f85c55027f40ca4735fb19e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:06 +0200 Subject: [PATCH 39/61] fixed gps fix estimation on gps timeout --- src/main/fc/fc_msp.c | 5 +---- src/main/io/gps.c | 43 ++++++++++++++++----------------------- src/main/io/gps_fake.c | 3 +-- src/main/io/gps_msp.c | 2 +- src/main/io/gps_nmea.c | 2 +- src/main/io/gps_private.h | 2 +- src/main/io/gps_ublox.c | 2 +- 7 files changed, 24 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 828fbe63ee8..efec7bf6440 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3515,7 +3515,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { gpsSolDRV.fixType = sbufReadU8(src); gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.flags.hasNewData = true; gpsSolDRV.numSat = sbufReadU8(src); if (gpsSolDRV.fixType != GPS_NO_FIX) { @@ -3536,14 +3535,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu gpsSolDRV.eph = 100; gpsSolDRV.epv = 100; - - ENABLE_STATE(GPS_FIX); } else { sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } // Feed data to navigation gpsProcessNewDriverData(); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 6b94c12ee0a..2d2f2c506b1 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -168,7 +168,7 @@ bool canEstimateGPSFix(void) #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: - //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once + //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && @@ -231,7 +231,6 @@ void updateEstimatedGPSFix(void) gpsSol.fixType = GPS_FIX_3D; gpsSol.hdop = 99; - gpsSol.flags.hasNewData = true; gpsSol.numSat = 99; gpsSol.eph = 100; @@ -298,8 +297,9 @@ void gpsProcessNewDriverData(void) //called after: //1)driver copies gpsSolDRV to gpsSol //2)gpsSol is processed by "Disable GPS logical switch" -//3)gpsSol is processed by GPS Fix estimator -void gpsProcessNewSolutionData(void) +//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix() +//On GPS sensor timeout - called after updateEstimatedGPSFix() +void gpsProcessNewSolutionData(bool timeout) { if ( gpsSol.numSat == 99 ) { ENABLE_STATE(GPS_ESTIMATED_FIX); @@ -320,8 +320,10 @@ void gpsProcessNewSolutionData(void) } } - // Set sensor as ready and available - sensorsSet(SENSOR_GPS); + if (!timeout) { + // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) + sensorsSet(SENSOR_GPS); + } // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -357,15 +359,13 @@ static void gpsResetSolution(gpsSolutionData_t* gpsSol) void gpsTryEstimateOnTimeout(void) { - gpsResetSolution(&gpsSolDRV); gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); updateEstimatedGPSFix(); - if (STATE(GPS_ESTIMATED_FIX)) { - onNewGPSData(); - gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in + gpsProcessNewSolutionData(true); } } @@ -462,28 +462,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) - { + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) { gpsSetState(GPS_LOST_COMMUNICATION); sensorsClear(SENSOR_GPS); gpsStats.timeouts = 5; gpsTryEstimateOnTimeout(); - return false; - } - else - { + } else { gpsSetState(GPS_RUNNING); sensorsSet(SENSOR_GPS); - bool res = gpsSol.flags.hasNewData; - gpsSol.flags.hasNewData = false; - return res; } + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } #endif - // Assume that we don't have new data this run - gpsSol.flags.hasNewData = false; - switch (gpsState.state) { default: case GPS_INITIALIZING: @@ -491,11 +484,9 @@ bool gpsUpdate(void) if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { // Reset internals DISABLE_STATE(GPS_FIX); - gpsSol.fixType = GPS_NO_FIX; // Reset solution gpsResetSolution(&gpsSolDRV); - gpsResetSolution(&gpsSol); // Call GPS protocol reset handler gpsProviders[gpsState.gpsConfig->provider].restart(); @@ -529,7 +520,9 @@ bool gpsUpdate(void) gpsTryEstimateOnTimeout(); } - return gpsSol.flags.hasNewData; + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 30e8eeb94ab..8471800a0e4 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -46,7 +46,7 @@ void gpsFakeRestart(void) void gpsFakeHandle(void) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } void gpsFakeSet( @@ -79,7 +79,6 @@ void gpsFakeSet( gpsSolDRV.flags.validVelNE = true; gpsSolDRV.flags.validVelD = true; gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.hasNewData = true; if (time) { struct tm* gTime = gmtime(&time); diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index d4958117c92..1fd4b1cf15e 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -63,7 +63,7 @@ void gpsRestartMSP(void) void gpsHandleMSP(void) { if (newDataReady) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); newDataReady = false; } } diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c index 7269b102fbd..be27d22ab54 100755 --- a/src/main/io/gps_nmea.c +++ b/src/main/io/gps_nmea.c @@ -313,7 +313,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThreadNMEA) // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore while (1) { ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } ptEnd(0); diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 2a46c8670d2..8c0d8a3ec4e 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -68,7 +68,7 @@ extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainHDOP(uint32_t hdop); void gpsProcessNewDriverData(void); -void gpsProcessNewSolutionData(void); +void gpsProcessNewSolutionData(bool); void gpsSetProtocolTimeout(timeMs_t timeoutMs); extern void gpsRestartUBLOX(void); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index aba2d930f92..8f5be8186ff 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1053,7 +1053,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore while (1) { ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } ptEnd(0); From 3040cd727b0445d2e00bb674a5543d6d83192e51 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:34 +0200 Subject: [PATCH 40/61] return 99 satellites for logic condition on estimation --- src/main/programming/logic_condition.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index cbcbfa57385..f4600b73511 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -663,7 +663,9 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + if ( STATE(GPS_ESTIMATED_FIX) ){ + return gpsSol.numSat; //99 + } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { return 0; } else { return gpsSol.numSat; From daac5b5df36fd64fc7399cb5890a3dd9bbbd9a03 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:55 +0200 Subject: [PATCH 41/61] removed redundand code --- src/main/io/osd.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bbf02b359d1..1f5333861ee 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1728,9 +1728,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (STATE(GPS_ESTIMATED_FIX)) { - if (!STATE(GPS_FIX)) { - strcpy(buff + 2, "ES"); - } + strcpy(buff + 2, "ES"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { From f18b5a0fc5cd1b8cfb1b2b493c5cd24dda46d7db Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:41:18 +0200 Subject: [PATCH 42/61] allow course correction on estimation with gps sensor timeout --- src/main/flight/imu.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 1c9feddf144..0fd56f82901 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -323,10 +323,9 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c #endif } -//NOTE: checks if real GPS data is present, ignores any available GPS Fix estimation bool isGPSTrustworthy(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; + return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) From 7e777faacb15c19a9be73567a702c1a3f68fa4a3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 20 May 2023 01:21:57 +0300 Subject: [PATCH 43/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4f43eed353d..0bf53fa063c 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,22 +19,21 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. -GPS Fix is also estimated on GPS Sensor timeout (harware failure). +GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. - -Without GPS fix, plane has nose heading from magnetometer only. +Without GPS fix, plane has nose heading from magnetometer and height from barometer only. To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing - (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). +It is posible to roughtly estimate position using these assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. @@ -49,7 +48,7 @@ GPS Fix estimation is enabled with CLI command: ```set inav_allow_gps_fix_estimation=ON``` -Also you have to specify criuse airspeed of the plane. +Also you have to specify cruise airspeed of the plane. To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. @@ -64,7 +63,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s *It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.* -*If pitot is available, pitot sensor data will be used instead of constant.* +*If pitot is available, pitot sensor data will be used instead of constant. It is not necessary to specify fw_reference_airspeed. However, it is still adviced to specify for the case of pitot failure.* *Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.* @@ -84,7 +83,19 @@ For testing purpoces, it is possible to disable GPS sensor fix from RC controlle ```failsafe_gps_fix_estimation_delay``` -Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator. + +# Expected error + +Relistic expected error is up to 20%. In tests, 500m drift per 5km path was seen. + +To dicrease drift: +- fly one large circle with GPS available to get good wind estimation +- use airspeed sensor +- do large turns +- make sure compass is pointing nose direction precicely +- calibrate compass correctly + # Is it possible to implement this for multirotor ? From ff1cd25a163743eaf3821b9b5644a35ed2d55b13 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 20 May 2023 13:33:59 +0200 Subject: [PATCH 44/61] fixed compilation error --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2d2f2c506b1..5af00c8dcd0 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -259,7 +259,7 @@ void updateEstimatedGPSFix(void) } // here (velX, velY) is estimated horizontal speed with wind influence = ground speed - if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) { + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) { velX = 0; velY = 0; } From 8778980868e737c64530b0dc48e765fcbefbe6df Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:07:21 +0300 Subject: [PATCH 45/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 0bf53fa063c..533b0e2da28 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -87,15 +87,23 @@ Controls whether waypoint mission is allowed to proceed with gps fix estimation. # Expected error -Relistic expected error is up to 20%. In tests, 500m drift per 5km path was seen. +Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. To dicrease drift: - fly one large circle with GPS available to get good wind estimation -- use airspeed sensor -- do large turns -- make sure compass is pointing nose direction precicely +- use airspeed sensor. If airspeed sensor is not installed, fly in cruise mode without throttle override. +- do smooth, large turns +- make sure compass is pointing in nose direction precicely - calibrate compass correctly +This video shown real world test where GPS was disabled occasionally. Wind is 10km/h south-west: + + +https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592 + + +Purple line shows estimated position. Black line shows real position. "EST ERR" sensor shows estimation error in metters. Estimation is running when satellite icon displays "ES". Estimated position snaps to real position when GPS fix is reaquired. + # Is it possible to implement this for multirotor ? From 5eb46f5328759f522011b17a06161a39efdd4246 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:32:55 +0300 Subject: [PATCH 46/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 533b0e2da28..04fe34ea7f5 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -15,7 +15,7 @@ Plane should have the following sensors: - magnethometer - pitot (optional) -By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above inreachable spaces, plane will be lost. +By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. GPS fix estimation allows to recover plane using magnetometer and baromener only. From 8b1330c4388d1e60eed645bdfa87d4e73dc15b70 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:34:29 +0300 Subject: [PATCH 47/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 04fe34ea7f5..dd7a4de55fb 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -33,7 +33,7 @@ To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing - (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using these assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). +It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. From ee66d0f6194baeb37f958bd083157e1340997fee Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:35:18 +0300 Subject: [PATCH 48/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index dd7a4de55fb..d5bb7c7465b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -35,9 +35,9 @@ To navigate without GPS fix, we make the following assumptions: It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). -From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. +From estimated heading direction and speed, plane is able to **roughty** estimate it's position. -It is assumed, that plane will fly in roughtly estimated direction to home position untill either GPS fix or RC signal is recovered. +It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered. *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. From ae5c14409fe8e173ce59dbe0edb8bc2efecdb3c3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:37:37 +0300 Subject: [PATCH 49/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index d5bb7c7465b..4ed5365e301 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -96,7 +96,7 @@ To dicrease drift: - make sure compass is pointing in nose direction precicely - calibrate compass correctly -This video shown real world test where GPS was disabled occasionally. Wind is 10km/h south-west: +This video shows real world test where GPS was disabled occasionally. Wind is 10km/h south-west: https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592 From 599e45c48f2eb8cd91c462712b9eecdfbe05c84b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 9 Aug 2023 09:48:46 +0200 Subject: [PATCH 50/61] added USE_GPS_FIX_ESTIMATION fixed indentation --- src/main/fc/fc_msp.c | 186 ++++---- src/main/fc/runtime_config.h | 20 +- src/main/fc/settings.yaml | 2 + src/main/flight/failsafe.c | 23 +- src/main/flight/failsafe.h | 4 + src/main/flight/imu.c | 6 +- src/main/flight/wind_estimator.c | 13 +- src/main/io/gps.c | 34 +- src/main/io/gps_ublox.c | 52 +-- src/main/io/osd.c | 398 ++++++++++-------- src/main/io/osd_dji_hd.c | 6 +- src/main/navigation/navigation.c | 8 +- src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 10 +- .../navigation/navigation_pos_estimator.c | 35 +- src/main/programming/logic_condition.c | 13 +- src/main/programming/logic_condition.h | 4 + src/main/sensors/diagnostics.c | 38 +- src/main/sensors/pitotmeter.c | 8 +- src/main/target/common.h | 1 + src/main/telemetry/ghst.c | 6 +- src/main/telemetry/hott.c | 12 +- src/main/telemetry/ibus_shared.c | 96 ++++- src/main/telemetry/jetiexbus.c | 6 +- src/main/telemetry/ltm.c | 6 +- src/main/telemetry/mavlink.c | 12 +- src/main/telemetry/sim.c | 6 +- src/main/telemetry/smartport.c | 6 +- src/main/telemetry/srxl.c | 12 +- 29 files changed, 627 insertions(+), 398 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7ea2ff20ebb..b4e22087453 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3492,8 +3492,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { @@ -3508,15 +3508,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu // Review: Many states were affected. Reboot? disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } + } } else { - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { sensorsSet(SENSOR_BARO); setTaskEnabled(TASK_BARO, true); DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - baroStartCalibration(); + baroStartCalibration(); } #endif @@ -3530,80 +3530,80 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu mag.magADC[Z] = 0; } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); LOG_DEBUG(SYSTEM, "Simulator enabled"); } - if (dataSize >= 14) { - - if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSolDRV.fixType = sbufReadU8(src); - gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.numSat = sbufReadU8(src); - - if (gpsSolDRV.fixType != GPS_NO_FIX) { - gpsSolDRV.flags.validVelNE = true; - gpsSolDRV.flags.validVelD = true; - gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.validTime = false; - - gpsSolDRV.llh.lat = sbufReadU32(src); - gpsSolDRV.llh.lon = sbufReadU32(src); - gpsSolDRV.llh.alt = sbufReadU32(src); - gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); - gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); - - gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); - - gpsSolDRV.eph = 100; - gpsSolDRV.epv = 100; - } else { - sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + if (dataSize >= 14) { + + if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = sbufReadU8(src); + + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + } else { + sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } // Feed data to navigation - gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); - } else { - sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + gpsProcessNewDriverData(); + gpsProcessNewSolutionData(false); + } else { + sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } - if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { - attitude.values.roll = (int16_t)sbufReadU16(src); - attitude.values.pitch = (int16_t)sbufReadU16(src); - attitude.values.yaw = (int16_t)sbufReadU16(src); - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { + attitude.values.roll = (int16_t)sbufReadU16(src); + attitude.values.pitch = (int16_t)sbufReadU16(src); + attitude.values.yaw = (int16_t)sbufReadU16(src); + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } // Get the acceleration in 1G units - acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accVibeSq[X] = 0.0f; - acc.accVibeSq[Y] = 0.0f; - acc.accVibeSq[Z] = 0.0f; + acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accVibeSq[X] = 0.0f; + acc.accVibeSq[Y] = 0.0f; + acc.accVibeSq[Z] = 0.0f; // Get the angular velocity in DPS - gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - - if (sensors(SENSOR_BARO)) { - baro.baroPressure = (int32_t)sbufReadU32(src); - baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); - } else { - sbufAdvance(src, sizeof(uint32_t)); - } + gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - if (sensors(SENSOR_MAG)) { - mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero - mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (sensors(SENSOR_BARO)) { + baro.baroPressure = (int32_t)sbufReadU32(src); + baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); + } else { + sbufAdvance(src, sizeof(uint32_t)); + } + + if (sensors(SENSOR_MAG)) { + mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero + mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } #if defined(USE_FAKE_BATT_SENSOR) if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { @@ -3617,7 +3617,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); - } else { + } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { sbufReadU16(src); } @@ -3626,35 +3626,35 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; } - } else { - DISABLE_STATE(GPS_FIX); - } - } + } else { + DISABLE_STATE(GPS_FIX); + } + } - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); - sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); + sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); - simulatorData.debugIndex++; - if (simulatorData.debugIndex == 8) { - simulatorData.debugIndex = 0; - } + simulatorData.debugIndex++; + if (simulatorData.debugIndex == 8) { + simulatorData.debugIndex = 0; + } - tmp_u8 = simulatorData.debugIndex | - ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | - (ARMING_FLAG(ARMED) ? 64 : 0) | - (!feature(FEATURE_OSD) ? 32: 0) | - (!isOSDTypeSupportedBySimulator() ? 16 : 0); + tmp_u8 = simulatorData.debugIndex | + ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | + (ARMING_FLAG(ARMED) ? 64 : 0) | + (!feature(FEATURE_OSD) ? 32: 0) | + (!isOSDTypeSupportedBySimulator() ? 16 : 0); - sbufWriteU8(dst, tmp_u8); - sbufWriteU32(dst, debug[simulatorData.debugIndex]); + sbufWriteU8(dst, tmp_u8); + sbufWriteU32(dst, debug[simulatorData.debugIndex]); - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, attitude.values.yaw); + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, attitude.values.yaw); - mspWriteSimulatorOSD(dst); + mspWriteSimulatorOSD(dst); *ret = MSP_RESULT_ACK; break; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3b55a4b2816..48cf080ddd8 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -123,7 +123,9 @@ typedef enum { NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED = (1 << 9), +#ifdef USE_GPS_FIX_ESTIMATION GPS_ESTIMATED_FIX = (1 << 10), +#endif NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), @@ -176,12 +178,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); typedef enum { HITL_RESET_FLAGS = (0 << 0), - HITL_ENABLE = (1 << 0), - HITL_SIMULATE_BATTERY = (1 << 1), - HITL_MUTE_BEEPER = (1 << 2), - HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) - HITL_HAS_NEW_GPS_DATA = (1 << 4), - HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 + HITL_ENABLE = (1 << 0), + HITL_SIMULATE_BATTERY = (1 << 1), + HITL_MUTE_BEEPER = (1 << 2), + HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) + HITL_HAS_NEW_GPS_DATA = (1 << 4), + HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 HITL_AIRSPEED = (1 << 6), HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 HITL_GPS_TIMEOUT = (1 << 8), @@ -189,10 +191,10 @@ typedef enum { } simulatorFlags_t; typedef struct { - simulatorFlags_t flags; - uint8_t debugIndex; + simulatorFlags_t flags; + uint8_t debugIndex; uint8_t vbat; // 126 -> 12.6V - uint16_t airSpeed; // cm/s + uint16_t airSpeed; // cm/s int16_t input[4]; } simulatorData_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fb25aeee7c1..b1e4d3f603b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -834,6 +834,7 @@ groups: max: 600 - name: failsafe_gps_fix_estimation_delay description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." + condition: USE_GPS_FIX_ESTIMATION default_value: 7 min: -1 max: 600 @@ -2195,6 +2196,7 @@ groups: type: bool - name: inav_allow_gps_fix_estimation description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." + condition: USE_GPS_FIX_ESTIMATION default_value: OFF field: allow_gps_fix_estimation type: bool diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index e2cd71cc9d7..d1d1ede6282 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -82,7 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) +#ifdef USE_GPS_FIX_ESTIMATION .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied +#endif ); typedef enum { @@ -355,7 +357,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { + ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); @@ -368,7 +374,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) return failsafeConfig()->failsafe_procedure; } - +#ifdef USE_GPS_FIX_ESTIMATION bool checkGPSFixFailsafe(void) { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { @@ -387,6 +393,7 @@ bool checkGPSFixFailsafe(void) } return false; } +#endif void failsafeUpdateState(void) @@ -418,9 +425,12 @@ void failsafeUpdateState(void) failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } +#ifdef USE_GPS_FIX_ESTIMATION if ( checkGPSFixFailsafe() ) { reprocessState = true; - } else if (!receivingRxDataAndNotFailsafeMode) { + } else +#endif + if (!receivingRxDataAndNotFailsafeMode) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero @@ -432,7 +442,7 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { @@ -488,11 +498,14 @@ void failsafeUpdateState(void) } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; reprocessState = true; - } else { + } +#ifdef USE_GPS_FIX_ESTIMATION + else { if ( checkGPSFixFailsafe() ) { reprocessState = true; } } +#endif break; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d80ab650c78..459a201d242 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,7 +43,9 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) +#ifdef USE_GPS_FIX_ESTIMATION int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) +#endif } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -150,7 +152,9 @@ typedef struct failsafeState_s { timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time +#ifdef USE_GPS_FIX_ESTIMATION timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation +#endif failsafeProcedure_e activeProcedure; failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b46224063f9..94223f04492 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -323,7 +323,11 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c bool isGPSTrustworthy(void) { - return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); + return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ; } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index b6174611add..1cf3b8941ee 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT]; bool isEstimatedWindSpeedValid(void) { - return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. + return hasValidWindEstimate +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation. +#endif + ; } float getEstimatedWindSpeed(int axis) @@ -90,8 +94,11 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (!STATE(FIXED_WING_LEGACY) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || - !gpsSol.flags.validVelD || - STATE(GPS_ESTIMATED_FIX)) { + !gpsSol.flags.validVelD +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 965b89531a8..0e5d166c7c7 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -223,7 +223,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) gpsState.timeoutMs = timeoutMs; } - +#ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) @@ -238,10 +238,11 @@ bool canEstimateGPSFix(void) #else return false; -#endif - +#endif } +#endif +#ifdef USE_GPS_FIX_ESTIMATION void processDisableGPSFix(void) { static int32_t last_lat = 0; @@ -268,7 +269,9 @@ void processDisableGPSFix(void) last_alt = gpsSol.llh.alt; } } +#endif +#ifdef USE_GPS_FIX_ESTIMATION //called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" void updateEstimatedGPSFix(void) { @@ -345,14 +348,17 @@ void updateEstimatedGPSFix(void) gpsSol.velNED[Y] = (int16_t)(velY); gpsSol.velNED[Z] = 0; } +#endif void gpsProcessNewDriverData(void) { gpsSol = gpsSolDRV; +#ifdef USE_GPS_FIX_ESTIMATION processDisableGPSFix(); updateEstimatedGPSFix(); +#endif } //called after: @@ -362,11 +368,13 @@ void gpsProcessNewDriverData(void) //On GPS sensor timeout - called after updateEstimatedGPSFix() void gpsProcessNewSolutionData(bool timeout) { +#ifdef USE_GPS_FIX_ESTIMATION if ( gpsSol.numSat == 99 ) { ENABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_ESTIMATED_FIX); +#endif // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { @@ -379,7 +387,9 @@ void gpsProcessNewSolutionData(bool timeout) gpsSol.flags.validEPE = false; DISABLE_STATE(GPS_FIX); } +#ifdef USE_GPS_FIX_ESTIMATION } +#endif if (!timeout) { // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) @@ -423,11 +433,15 @@ void gpsTryEstimateOnTimeout(void) gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); - updateEstimatedGPSFix(); +#ifdef USE_GPS_FIX_ESTIMATION + if ( canEstimateGPSFix() ) { + updateEstimatedGPSFix(); - if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in - gpsProcessNewSolutionData(true); + if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in + gpsProcessNewSolutionData(true); + } } +#endif } void gpsPreInit(void) @@ -577,7 +591,7 @@ bool gpsUpdate(void) break; } - if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { + if ( !sensors(SENSOR_GPS) ) { gpsTryEstimateOnTimeout(); } @@ -630,7 +644,11 @@ bool isGPSHealthy(void) bool isGPSHeadingValid(void) { - return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; + return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed >= 300; } #endif diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 10f1e6d9473..59eb68bd93b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -147,7 +147,7 @@ static union { bool gpsUbloxHasGalileo(void) { return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); - } +} bool gpsUbloxHasBeidou(void) { @@ -622,8 +622,8 @@ static bool gpsParseFrameUBLOX(void) gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { if (_buffer.ver.swVersion[9] > '2' || true) { - // check extensions; - // after hw + sw vers; each is 30 bytes + // check extensions; + // after hw + sw vers; each is 30 bytes bool found = false; for (int j = 40; j < _payload_length && !found; j += 30) { @@ -863,8 +863,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } else { // u-Blox 5/6/7/8 or unknown @@ -901,8 +901,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } // u-Blox 5/6 doesn't support PVT, use legacy config // UNKNOWN also falls here, use as a last resort @@ -944,19 +944,19 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); - } else { - configureGNSS(); - } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } else { + configureGNSS(); + } + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - if(_ack_state == UBX_ACK_GOT_NAK) { + if(_ack_state == UBX_ACK_GOT_NAK) { gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; - } + } } ptEnd(0); @@ -1019,18 +1019,18 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Reset protocol timeout - gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); + // Reset protocol timeout + gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); - // Attempt to detect GPS hw version - gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; - gpsState.autoConfigStep = 0; + // Attempt to detect GPS hw version + gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; + gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); gpsState.autoConfigStep = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; @@ -1062,7 +1062,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) { pollVersion(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - } + } pollGnssCapabilities(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 518cdc61aa1..51ca9d03a73 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1080,7 +1080,7 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { TEXT_ATTRIBUTES_ADD_BLINK(*attr); -} + } } void osdCrosshairPosition(uint8_t *x, uint8_t *y) @@ -1114,7 +1114,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes if (useScaled) { buff[0] = SYM_SCALE; } else { - buff[0] = SYM_BLANK; + buff[0] = SYM_BLANK; } buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { @@ -1315,7 +1315,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); float poiAngle = DEGREES_TO_RADIANS(directionToPoi); @@ -1429,7 +1433,11 @@ static void osdDisplayTelemetry(void) static uint16_t trk_bearing = 0; if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ){ if (GPS_distanceToHome > 5) { trk_bearing = GPS_directionToHome; trk_bearing += 360 + 180; @@ -1729,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item) // Shown in Ah buff[mah_digits] = SYM_AH; } else { - // Shown in mAh + // Shown in mAh buff[mah_digits] = SYM_MAH; } buff[mah_digits + 1] = '\0'; @@ -1786,12 +1794,16 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { strcpy(buff + 2, "ES"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); + } else +#endif + if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; @@ -1843,7 +1855,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } @@ -2331,11 +2347,19 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (osdConfig()->hud_homing && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { osdHudDrawHoming(elemPosX, elemPosY); } - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && isImuHeadingValid()) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { osdHudClear(); @@ -2959,8 +2983,12 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -3030,7 +3058,11 @@ static bool osdDrawSingleElement(uint8_t item) int32_t value = 0; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -3182,7 +3214,11 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; int digitsRemoved = osdConfig()->plus_code_short * 2; - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { // +codes with > 8 digits have a + at the 9th digit @@ -4112,13 +4148,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); } else if (page == 1) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); } if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { + if (feature(FEATURE_GPS)) { if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); @@ -4130,32 +4166,32 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); - osdFormatDistanceStr(buff, stats.max_distance*100); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); + osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); - osdFormatDistanceStr(buff, getTotalTravelDistance()); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); + osdFormatDistanceStr(buff, getTotalTravelDistance()); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); - osdFormatAltitudeStr(buff, stats.max_altitude); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); + osdFormatAltitudeStr(buff, stats.max_altitude); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); itoa(stats.min_rssi, buff, 10); @@ -4168,172 +4204,172 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } - - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); - } - - if (isSinglePageStatsCompatible || page == 1) { - if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); - } - tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } + + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + } - displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + if (isSinglePageStatsCompatible || page == 1) { + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_WH); + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); } + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); - int32_t totalDistance = getTotalTravelDistance(); - bool moreThanAh = false; - bool efficiencyValid = totalDistance >= 10000; - if (feature(FEATURE_GPS)) { - displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - uint8_t digits = 3U; // Total number of digits (including decimal point) - #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so no switch to scaled decimal occurs above 99 - digits = 4U; - } - #endif - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + if (feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + } else { + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH); + } + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; + if (feature(FEATURE_GPS)) { + displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; } - } - break; - case OSD_UNIT_GA: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + #endif + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_NM_0; - buff[4] = SYM_MAH_NM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } - break; + break; + } + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); } - } - const float max_gforce = accGetMeasuredMaxG(); - displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; - displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b8ddd058cb3..7b837e8ceef 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -786,7 +786,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 47d644b70b5..f289a4ab27a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2378,6 +2378,7 @@ bool validateRTHSanityChecker(void) return true; } +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump @@ -2386,6 +2387,7 @@ bool validateRTHSanityChecker(void) posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; return true; } +#endif // Check at 10Hz rate if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { @@ -3993,7 +3995,11 @@ void updateWpMissionPlanner(void) { static timeMs_t resetTimerStart = 0; if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { - const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); + const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ); posControl.flags.wpMissionPlannerActive = true; if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5b1abf0f88f..717f0df0e75 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -210,7 +210,9 @@ typedef struct positionEstimationConfig_s { uint8_t use_gps_no_baro; +#ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; +#endif } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 51aa03b5d43..405a06b4a59 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -313,13 +313,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; + posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right + // turn direction to next waypoint + loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = true; + needToCalculateCircularLoiter = true; } posControl.flags.wpTurnSmoothingActive = true; } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 9fed0f69786..50f961f1d6b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -92,8 +92,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, - +#ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT +#endif ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -152,12 +153,14 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump previousTime = 0; return true; } +#endif if (previousTime == 0) { isGlitching = false; @@ -210,8 +213,16 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { - if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + if (!(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { isFirstGPSUpdate = true; return; } @@ -492,7 +503,11 @@ static bool navIsAccelerationUsable(void) static bool navIsHeadingUsable(void) { - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { // If we have GPS - we need true IMU north (valid heading) return isImuHeadingValid(); } @@ -507,7 +522,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; - if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && + if ((sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { @@ -704,7 +723,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && navIsHeadingUsable()) { static timeUs_t lastUpdateTimeUs = 0; if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f4600b73511..19ef76e5155 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -471,8 +471,9 @@ static int logicConditionCompute( } else { return false; } - break; + break; +#ifdef USE_GPS_FIX_ESTIMATION case LOGIC_CONDITION_DISABLE_GPS_FIX: if (operandA > 0) { LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); @@ -480,8 +481,9 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); } return true; - break; - + break; +#endif + default: return false; break; @@ -663,9 +665,12 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: +#ifdef USE_GPS_FIX_ESTIMATION if ( STATE(GPS_ESTIMATED_FIX) ){ return gpsSol.numSat; //99 - } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + } else +#endif + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { return 0; } else { return gpsSol.numSat; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 69565393911..1dd197be9ff 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,7 +81,9 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, +#ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_DISABLE_GPS_FIX = 52, +#endif LOGIC_CONDITION_LAST = 53, } logicOperation_e; @@ -186,7 +188,9 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10), +#ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), +#endif } logicConditionsGlobalFlags_t; typedef enum { diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index cc874b6bb81..56c4d12417b 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void) if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (accIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } - } - else { + } else { if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; @@ -64,13 +62,13 @@ hardwareSensorStatus_e getHwCompassStatus(void) { #if defined(USE_MAG) #ifdef USE_SIMULATOR - if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { + if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { if (compassIsHealthy()) { return HW_SENSOR_OK; } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { @@ -96,7 +94,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) { #if defined(USE_BARO) #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { + if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) { return HW_SENSOR_NONE; } else if (baroIsHealthy()) { @@ -104,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (baroIsHealthy()) { @@ -135,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void) if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (rangefinderIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -144,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void) if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -161,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void) if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (pitotIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -170,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void) if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -187,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) if (sensors(SENSOR_GPS)) { if (isGPSHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -196,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -213,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void) if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (opflowIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -222,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void) if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 2fae0a16922..e24898cdd14 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -215,14 +215,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + } #endif ptYield(); @@ -248,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitot.airSpeed = simulatorData.airSpeed; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); - } + } #endif } diff --git a/src/main/target/common.h b/src/main/target/common.h index d18bdfb1a2e..242e03fc62c 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -56,6 +56,7 @@ #define USE_GPS_PROTO_MSP #define USE_TELEMETRY #define USE_TELEMETRY_LTM +#define USE_GPS_FIX_ESTIMATION // This is the shortest period in microseconds that the scheduler will allow #define SCHEDULER_DELAY_LIMIT 10 diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index a9630efa317..c468d2cfac5 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,7 +147,11 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { gpsFlags |= GPS_FLAGS_FIX; } if (STATE(GPS_FIX_HOME)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index a9810eb8f6d..984b03caa6a 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,7 +234,11 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if (!(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX)) +#endif + ) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } @@ -476,7 +480,11 @@ static bool processBinaryModeRequest(uint8_t address) switch (address) { #ifdef USE_GPS case 0x8A: - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { hottPrepareGPSResponse(&hottGPSMessage); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); return true; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1bab900776a..3c7dfd2875a 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -136,7 +136,11 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { #if defined(USE_GPS) uint8_t fix = 0; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { if (gpsSol.fixType == GPS_NO_FIX) fix = 1; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; @@ -196,7 +200,11 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { status += gpsSol.numSat * 1000; status += fix * 100; if (STATE(GPS_FIX_HOME)) status += 500; @@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { return sendIbusMeasurement2(address, status); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t #endif return sendIbusMeasurement2(address, 0); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 53de96cc5d4..b0ffad84347 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -575,7 +575,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); if (!allSensorsActive) { - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { enableGpsTelemetry(true); allSensorsActive = true; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index e194cfed937..84dccddb869 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -117,7 +117,11 @@ void ltm_gframe(sbuf_t *dst) uint8_t gps_fix_type = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { if (gpsSol.fixType == GPS_NO_FIX) gps_fix_type = 1; else if (gpsSol.fixType == GPS_FIX_2D) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c0759265808..3090ed68dc7 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -523,7 +523,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; - if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) return; if (gpsSol.fixType == GPS_NO_FIX) @@ -638,7 +642,11 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index a0537bfcd5c..9b1f7cc6403 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,11 @@ static void sendSMS(void) ZERO_FARRAY(pluscode_url); - if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { + if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 64a6559f1d0..9d804be52d8 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -414,7 +414,11 @@ static bool smartPortShouldSendGPSData(void) // or the craft has never been armed yet. This way if GPS stops working // while in flight, the user will easily notice because the sensor will stop // updating. - return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); + return feature(FEATURE_GPS) && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + || !ARMING_FLAG(WAS_EVER_ARMED)); } void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index d8a8c15c33a..c341363f160 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -303,7 +303,11 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; - if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.numSat < 6) { return false; } @@ -371,7 +375,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; - if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )|| gpsSol.numSat < 6) { return false; } From 4602a837fbce0ce2226b9c6c2b3ca3db31705b69 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 9 Aug 2023 10:14:57 +0200 Subject: [PATCH 51/61] fixed unit test error --- src/main/telemetry/hott.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 984b03caa6a..8e2e7d8c075 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,11 +234,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!(STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) +#else + if (!(STATE(GPS_FIX))) #endif - ) { + { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } From dc1d2e8d4e79081c381871a97394007a51cfaef2 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 10 Aug 2023 00:34:12 +0200 Subject: [PATCH 52/61] ignore project file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index ea13b9f1789..6f74144b06b 100644 --- a/.gitignore +++ b/.gitignore @@ -31,3 +31,4 @@ README.pdf # local changes only make/local.mk +/inav.code-workspace From 86b875da5a8ca58545c8f9e8dda556cc73f558cd Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 3 Sep 2023 22:31:20 +0200 Subject: [PATCH 53/61] allow gps fix estimation without compass --- src/main/io/gps.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0e5d166c7c7..129d05c4173 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -226,14 +226,13 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) #ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { -#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) +#if defined(USE_GPS) && && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && - sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); #else From 6168348ca70425a0e375ed2af84f4a260c594f94 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:02:33 +0300 Subject: [PATCH 54/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4ed5365e301..672f4845c2b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -12,7 +12,7 @@ Plane should have the following sensors: - acceleromenter, gyroscope - barometer - GPS -- magnethometer +- magnethometer (optional, highly recommended) - pitot (optional) By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. @@ -41,6 +41,23 @@ It is assumed, that plane will fly in roughly estimated direction to home positi *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. +# Navigation without magnethometer + +Without magnethometer, navigation accuracy is very poor. The problem is heading drift. + +The longer plane flies without magnethometer or GPS, the bigger is course estimation error. + +After few minutes and few turns, "North" direction estimation can be completely broken. +In general, accuracy is enought to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. + +![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) + +(purple line - estimated position, black line - real position). + +It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing. + +It is up to user to estimate the risk of fly-away. + # Settings @@ -75,7 +92,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s ![](Screenshots/programming_disable_gps_sensor_fix.png) -For testing purpoces, it is possible to disable GPS sensor fix from RC controller in programming tab: +For testing purposes, it is possible to disable GPS sensor fix from RC controller in programming tab: *GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.* From 713b52176e2b0635adfe774fb5005f83280bfdb4 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:04:54 +0300 Subject: [PATCH 55/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 672f4845c2b..37cfa73df20 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -41,7 +41,7 @@ It is assumed, that plane will fly in roughly estimated direction to home positi *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. -# Navigation without magnethometer +# Estimation without magnethometer Without magnethometer, navigation accuracy is very poor. The problem is heading drift. From 0829ef6f01f39d061de44311b87b4067cb419fdf Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:05:44 +0300 Subject: [PATCH 56/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 37cfa73df20..dcd1b2ac7ce 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ Without magnethometer, navigation accuracy is very poor. The problem is heading The longer plane flies without magnethometer or GPS, the bigger is course estimation error. After few minutes and few turns, "North" direction estimation can be completely broken. -In general, accuracy is enought to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. +In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. ![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) From 3ee200de83568ca69d7f7f7efb50f6eb002f60f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:06:49 +0300 Subject: [PATCH 57/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index dcd1b2ac7ce..44487a6c062 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ Without magnethometer, navigation accuracy is very poor. The problem is heading The longer plane flies without magnethometer or GPS, the bigger is course estimation error. After few minutes and few turns, "North" direction estimation can be completely broken. -In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. +In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages. ![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) From f05dcc17e9e46d34134805afc123f20e26525734 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:08:26 +0300 Subject: [PATCH 58/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 44487a6c062..ed67682353f 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -102,7 +102,7 @@ For testing purposes, it is possible to disable GPS sensor fix from RC controlle Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator. -# Expected error +# Expected error (mag + baro) Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. From b7745af0f042ebff2fa2bb2ebbef45ee7505696a Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 3 Sep 2023 23:51:42 +0200 Subject: [PATCH 59/61] fixed compilation error --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index aa5450fc9b4..28b666c1ac9 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -231,7 +231,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) #ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { -#if defined(USE_GPS) && && defined(USE_BARO) +#if defined(USE_GPS) && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once From f3359efbc6010b29cc7b3ef6cd256811f5064962 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 3 Nov 2023 15:46:19 +0200 Subject: [PATCH 60/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index ed67682353f..28e2f170f68 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,7 +19,7 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). From 229eac45f08c3cf9fd15d124b3f2979046e1d697 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 3 Nov 2023 15:47:12 +0200 Subject: [PATCH 61/61] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 28e2f170f68..d8175549d35 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,10 +19,10 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. - GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. + # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints.