MC stabilized mode: move from mc_pos_control to mc_att_control #10805

bkueng · 2018-11-05T10:03:59Z

Handling the Stabilized fight mode in mc_pos_control adds unnecessary dependencies. This recently became apparent that Stabilized would not work correctly anymore if there was not position published.
So this PR moves the complete handling into mc_att_control.
Additional benefits:

ability to stop the position controller completely in Stabilized.
Easier code path for Stabilized as there are less transformations. I find this important especially since Stabilized is often used as a fail-safe flight mode.

Testing

I did extensive testing in SITL and HITL, especially mode changes.
Flight logs:
- https://logs.px4.io/plot_app?log=50ef926e-6332-4dc7-aab6-b54a572518cb
- https://logs.px4.io/plot_app?log=a7ae3482-7174-4ce0-bae4-1180441e6f42
The code is mostly the same as pre-flight task, so it already comes with further test coverage.

Additional changes

Fix for rattitude (fa4f38f)
Disable Flight tasks if none of them is supposed to run (Fixes Flight Tasks: velocity & attitude setpoints in Acro #10561)
Use correct maximum tilt angle parameter for altitude mode
Restore the pre-flight-task yaw behavior (yaw offset) in Stabilized, which is more intuitive to fly

…nning Previously when switching e.g. from stabilized from acro, the stabilized flight task kept running and publishing setpoints. Luckily it caused no problems, but the log showed arbitrary attitude setpoints.

use nullptr for instance to make sure we always get the first instance.

- better in terms of dependencies: - the position controller code depended on position topics for yaw - mc_pos_control does not have to be run for Stabilized mode - the code path is much simpler, and thus less error prone. This is especially important since Stabilized is often used as a fail-safe flight mode.

When switching back from rate to attitude control, the code depended on a vehicle_control_mode topics update, but the publication frequency of that is low. So the switch was noticeably delayed.

MaEtUgR

Thoughts on the first glance. Does the weather vane feature need to run in stabilized?
I might have some more questions after testing and studying completely.

MaEtUgR · 2018-11-08T08:07:25Z

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

-	bool ret = FlightTaskManualStabilized::activate();
-	_thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity
+	bool ret = FlightTaskManual::activate();
+	_yaw_setpoint = _yaw;


The _yaw_setpoint should be set to NAN on activation because we don't know if the vehicle has a yawspeed upon mode switch. If the vehicle has no yawspeed and the commanded yaw_speed is 0 it will by design very quickly switch to yaw lock which does a _yaw_setpoint = _yaw once. But if the vehicle has a yawspeed it should first brake down and not start in yaw lock.

MaEtUgR · 2018-11-08T09:05:42Z

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

-	// reuse same scaling as for stabilized
-	FlightTaskManualStabilized::_scaleSticks();
+	/* Scale sticks to yaw and thrust using
+	 * linear scale for yaw and piecewise linear map for thrust. */


the comment doesn't fit anymore in this spot
my suggestion:

// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed

MaEtUgR · 2018-11-08T09:11:12Z

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
-					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P
+					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,


not related but /**< position controller altitude propotional gain */

MaEtUgR · 2018-11-08T10:25:10Z

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -334,7 +334,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
 *
 * @unit deg
 * @min 0.0
- * @max 90.0
+ * @max 170.0


feeling adventurous? 😄 It's a good idea to check corner angles (first in simulation!) to verify the implementation. 180 should theoretically also work or do we run into gimbal lock problems with the setpoint generation?

Yes, see the commit message: 78da9ff. 180 is fine setpoint-wise, beyond that the yaw inverts. I wanted to account for overshoots, but I realize now that even with overshoots the attitude should be tracked correctly up to 180 degrees setpoints.

I tested it again and 180 works fine. Interestingly even above that works now. I increased the limit to 180.

MaEtUgR · 2018-11-08T10:31:41Z

src/modules/mc_pos_control/mc_pos_control_main.cpp

-			if (_wv_controller->weathervane_enabled() && !(_control_mode.flag_control_manual_enabled
-					&& !_control_mode.flag_control_position_enabled)) {
+			if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled
+					|| _control_mode.flag_control_position_enabled)) {


!(A && !B) -> (!A || B) ✔️
according to de morgan

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

src/modules/mc_pos_control/mc_pos_control_main.cpp

RomanBapst

Looks good in general.

Avysuarez · 2018-11-08T22:06:17Z

Tested on pixhawk 4. Good flight, no issues.
https://review.px4.io/plot_app?log=9cdf5b96-d563-4163-8dee-9d0066add5e8

This is especially useful for testing (the vehicle must behave correctly even at high tilt angles).

… task

Also update some comments. In case we activate the task and don't have a locked yaw, we should initialize the yaw setpoint with NAN to avoid any abrupt changes.

… NaN for FW

bkueng · 2018-11-09T07:32:04Z

All comments addressed.

during a transition - this regression most likely comes from PR #10805 Signed-off-by: Roman <bapstroman@gmail.com>

during a transition - this regression most likely comes from PR PX4#10805 Signed-off-by: Roman <bapstroman@gmail.com>

This was only necessary for stabilized mode before #10805. The unit length thrust setpoint will anyways not be available anymore soon because it gets replaced with the acceleration setpoint in m/s².

bkueng requested review from MaEtUgR, RomanBapst and Stifael November 5, 2018 10:04

bkueng added 7 commits November 7, 2018 19:45

fix mc_pos_control: disable flight tasks if none of them should be ru…

eb3934f

…nning Previously when switching e.g. from stabilized from acro, the stabilized flight task kept running and publishing setpoints. Luckily it caused no problems, but the log showed arbitrary attitude setpoints.

mc_pos_control: print active flight task for 'mc_pos_control status' cmd

60a32b7

mc_pos_control: simplify weathervane logic

862369f

mc_att_control: use orb_publish_auto

5181378

use nullptr for instance to make sure we always get the first instance.

FlightTask.hpp: minor code cleanup

0d55dbc

mc_att_control: fix for Rattitude mode

40836bf

When switching back from rate to attitude control, the code depended on a vehicle_control_mode topics update, but the publication frequency of that is low. So the switch was noticeably delayed.

bkueng force-pushed the mc_stabilized_to_mc_att_control branch from ef8eeaf to 78da9ff Compare November 7, 2018 18:45

bkueng requested a review from a team November 7, 2018 19:51

MaEtUgR reviewed Nov 8, 2018

View reviewed changes

RomanBapst reviewed Nov 8, 2018

View reviewed changes

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp Outdated Show resolved Hide resolved

RomanBapst reviewed Nov 8, 2018

View reviewed changes

src/modules/mc_pos_control/mc_pos_control_main.cpp Show resolved Hide resolved

RomanBapst requested changes Nov 8, 2018

View reviewed changes

bkueng added 5 commits November 9, 2018 08:20

MPC_MAN_TILT_MAX: increase max limit from 90 to 180 degrees

b60f8dc

This is especially useful for testing (the vehicle must behave correctly even at high tilt angles).

flight tasks: move weather vane yaw handler from Altitude to Position…

9711649

… task

FlightTaskManualAltitude: initialze yaw setpoint with NAN

64213a0

Also update some comments. In case we activate the task and don't have a locked yaw, we should initialize the yaw setpoint with NAN to avoid any abrupt changes.

mc_pos_control: fix task activation failure for Transition

4d43dba

vehicle_attitude_setpoint: add comment that attitude sp angles can be…

454786c

… NaN for FW

bkueng force-pushed the mc_stabilized_to_mc_att_control branch from 78da9ff to 454786c Compare November 9, 2018 07:27

RomanBapst approved these changes Nov 12, 2018

View reviewed changes

bkueng merged commit 9a9c17b into master Nov 13, 2018

bkueng deleted the mc_stabilized_to_mc_att_control branch November 13, 2018 08:30

MaEtUgR mentioned this pull request Nov 14, 2018

Landing gear improvement #10842

Merged

RomanBapst added a commit that referenced this pull request Dec 8, 2018

mc_att_control: don't generate attitude setpoint for tailsitters

d9cc4df

during a transition - this regression most likely comes from PR #10805 Signed-off-by: Roman <bapstroman@gmail.com>

RomanBapst mentioned this pull request Dec 8, 2018

mc_att_control: don't generate attitude setpoint for tailsitters during transitions #11007

Closed

RomanBapst added a commit that referenced this pull request Dec 10, 2018

mc_att_control: don't generate attitude setpoint for tailsitters

8898a64

during a transition - this regression most likely comes from PR #10805 Signed-off-by: Roman <bapstroman@gmail.com>

RomanBapst added a commit that referenced this pull request Dec 11, 2018

mc_att_control: don't generate attitude setpoint for tailsitters

30c9c8b

during a transition - this regression most likely comes from PR #10805 Signed-off-by: Roman <bapstroman@gmail.com>

RomanBapst added a commit that referenced this pull request Jan 22, 2019

mc_att_control: don't generate attitude setpoint for tailsitters

a042e40

during a transition - this regression most likely comes from PR #10805 Signed-off-by: Roman <bapstroman@gmail.com>

sfuhrer pushed a commit to sfuhrer/Firmware that referenced this pull request Mar 19, 2019

mc_att_control: don't generate attitude setpoint for tailsitters

96fc01b

during a transition - this regression most likely comes from PR PX4#10805 Signed-off-by: Roman <bapstroman@gmail.com>

MaEtUgR mentioned this pull request Oct 9, 2019

FlightTasks: disable failsafe descend on RC loss #13125

Closed

MaEtUgR mentioned this pull request Oct 16, 2019

Position publish disarmed attitude #13210

Merged

8 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

MC stabilized mode: move from mc_pos_control to mc_att_control #10805

MC stabilized mode: move from mc_pos_control to mc_att_control #10805

bkueng commented Nov 5, 2018 •

edited by AuterionWrikeBot

Loading

MaEtUgR left a comment

MaEtUgR Nov 8, 2018

MaEtUgR Nov 8, 2018

MaEtUgR Nov 8, 2018

MaEtUgR Nov 8, 2018

bkueng Nov 8, 2018

bkueng Nov 9, 2018

MaEtUgR Nov 8, 2018

RomanBapst left a comment

Avysuarez commented Nov 8, 2018

bkueng commented Nov 9, 2018

MC stabilized mode: move from mc_pos_control to mc_att_control #10805

MC stabilized mode: move from mc_pos_control to mc_att_control #10805

Conversation

bkueng commented Nov 5, 2018 • edited by AuterionWrikeBot Loading

Testing

Additional changes

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR Nov 8, 2018

Choose a reason for hiding this comment

MaEtUgR Nov 8, 2018

Choose a reason for hiding this comment

MaEtUgR Nov 8, 2018

Choose a reason for hiding this comment

MaEtUgR Nov 8, 2018

Choose a reason for hiding this comment

bkueng Nov 8, 2018

Choose a reason for hiding this comment

bkueng Nov 9, 2018

Choose a reason for hiding this comment

MaEtUgR Nov 8, 2018

Choose a reason for hiding this comment

RomanBapst left a comment

Choose a reason for hiding this comment

Avysuarez commented Nov 8, 2018

bkueng commented Nov 9, 2018

bkueng commented Nov 5, 2018 •

edited by AuterionWrikeBot

Loading