Airspeed validation & selection module #12353

sfuhrer · 2019-06-27T17:36:40Z

This PR re-purposes the current wind estimator module into an airspeed (selection) module.

Overview

performs validity checks on airspeed sensor measurements
if no valid airspeed sensor is available, it estimates airspeed by calculating groundspeed minus windspeed
can estimate an airspeed scale factor and outputs an equivalent airspeed (EAS: indicated * scale ) (plus this scale can also be set via a parameter)
can also handle multiple airspeed sensors (currently not yet supported from a drivers side) and switches to valid sensor if failure on one is detected

The airspeed valid checks are taken from #11846 . They are here performed for every sensor and the code is located in the new airspeed selector module (currently the checks in commander are still there, but the idea is to only have them in the airspeed module in the end).

The output of the airspeed module is a airspeed validated topic, that gets logged but currently doesn't affect any control modules. All control modules still subscribe to the "old" airspeed topic. Of course this is supposed to be changed in the future (fuse all airspeed data through this module before using it).

Here's a rough drawing:

Description

The output of the module is a airspeed_validated topic, containing:

indicated airspeed (IAS)
equivalent airspeed (EAS)
true airspeed (TAS)
true_ground_minus_wind_m_s airspeed estimate
indicated_ground_minus_wind_m_s airspeed estimate
index of selected airspeed sensor (0: first sensor, 1: second sensor, -1: groundspeed-windspeed, -2: invalid airspeed)

For failure detection as well as for the estimation of a scale factor from IAS to EAS, per airspeed sensor a wind estimator is run, fusing sideslip and the airspeed measurement from the corresponding sensor. Another wind estimator is run to estimate the windspeed used for the groundspeed-windspeed airspeed estimate (this wind estimator is only fusing sideslip, no airspeed measurement).

Detailed diagram of new Module:
https://www.draw.io/#G1xrT_nPgBEt5Qh5dGYbX-CyKz9cROWnRT

Usage

Performing airspeed checks
The same/very similar checks as with the current checks in commander. Also takes the same parameter to set them (eg COM_TAS_FS_INNOV for innovation threshold). If the airspeed measurement for the sensor is declared invalid, the module outputs all airspeed(IAS, EAS, TAS) based on windpeed-groundspeed (if local position is valid) and sets the flag airspeed_sensor_measurement_valid to false. If local position is also invalid, then airspeed_valid is set to false.
Estimating airspeed scale
In-flight estimation of the scale factor (EAS = scale * IAS) is turned on by setting the parameter ARSP_SCALE_EST to 1. Then the factor is estimated inside the wind estimator (otherwise it is set to 1 inside the wind estimator). After some minutes of flight, the scale should have converged to the correct value, and the estimation can be switched off by setting ARSP_SCALE_EST to 0. By doing this, the estimated scale is written to the parameter ARSP_ARSP_SCALE and keeps being used to scale the airspeed. The value is limited to 0.75..1.25.
The scale can also be set manually by setting ARSP_ARSP_SCALE manually.

Test data / coverage

SITL (VTOL): Procedure:

