move hover_thrust_estimator to new module (mc_hover_thrust_estimator) #14182

dagar · 2020-02-18T21:07:07Z

This is a quick PR that moves the hover_thrust_estimator (#13981) lib and mc_pos_control integration into a new standalone module (now called mc_hover_thrust_estimator for MC grouping).

Pros

can be easily included or excluded as needed (flash, systems where it's not needed/wanted for whatever reason)
run at lower priority on the side
possibly run at a lower rate (decoupled from controllers and real-time requirements)
could be updated to operate in stabilized mode as well
parameter culling for non-MC builds (tiny benefit)

Cons

lost integration with MPC takeoff state
- possibly questionable coupling with the land detector (I added a reset() if landed or disarmed).
a bit more overhead (mainly flash per ModuleBase)

To be honest after having done this I could go either way on this one.

bresch

Thanks a lot! I'll make the required modifications and it should be good to go.

bresch · 2020-02-19T18:03:15Z

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

+		_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+	}
+
+	if (_armed && !_landed) {


We can add a check here to start the estimator only when the the drone is above 1m AGL or x seconds after !_landed.

bresch · 2020-02-19T18:14:20Z

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

+
+			_hover_thrust_ekf.predict(dt);
+
+			if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) {


We could even take the body thrust from the controller outputs and rotate it in local frame so the estimator will be able to run in all modes.

I was tempted to do that immediately, but thought it might be safer to first reproduce things as closely as possible outside of mc_pos_control.

Note: be aware of the optional battery thrust scaling (MC_BAT_SCALE_EN).

https://github.com/PX4/Firmware/blob/43a1257843173359d404debb7ce1d857f573c0a7/src/modules/mc_rate_control/MulticopterRateControl.cpp#L295-L310

bresch · 2020-02-24T21:41:16Z

The estimator module now constantly publishes an estimate even when the estimator isn't running (on ground) to be a proper substitute of MPC_THR_HOVER for the land detector and the position controller.

Tested in SITL with a wrong initial thrust estimate and changing the weight on the vehicle during flight:

bresch · 2020-02-25T15:50:37Z

src/modules/mc_pos_control/mc_pos_control_params.c

+ * (EXPERIMENTAL) Set true to use the value computed by the hover thrust estimator
+ *
+ * @boolean
+ * @max 0


@dagar Does that work? Since this is experimental, I would like to have to click on "force save" to enable it.

Let's change the category.

bresch · 2020-02-26T07:41:17Z

@PX4/testflights Could you please test this PR on a multicopter in altitude/position and auto mode please?

You would need to set MPC_USE_HTE to 1.
Make sure that you are ready to switch to stabilized/manual mode if something goes wrong.
This PR mostly affects the vertical thrust and the land detector.

dannyfpv · 2020-02-28T23:19:59Z

tested on pixhawk4 v5 f-450
Modes Tested*

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.

- Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoin activate RTL

Notes:
No issues noted, good flight in general.

Log:

https://review.px4.io/plot_app?log=38b3ef8f-1e04-4464-8220-c867c850645c

jorge789 · 2020-02-28T23:53:44Z

Tested on PixRacer V4

Tested Modes
Position Mode: Good
Altitude Mode:
Good.
Mission plan mode (automated): good.
RTL (Return To Land): Good.

Process
Arm and take off in position mode, after flying for approximately one minute, switch to Altitude mode and then to mission plan and then make sure that the vehicle follows all the waypoints as shown in QGC, after completing all waypoints and then switch to RTL.
Note:
MPC_USE_HTE parameter was changed from 0 to 1

Logs:
https://review.px4.io/plot_app?log=82ee3e29-5422-450b-8030-df6a93b81a88

https://review.px4.io/plot_app?log=f484d2ef-2e59-442e-aa05-c0ecff3fdf36

Tested on CUAV nano V5

Tested Modes
Position Mode: Good
Altitude mode:
Good.
Mission plan mode (automated): good.
RTL (Return To Land): Good.

Process
Arm and take off in position mode, after flying for approximately one minute, switch to Altitude mode and then to mission plan and then make sure that the vehicle follows all the waypoints as shown in QGC, after completing all waypoints and then switch to RTL.
Note:
MPC_USE_HTE parameter was changed from 0 to 1

Log:
https://review.px4.io/plot_app?log=939dd9d3-8ecb-485a-8144-4d7ee861020b

https://review.px4.io/plot_app?log=1cf3a958-3618-4a06-9ab5-51fd66f00f38

Junkim3DR · 2020-02-29T04:23:47Z

Tested on NXP FMUK66 v3

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Untested.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Logs
https://review.px4.io/plot_app?log=533e3823-11d2-4610-a0b4-b2343a64b5c2

Tested on Pixhawk 3 v4Pro

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Untested.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Logs
https://review.px4.io/plot_app?log=a9ef5fab-73a6-429e-81e0-41eadc901583

dagar · 2020-03-10T14:39:25Z

After a merge this is now fitting everywhere (for now at least).

The unit test is failing.

bresch · 2020-03-10T17:13:36Z

@dagar yes, and the failures are real. I'm currently fixing that

Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.

local_position.z is relative to the origin of the EKF while dist_bottom is above ground

bresch · 2020-03-11T17:20:45Z

@dagar Good to be merged now, I'll update the default tuning in a separate PR.

src/modules/mc_pos_control/mc_pos_control_params.c

src/modules/land_detector/MulticopterLandDetector.cpp

src/modules/land_detector/MulticopterLandDetector.h

src/modules/mc_pos_control/mc_pos_control_main.cpp

dagar requested a review from bresch February 18, 2020 21:07

bresch added the Multirotor 🛸 label Feb 18, 2020

bresch requested changes Feb 21, 2020

View reviewed changes

weekly-digest bot mentioned this pull request Feb 23, 2020

Weekly Digest (16 February, 2020 - 23 February, 2020) #14213

Closed

bresch force-pushed the pr-hte_module branch 2 times, most recently from 60eb339 to d240d34 Compare February 24, 2020 21:35

bresch marked this pull request as ready for review February 24, 2020 21:41

bresch previously approved these changes Feb 24, 2020

View reviewed changes

bresch dismissed their stale review via 7dc98a3 February 25, 2020 15:46

bresch force-pushed the pr-hte_module branch from d240d34 to 7dc98a3 Compare February 25, 2020 15:46

bresch force-pushed the pr-hte_module branch from 7dc98a3 to b03ba4d Compare February 25, 2020 15:47

bresch reviewed Feb 25, 2020

View reviewed changes

weekly-digest bot mentioned this pull request Mar 1, 2020

Weekly Digest (23 February, 2020 - 1 March, 2020) #14268

Closed

bresch force-pushed the pr-hte_module branch from 82e0673 to f2bbc1f Compare March 6, 2020 16:49

dagar and others added 5 commits March 11, 2020 18:19

move hover_thrust_estimator to new module (mc_hover_thrust_estimator)

8038c1e

MCHoverThrustEstimator: Always publish an estimate

5960d75

Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.

MC_HTE: unitialize with hover_thrust parameter

67a001c

MC_HTE: constrain hover thrust setter between 0.1 and 0.9

269fafe

MC_HTE: integrate with land detector and velocity controller

33f272f

MC_HTE: use altitude agl threshold to start the estimator

f2cb49d

local_position.z is relative to the origin of the EKF while dist_bottom is above ground

bresch force-pushed the pr-hte_module branch from 34d9fe3 to f2cb49d Compare March 11, 2020 17:19

dagar commented Mar 11, 2020

View reviewed changes

src/modules/mc_pos_control/mc_pos_control_params.c Outdated Show resolved Hide resolved

Update src/modules/mc_pos_control/mc_pos_control_params.c

607ca63

bresch previously approved these changes Mar 11, 2020

View reviewed changes