Refactor position estimation code. Add support for optic flow for position estimation

[digitalentity]

Supersedes #3253, requires some cleanup

[digitalentity]

Rebased, seems to build correctly. Test pending for next days.

[digitalentity]

Based on what I see in the code transition from optic flow in dead-reckoning mode to GPS will result in drastic position change.
I guess when transitioning from dead reckoning to GPS for the very first time we need to recalculate origin point needs to be calculated in such way that estimated position doesn't change. As for consecutive transitions to/from opflow DR - I'm not sure.

[stronnag]

trivia:

%% pitotmeter.c 
./src/main/navigation/navigation_pos_estimator_flow.c: In function 'estimationCalculateCorrection_XY_FLOW':
./src/main/navigation/navigation_pos_estimator_flow.c:64:65: warning: unused parameter 'ctx' [-Wunused-parameter]
 bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
                                           ~~~~~~~~~~~~~~~~~~~~~~^~~
%% rangefinder.c 
./src/main/navigation/navigation_pos_estimator_flow.c:122:1: warning: control reaches end of non-void function [-Wreturn-type]
 }
 ^

[digitalentity]

@stronnag thanks, I mentioned that as well. Needs fixing obviously 😄

[digitalentity]

@stronnag if you are going to test this on existing setups - use caution, @YGlamazdin did a pretty massive change to position estimation code, something might be broken.

[stronnag]

Sole reason for building it. I'll be careful (famous last words). One needs a little drama from time to time.

[digitalentity]

A PR that touches almost entire position estimation seems a valid possibility to have some drama. Good luck @stronnag 👍

[stronnag]

I briefly flew this today (tested would be too grand a claim). A bit like December 2015 revisited, without the cold; working but not unexpected there was a regression against current capability. Generally, it was OK navigating in a straight line at constant altitude; ask to change altitude (and to a lesser degree, direction) and it would deviate significantly from the required course.

The test machine is pretty much a throw together from the spares box, not (well / at all) tuned. There are two logs attached, with a link to a video rendition. In each case:

• Initial PH (green icon / track)
• RTH from c. 15m (cyan fly out track, yellow icon and RTH track)
• Simple (easy LOS recovery) mission (white icon / track)

Blackbox logs attached (apologies if not useful resolution due to 2Mb SPI Flash on QVF4 board --- if necessary can be repeated with higher logging rate). Videos are from mwp logs as this provides more context (e.g. WP numbers). Note that due to an error by the mission drafter there are some coincident WPs).

Test 1: New PosEst branch:

• Initial PH is not stable, abnormal 'toilet bowling' observed (1:20 - 1:40 in video)
• Large movement during RTH climb / settle phase, significant landing phase (descent) and subsequent PH mode movement.
• Mission. Large deviation from track on initial climb to mission altitude. Pretty wild (and increasing) 'toilet bowl' oscillation after RTH, resulting the pilot hitting manual RTH to escape mission mode and gain altitude, following by manual recovery.

https://vimeo.com/274549508
BBL_2018-06-11_162119_1.TXT

Test 2: Mainline (dev) PosEst branch:

• Not withstanding the lack of tune, the initial PH is stable
• RTH is precise, descent / landing is stable (no appreciable 'tb')
• Mission is reasonably precise, RTH phase / landing is stable (no appreciable 'tb')

https://vimeo.com/274548859
BBL_2018-06-11_162119_6.TXT

For both flights there was an unstable / gusty 15 kph wind from NE / E.

The same settings where applied for each test, in particular, default 1.9+ nav_mc_pos_xy_* and nav_mc_vel_xy_*

[digitalentity]

@stronnag thanks for testing. I wouldn't expect it to be bug-free. Now things are finally getting interesting 😄

[stronnag]

Interesting how the eye deceives; in the field, omg!!!! it's oscillating wildly over a 10m radius; we're doomed for sure, crash is inevitable, take cover. On replay, it's more like 2m (still not so good).

[digitalentity]

@stronnag found a really stupid typo in the code. Explains what you've seen - extreme position deviation when changing altitude.

[stronnag]

Excellent, that explains it. I'll try the same crash dummy again, probably on Wednesday.

[stronnag]

I put 20 flights or more on this branch today, PH, RTH, WP, all excellent, at least as good as the extant position estimator, and remarkably good (now) holding course during rapid altitude change.
Flown on two different machines in rather windy (20knots / 37 kph) conditions.

It also flew (autonomously) through a couple of 15 satellites - 0 satellites - 15 satellites dropouts (over 1 possibly 2 seconds) without much excitement, maybe a 2-3m position jump --- I need to review the logs to see exactly what happened.

Bottom line: I'm content to fly this pos estimator for navigation modes. It goes on the 'precious tricopter' on Friday; I cannot place more trust in the code.

[digitalentity]

Good to know, thanks a lot @stronnag!
After a few minor cleanups I'm going to merge this. Although if it doesn't introduce complete and reliable optic flow support, it's a good first step.

[stronnag]

For completeness, I flew this on the 'precious tricopter' and nothing out of the ordinary occurred (which is a sort of complement).

[digitalentity]

Thanks @stronnag, I believe this is safe to merge now.

[Robokishan]

It seems that in this commit a60e9f6 lots of code has been removed and added new better. position estimate history code is one of them so then gps history has been removed . gps sample rate is at 10Hz , accelerometer sample rate is at 1000Hz. and TASK_GYROPID is running at 1000hz . so are we using same gps reading for calculation ? since gps is at slow rate . how accelerometer sample rate is matching with gps sample rate in estimation calculation .