after 57s, ARSPV_SCALE_EST is set to 1 (starting estimation of scale factor)
at 80s, ARSPV_SCALE_EST is set to 0 (ending estimation). Now the estimated value is printed in log (see below) and the parameter ARSPV_ARSP_SCALE is set to the estimated value (in this case 1, as in SITL we don't have sensor placement/blockage errors)
at 110s, the stall speed of the vehicle (COM_ASPD_STALL) is set much too high (50), thus triggering the load factor check (flying below stall speed). The airspeed sensor is detected as invalid, and the index switches to -1 (groundspeed-windspeed).
logs: https://review.px4.io/plot_app?log=e63b36b6-53d1-48ef-aaea-6151ad5487cd
Messages in QGC:

Flight test log: scale estimation, load factor and innovation check fail
Procedure:

enable scale estimation at 57s, disable at 119s --> estimated scale 1.03
at 150s, set stall speed to 30, triggers load factor fail --> switched to ground-windspeed
reset stallspeed to 6, airspeed declared valid again
repeat the the load factor triggering while doing automatic loitering (ca at 170s and 200s)
set innovation check levels (COM_TAS_FS_INNOV and COM_TAS_FS_INTG) to 0.1 -->innovation check failure is triggered as windspeed innovation are above these limits --> goes to ground-windspeed
-set the innovation check params to normal values --> airspeed is declared valid again

In these plots you can see how the airspeed_validated/true_airspeed is bounded between 12.5 and 14.2m/s while the artificial airspeed check failures are produced and it is thus sometimes calculated from groundspee-windspeed (when measurement not valid) or from pitot measurement with scale applied.

We also did the scale estimation while loitering (not in this flight), with much slower convergence. Should consider tuning the wind estimator to make if faster.

https://review.px4.io/plot_app?log=1c98f1a2-6d0e-4a79-b065-2f67b603228f

Bench test: Detection of data_missing check:
(for this test we connected 2 airspeed sensors and made each publish an airspeed topic)

Description: data_missing failure was triggered by unplugging the airspeed sensor with index 0. The airspeedValidator detects that and switches to index 1 (second sensor). Next all the airspeed sensors are plugged out, and the validator switches to index -2 (indicating no valid airspeed, as groundspeed-windspeed is not valid as performed indoor without GPS). Afterwards, the sensors are reconnected and the valid sensor index goes back to 0.

Further work
As mentioned, this PR only brings in the new module passively, without effect on any control modules. The idea is to get feedback and work e.g. on the tuning of the validity checks, and also to get it tested with higher winds (so far the max we tested it with was about 4 m/s). Follow up work is:

enable multiple airspeed sensors from a driver side (also consider calibration)
get rid of checks in commander and adapt commander to consider the airspeed_validated topic, and take action based on the validity flags of this topic (e.g. RTL when airspeed is invalid)
use the airspeed_validated topic in control modules (e.g. EAS replaces the old IAS for attitude controller, TAS from the validated topic for TECS/L1)

sfuhrer · 2019-06-27T17:37:35Z

@RomanBapst please have a first look

src/modules/commander/commander_params.c

RomanBapst

@sfuhrer I made a first pass. Will probably need a couple more iterations.

My suggestion for now is the following:

address the my review and push fixes to this branch if necessary
Clean up the history as discussed. (Check out a new branch based on master and then add the changes. Make separate commits for the library and the airspeed module).
Don't add the changes for multiple airspeed sensors in the sensors.cpp. Let's focus on merging the airdata module first. Also don't do any changes in the commander.

msg/airspeed_validated.msg

src/lib/AirspeedValidator/AirspeedValidator.cpp

src/modules/wind_estimator/wind_estimator_main.cpp

RomanBapst · 2019-07-01T09:02:43Z

src/modules/wind_estimator/wind_estimator_main.cpp

+			}
+		}
+
+		if (_valid_airspeed_index < 0 && !_vehicle_local_position_valid) {


@sfuhrer For now this is ok, but we need to figure out a way how to tell if the wind from the sideslip estimator is healthy.

@sfuhrer Also move this statement outside of the if (find_new_valid_index). If you have no airspeed sensors then the system will keep index =-1 even if local position is not valid.

src/modules/wind_estimator/wind_estimator_main.cpp

src/lib/AirspeedValidator/AirspeedValidator.hpp

sfuhrer · 2019-07-11T14:56:00Z

@tstastny would be cool if you could test the scale estimation and compare it to what you get. As I mentioned in the description it is currently quite slow, so you would have to have it enabled for a few minutes. Feel free to play around with the wind estimator tuning gains!

sfuhrer · 2019-07-11T14:59:41Z

@bkueng could you please review?

RomanBapst · 2019-08-08T14:57:37Z

@dagar Could you please make a suggestion regarding the few failing builds? This is ready to merge from our side.

dagar · 2019-08-08T15:08:20Z

@dagar Could you please make a suggestion regarding the few failing builds? This is ready to merge from our side.

remove the new/updated module from omnibus
add RTPS ids for the new messages - https://github.com/PX4/Firmware/blob/master/msg/tools/uorb_rtps_message_ids.yaml

sfuhrer · 2019-08-08T15:53:51Z

1. remove the new/updated module from omnibus

It is already removed from the omnibus. Probably the updated ecl takes up too much flash...anything else we could disable to make if fit on the omnibus @dagar @bkueng ?

add RTPS ids for the new messages - https://github.com/PX4/Firmware/blob/master/msg/tools/uorb_rtps_message_ids.yaml

Done.

… state (equivalent airspeed) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

This validator takes measurements from a single airspeed sensor and: -checks validity of measurement -can estimate an airspeed scale factor (estimation enabled with parameter setting) to accout for errors in airspeed due due placement of sensor Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

…getting low Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

…on, validation) module. This new airspeed module does: -runns an airspeed validator for every airspeed sensor present, which checks measurement validity and estimates an airspeed scale -selects another airspeed sensor if for the current one a failure is detected -estimates airspeed with groundspeed-windspeed if no valid airspeed sensor is present -outputs airspeed_validated topic Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

