inav
inav copied to clipboard
iNavFlight
GPS Fix estimation (dead reconing, RTH without GPS fix) for fixed wing
GPS Fix estimation (dead reconing, RTH without GPS fix) for fixed wing
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.
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 "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.
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
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
I think some problems occurred during your push... Because you have several files modified, with the HIL mode included... You have 2 options:
1 - Close this PR and open a new one with only the modifications of this new recurse.
2 - Sync the branch of this PR with the Master, and delete the files from the HIL mode.
As there were many files changed, I myself couldn't read the algorithm and understand what you changed to add the feature (maybe it's even laziness on my part)
yeah... this PR is hard to analyze now, I'll leave it for the future
the "dead-reckoning" part of the coordinates, wind and fuselage velocity also exists in a new AHRS that I'm working on bringing to iNav... Glad we had the same idea ^^
the "BOXGPSOFF" option should only be kept during testing before that one day it is approved to merge... It will be good help for Dev's testing... When this feature has a final version, I recommend removing the option to force the GPS failure (I don't think you want users bothering you and saying that your algorithm crashes their planes)
Does the wind estimator only seem to work if you fly in circles ? This is typically how wind estimators work with software for paragliders/hang gliders etc, either rely on flying in circles (thermaling) or flying in zig zags was another option.
Zigzag should work too.
Wind estimator does estimation when attititude changes by ~10 degrees. Then estimated wind vector is updated with 5% of computed value. ~36 estimations in circle is enough to get good estimation.
https://drive.google.com/file/d/0ByvTkVQo3tqXVzBYQUZicUNvbEE/view?resourcekey=0-s-7v3PgOUrZt7fI6JNlu-Q
@RomanLut
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
is this cm/s or m/s ?
is this cm/s or m/s ?
You are right, it should be cm/s, updated documentation. Thanks.
Hi @RomanLut I want to real flight test this feature in the next days. But it has conflicts. Can you solve them so I can put it in a test build?
- Fixed conflicts with current master branch
- Removed "GPS Off" box
- Implemented "Disable GPS Sensor Fix" logic condition https://github.com/iNavFlight/inav-configurator/pull/1669
Hi @RomanLut I want to real flight test this feature in the next days. But it has conflicts. Can you solve them so I can put it in a test build?
Quick reminder:
- set inav_allow_gps_fix_estimation=ON
- set fw_reference_airspeed=2777
Set for your wing. 2777 is valid for FPV SurfWing. - save
- setup "Disable GPS Sensor fix" functionality in programming tab.
- Get used with feature in simulator
- Fly with stable RC and Video Link while testing!
More details in docs: https://github.com/RomanLut/inav/blob/submit-gps-fix-estimation/docs/GPS_fix_estimation.md
Thanks a lot. Yes I am currently testing the build in the SIM properly. I have build a Bixler 2 with Airspeed, Compass, Lidar and all the stuff as a test platform. I assume with an Airspeed sensor enabled I do not need to set reference airspeed? The AR Wing Mockup model has 2250cm/s ref airspeed.
@RomanLut during Simulator testing I found something weird. If I disable the GPS fix, the airspeed also changes to the fw_reference_airspeed and is not updated by the Airspeed sensor anymore.
Right now I have set the airspeed sensor for the simulator to VIRTUAL as I thought that the HITL Plugin then simulates a physical Airspeed sensor. Is this wrong?
Okay nevermind. Mea culpa. I forgot to turn on Pitot Simulation in the Plugin settings and it was always running from actual Virtual Pitot data.
Okay nevermind. Mea culpa. I forgot to turn on Pitot Simulation in the Plugin settings and it was always running from actual Virtual Pitot data.
Yes, without GPS fix, Virtual pitot falls back to fw_reference_airspeed.
So in the SIM it works really well. if Wind Speed is properly estimated, even with some gusts, I can run my full autoland mission on GPS estimation and still touch down within the runway boundaries.
But I have a suggestion for a feature change. For set inav_allow_gps_fix_estimation I suggest to add a 3rd option: ON_FS
So as soon as the GPS fix is lost, no matter what, the plane should initiate a RTH if that setting is set. having this feature on while in a WP mission without previously properly circle to get the correct wind speed, continuing the mission could be dangerous. Going home on a direct path massively raises the chance to get the plane back.
Currently the User would not be made aware of a GPS loss when it happens if the FPV feed is not monitored.
But I have a suggestion for a feature change. For set
inav_allow_gps_fix_estimationI suggest to add a 3rd option: ON_FS So as soon as the GPS fix is lost, no matter what, the plane should initiate a RTH if that setting is set.
Well, I am not yet sure we should pack multiple subjects into single setting because inav_allow_gps_fix_estimation has nothing to do with missions. It is about enabling or disabling core feature.
"ON_FS" (or longer "ON_WITH_FAILSAFE_MISSION) would mean:
- enable GFX Fiix esimation AND
- on lost GPS fix event, trigger failsafe action (as configured by failsafe_procedure), if flight mode is WP, with hardcoded delay.
Maybe there should be separate options for mission configuration, like failsafe_mission_on_gps_fix_estimation_procedure, failsafe_mission_on_gps_fix_estimation_delay.
On the other hand, I would stop developing options but evolve Programming tab for such behavior scripts instead. You already can detect GPS fix estimation (GPS_Sats =99), wait for delay, and trigger RTH or Failsafe (with RC_Channels_Override) in programming tab.
To make this more convient, I would adjust GPS_Fix operand with 0-no fix, 1-has fix, 2-estimation, and implement specific logic condition to trigger Failsave, RTH, Land, or Disarm actions without RC_Channels_Override trick.
I would like to hear core developers opinion on this because the one who accepts push requests does such decisions :)
Maybe we misunderstood each other?
Right now if there is no failsafe (radio link is stable) and inav_allow_gps_fix_estimation is turned ON, no matter what the plane is currently doing it will continue to do. If the pilot does not notice the GPS fix loss by the OSD he will not notice at all.
The plane will therefore just continue whatever it is doing (loitering, Mission, 3D Cruise mode) without notifying the pilot beside a blinking ES on the GPS symbol.
So my suggestion is, if you don't want to add more options to the parameter itself, to at least by default trigger a FAILSAFE for a short moment. Then normal FS Procedure is executed. If the user has set to ignore failsafe during a mission, then it is the pilots decision. But if the Feature is on, then GPS is lost, triggering a failsafe would gain the pilots attention for sure. He still can then move the sticks and override the failsafe to continue whatever he does with estimated GPS fix.
Maybe also add a System message to remind the pilot prominently about the GPS loss.
My opinion is just, that switching into estimated mode with no further notice than a small blinking icon is dangerous.
What I am trying to say is that we should not add more options to inav_allow_gps_fix_estimation to control multiple things.
- inav_allow_gps_fix_estimation should control gfx fix estimation only - on/off
- we have a setting to contol whether mission switches to failsafe on RC link lost: failsafe_mission_delay So there should be more settings like: failsafe_mission_gps_fix_estimation_delay - controls whether massion swithes to failsafe mode on gps fix estimation failsafe_mission_low_battery_delay - controls whether mission swithes to failsafe on low battery etc.
I agree on failsafe_mission_gps_fix_estimation_delay, this sounds like a good option. But maybe call it in general gps_fix_estimation_failsafe_delay and apply it to any navigation mode. So if GPS Fix Estimation has to take over due to a fix loss, the plane can failsafe and RTH in any GPS assisted mode unless the user takes back over with stick input or disables navigation.
regarding the low battery RTH I think this should be a separate PR in general but I really like that Idea. some people asked for low battery RTH already but low voltage based RTH makes no sense. But with the now properly working wind estimator, the flight time and flight distance estimation should be able to work out of the box without much tuning (beside idle power). what do you think @shellixyz? We can have windspeed and airspeed now from the AHRS and therefore also proper cruise power and efficiency very precisely. that should make Battery estimation much easier and simpler to set up.
Yes with accurate wind speed and direction information assuming the idle power, cruise power and cruise speed are set correctly it should be possible to have a reliable RTH based on the estimated time to go home with the specified battery margin which is already implemented. I have tested it several times in the past without wind and it seemed to be working fine. I haven't had any negative feedback about it either. Although I haven't got any feedback at all so I don't know how many people have used it and it has been a while since I last tested it so I don't know what impact modifitications to the code since I tested it had.
Basically adding the feature would just be a matter of adding a parameter to be able to enable/disable it and when it is enabled and calculateRemainingEnergyBeforeRTH() returns 0 trigger RTH.
Then I can give you feedback: If properly tuned it works very well. I always set it up a bit more conservative although mostly I had too much battery left after return but as wind speed estimation was never good, this was better than the opposite.
so an automatic trigger for GPS assisted modes with user override option would be great. Don't yxou think cruise power and cruise speed could be determined automatically now, during level flight in navigation modes? with an average over the last 1 or 2 minutes of flight?
anyway that's a topic for a different ticket/PR. Just came into my mind.
Sure it should be possible to auto-learn the cruise parameters fairly easily assuming air speed estimation is reliable
I agree on
failsafe_mission_gps_fix_estimation_delay, this sounds like a good option. But maybe call it in generalgps_fix_estimation_failsafe_delayand apply it to any navigation mode. So if GPS Fix Estimation has to take over due to a fix loss, the plane can failsafe and RTH in any GPS assisted mode unless the user takes back over with stick input or disables navigation.
RTH is GPS -assisted mode too :) So an option should be related to mission mode configuration only.
True, but even during RTH a failsafe can happen. And a failsafe is an indicator that something is wrong. That's why my idea is, to trigger a failsafe condition as soon as the GPS fix is lost. At least as an option. I see no reason why this should only be limited to missions.
Added 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.