…lash after ecl update for airspeed selection module Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

…g after rebase Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

moreba1 · 2019-08-14T18:15:35Z

Can you dampen airspeed oscilations by averaging ?

sfuhrer · 2019-08-16T15:48:05Z

Can you dampen airspeed oscilations by averaging ?

What airspeed oscillations are your referring to, measurement noise? As it is used in low-level control loops it is usually required to have airspeed with as little delay as possible, and thus isn't filtered a lot. But maybe we could increase filtering a bit yes.

moreba1 · 2019-08-16T16:04:59Z

What airspeed oscillations are your referring to, measurement noise? As it is used in low-level control loops it is usually required to have airspeed with as little delay as possible, and thus isn't filtered a lot. But maybe we could increase filtering a bit yes.

yes , i mean measurement noise.

moreba1 · 2019-09-03T07:00:15Z

@sfuhrer
i had a ground test of px4 beta 1.10 and I changed ARSP_ARSP_SACLE to 2 , The measured speed did not change and did not double. does it need to reboot the autopilot ?

sfuhrer · 2019-09-03T14:24:27Z

a ground test of px4 b

Did you look at airspeed_validated.equivalent_airspeed? Because that's the one that should get scaled then (and from it then also airspeed_validated.true_airspeed)

moreba1 · 2019-09-03T17:54:43Z

airspeed_validated.equivalent_airspeed

where is it ? i can not find it in tlog.

sfuhrer · 2019-09-04T11:29:46Z

airspeed_validated.equivalent_airspeed

where is it ? i can not find it in tlog.

Are you sure you're flying 1.10? It's not in 1.9

moreba1 · 2019-09-05T08:49:21Z

Are you sure you're flying 1.10

I find it in ulog file . i tested again but airspeed_validated.equivalent_airspeed did not change by changing ARSP_ARSP_SCALE.

moreba1 · 2019-09-20T08:31:54Z

@sfuhrer Did you test this feature ?

dagar requested review from dagar and RomanBapst June 27, 2019 17:41

dagar added Admin: Enhancement (improvement) 💡 Fixed Wing 🛩️ Hybrid VTOL 🛩️🚁 Multirotor + Fixedwing! labels Jun 27, 2019

dagar added this to the Release v1.10.0 milestone Jun 27, 2019

dagar reviewed Jun 27, 2019

View reviewed changes

src/modules/commander/commander_params.c Outdated Show resolved Hide resolved

sfuhrer force-pushed the pr-airspeed_module branch from a7bad41 to 713394c Compare June 28, 2019 08:02

weekly-digest bot mentioned this pull request Jun 30, 2019

Weekly Digest (23 June, 2019 - 30 June, 2019) #12374

Closed

RomanBapst requested changes Jul 1, 2019

View reviewed changes

RomanBapst reviewed Jul 1, 2019

View reviewed changes

src/modules/wind_estimator/wind_estimator_main.cpp Outdated Show resolved Hide resolved

RomanBapst reviewed Jul 1, 2019

View reviewed changes

src/modules/wind_estimator/wind_estimator_main.cpp Outdated Show resolved Hide resolved

sfuhrer force-pushed the pr-airspeed_module branch 3 times, most recently from 561cda0 to 745254c Compare July 1, 2019 14:25

weekly-digest bot mentioned this pull request Jul 7, 2019

Weekly Digest (30 June, 2019 - 7 July, 2019) #12430

Closed

RomanBapst reviewed Jul 8, 2019

View reviewed changes

src/modules/wind_estimator/wind_estimator_main.cpp Outdated Show resolved Hide resolved

RomanBapst reviewed Jul 8, 2019

View reviewed changes

src/lib/AirspeedValidator/AirspeedValidator.hpp Outdated Show resolved Hide resolved

sfuhrer force-pushed the pr-airspeed_module branch from f4b6053 to eafbefb Compare July 10, 2019 07:47

sfuhrer force-pushed the pr-airspeed_module branch 2 times, most recently from 7db8865 to 7880e2e Compare July 11, 2019 09:31

sfuhrer marked this pull request as ready for review July 11, 2019 14:53

sfuhrer force-pushed the pr-airspeed_module branch from 69c6c06 to 66d9adc Compare August 7, 2019 16:38

RomanBapst force-pushed the pr-airspeed_module branch from 4d619f2 to 46cb548 Compare August 8, 2019 10:07

sfuhrer added 7 commits August 9, 2019 09:30

Airspeed Selector: changes in airspeed library to account for new EAS…

25f1227

… state (equivalent airspeed) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Increased stack of lp work queue as with new airspeed modudle it was …

218a4cf

…getting low Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

AirspeedSelector: added RTPS ID for airspeed_validated message

9e7c129

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Omnibus: take out local_position_estimator from build as was out of f…

a37a61e

…lash after ecl update for airspeed selection module Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Airspeed Selector: resolve conflics for wind estimate topic publishin…

cdf28ac

…g after rebase Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

sfuhrer force-pushed the pr-airspeed_module branch from 35f5136 to cdf28ac Compare August 9, 2019 07:49

RomanBapst merged commit 07895cd into PX4:master Aug 9, 2019

sfuhrer mentioned this pull request Aug 10, 2019

scaling airspeed value MS4525DO #12676

Closed

weekly-digest bot mentioned this pull request Aug 11, 2019

Weekly Digest (4 August, 2019 - 11 August, 2019) #12682

Closed

sfuhrer deleted the pr-airspeed_module branch August 12, 2019 10:05

sfuhrer mentioned this pull request Sep 3, 2019

Airspeed selector follow-up #12887

Merged

mrpollo mentioned this pull request Mar 16, 2020

v1.11.0 Release Notes #14402

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Airspeed validation & selection module #12353

Airspeed validation & selection module #12353

sfuhrer commented Jun 27, 2019 •

edited

Loading

sfuhrer commented Jun 27, 2019

RomanBapst left a comment

RomanBapst Jul 1, 2019

RomanBapst Jul 1, 2019

sfuhrer commented Jul 11, 2019 •

edited

Loading

sfuhrer commented Jul 11, 2019

RomanBapst commented Aug 8, 2019

dagar commented Aug 8, 2019

sfuhrer commented Aug 8, 2019 •

edited

Loading

moreba1 commented Aug 14, 2019

sfuhrer commented Aug 16, 2019

moreba1 commented Aug 16, 2019

moreba1 commented Sep 3, 2019 •

edited

Loading

sfuhrer commented Sep 3, 2019

moreba1 commented Sep 3, 2019 •

edited

Loading

sfuhrer commented Sep 4, 2019

moreba1 commented Sep 5, 2019

moreba1 commented Sep 20, 2019

Airspeed validation & selection module #12353

Airspeed validation & selection module #12353

Conversation

sfuhrer commented Jun 27, 2019 • edited Loading

sfuhrer commented Jun 27, 2019

RomanBapst left a comment

Choose a reason for hiding this comment

RomanBapst Jul 1, 2019

Choose a reason for hiding this comment

RomanBapst Jul 1, 2019

Choose a reason for hiding this comment

sfuhrer commented Jul 11, 2019 • edited Loading

sfuhrer commented Jul 11, 2019

RomanBapst commented Aug 8, 2019

dagar commented Aug 8, 2019

sfuhrer commented Aug 8, 2019 • edited Loading

moreba1 commented Aug 14, 2019

sfuhrer commented Aug 16, 2019

moreba1 commented Aug 16, 2019

moreba1 commented Sep 3, 2019 • edited Loading

sfuhrer commented Sep 3, 2019

moreba1 commented Sep 3, 2019 • edited Loading

sfuhrer commented Sep 4, 2019

moreba1 commented Sep 5, 2019

moreba1 commented Sep 20, 2019

sfuhrer commented Jun 27, 2019 •

edited

Loading

sfuhrer commented Jul 11, 2019 •

edited

Loading

sfuhrer commented Aug 8, 2019 •

edited

Loading

moreba1 commented Sep 3, 2019 •

edited

Loading

moreba1 commented Sep 3, 2019 •

edited

Loading