[WIP] Legacy auto manual replacement #9368

Stifael · 2018-04-25T08:26:42Z

This Pr is the follow up of #9253.
Eventually this PR will remove the entire legacy code of the multicopter position control.

LorenzMeier · 2018-04-29T15:47:00Z

@RomanBapst Could you please review? Given the current flash limitations, I think we will make a full switch.

Stifael · 2018-05-04T09:11:56Z

About this PR:
This PR separates the legacy mc_pos_control_main into three sections:

FlightTask
mc_pos_control_main
PositionControl

FlightTask

The details about FlightTask were already covered in other PRs and discussions (for instance here).
To keep it short: FlightTask is a framework library that is vehicle INDEPENDENT for generating kinematic SETPOINTS (position-, velocity-, acceleration- (not supported yet), thrust-, yaw- and yawspeed-setpoints).
The FlightTaks library can be found here.

Flight-mode specific functionality were moved from mc_pos_control_main to a new task within FlightTask. The tasks are the following:

FlightTaskManualStabilized replaces the manual attitude mode
FlightTaskManualAltitude replaces the manual altitude mode without smoothing
FlightTaskManualAltitudeSmooth replaces the manual altitude mode with smoothing
FlightTaskManualPosition replaces the manual position control mode without smoothing
FlightTaskManualPositionSmooth replaces the manual position control mode with smoothing
FlightTaskAutoLine replaces the auto logic
FlightTaskAutoFollowMe replaces the follow-me logic
FlightTaskOffboard replaces the offboard logic

All tasks should be identical to the legacy code except with some changes to make it better fit into the FlightTask-framework:

Terrain-following:
- If terrain-following is enabled, instead of overwriting the velocity/position estimate the velocity/position setpoints are varied to keep the desired distance to ground.
Follow-me:
- the logic here was removed because in the legacy code there is no support for velocity and position setpoints simultaneously. However, in the code section above it sets velocity setpoints without setting the boolean flag run_pos_control to false and therefore these velocity setpoints will never be used.
speed limit downwards/upwards close to home altitude: In the legacy code, the speed in vertical direction during ANY mode is adjusted based on the relative altitude to the home-altitude. This functionality is still missing for manual controlled mode.
offboard lock: in the legacy code if the velocity setpoint is below a certain threshold, a position lock is applied. However, this is really a hack and nothing more. If the user wants to lock position, then a position setpoint can be sent.

PositionControl

This class contains the CORE-position/velocity controllers. The inputs are the position-, velocity-, acceleration- (not supported), thrust-, yaw- and yawspeed setpoints and outputs a thrust vector and yaw-/yawspeed-septoint.

mc_pos_control_main

This module is basically the interface between FlightTask and PositionControl. It makes adjustments to FlightTask setpoints based on vehicle state. For instance, it enables a smooth-takeoff, it keeps throttle low when being landed, it adjusts the thrust setpoint based on the landdetector state and so on. It also converts the thrust and yaw setpoint from PositionControl to a desired attitude and publishes it.

Flight-tests

So far I have tested the following modes:

SITL terrain following
SITL follow-me
SITL offboard position setpoints
SITL/HITL and OUTDOOR stabilized-, altitude- (without smoothing), position- (without smoothing), auto- mode.
SITL altitude and position mode with smoothing
SITL flow

It would be great if we can start to collect more flight hours for all different modes. When testing position and altitude mode, please keep in mind that NO smoothing is active. This is on purpose to make it easier to track down any unexpected bug. As I mentioned above, in any manual controlled mode the speed downwards/upwards close to ground is only limited by the global limits MPC_Z_VEL_MAX_DN/UP.

Conventions

I started to use a different style method for parameters and comments. It is not consistent yet and If I am the only one who sees the benefit in it, then I will revert it of course.

comments

the doxygen style that I started to use is the one already present in most modules:

for methods

/**
 *  Short description.
 *  Long desrciption
 *  @param
 *  @return
 */

for member variables

/**< bla bla */

comments in the code:

// bla bla
// bla bla

parameter naming

Instead of creating a name for a parameter, I just reused the meta-data parameter name. For instance, the parameter within a module that uses the MPC_XY_CRUISE parameter would be called MPC_XY_CRUISE as well. In my opinion, this has the following benefits:

no need to invent a new name
a parameter will have the same name in any module that uses that parameter
a clear distinction between a class member parameter (which usually should not be modified) and a class member variable that changes regularly (such as a setpoint)
from the prefix it is clear which module is the owner of the parameter
it is simpler to look up the full description (https://dev.px4.io/en/advanced/parameter_reference.html)
less prone to errors during coding
saves time during coding and during reading the code

Independent of this PR, I think it could be highly beneficial if we have more style related guidelines which are not covered by astyle. I can open a new issue to discuss possible code guidelines if desired.

Currently this branch fails because of:
../../src/systemcmds/tests/test_controlmath.cpp:106:1: internal compiler error: in trunc_int_for_mode, at explow.c:55

I saw that this error has popped up in the past already (https://github.com/PX4/Firmware/blob/master/src/systemcmds/tests/test_matrix.cpp#L628-L629).

Stifael · 2018-05-04T09:21:33Z

the ci fails for one of the unit-tests. In the worst case I can just remove the unit test to make the ci pass

dagar · 2018-05-04T12:54:20Z

the ci fails for one of the unit-tests. In the worst case I can just remove the unit test to make the ci pass

It's a compiler issue, if you drill down into ControlMathTest::testPrioritizeVector() it's likely only 1 or 2 offending lines.

Stifael · 2018-05-04T14:51:13Z

it's likely only 1 or 2 offending lines

I found now the line that caused the compilation problem.

MaEtUgR · 2018-05-14T13:18:31Z

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

+		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+		int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch;
+
+		if (!_constraints.landing_gear) {


I think _evaluateSticks() should only get the gear switch state and all the logic executing on it should be in a different function, here _applyGearSwitch().

MaEtUgR · 2018-05-16T08:41:53Z

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

+
+	} else {
+		// altitude based on locale coordinate system
+		_dist_to_ground_lock = NAN; // reset boolean since not used


It's not a boolean...

MaEtUgR · 2018-05-16T09:24:12Z

Altitude mode takeoff and altitude hold near the floor is sadly broken. I had the following cases tested with make posix_sitl_default gazebo_iris_opt_flow now:

Doesn't take off at all in altitude mode
Does takeoff immediately after arming to the minimum altitude for a usable range sensor even if the sticks didn't command anything yet
Everything works

I'm trying to find out what the root cause is but it needs to have something to do with _respectMinAltitude() introduced in 51ea83d#diff-5eabb1783ad4b7c01186be612b61631bR86 and how the vehicle is lock idle on the ground as long as there's no takeoff commanded.

EDIT:
I think I found the problem for the automatic jump without stick input:

Under some circumstances which not always happen in SITL on my PC all of these conditions are true 51ea83d#diff-5eabb1783ad4b7c01186be612b61631bR89 and the position hold setpoint jumps to the minimum distance sensor range altitude
The position controller module decides to not be idle anymore as soon as there's a position or velocity setpoint above a certain thershold https://github.com/PX4/Firmware/pull/9368/files#diff-fc77c3ef569029d45764664c75d4b0c1R647

Still doesn't explain why somethimes I cannot take off with my SITL. I'll try to reproduce with some conditions hardcoded like a unit test...

EDIT2:
I found the reason why it doesn't take off at all:
The thrust setpoint vector which gets to the evaluation here: https://github.com/PX4/Firmware/pull/9368/files#diff-d241541147a774870439717a5b9145e4R107 still contains NANs for x and y in the optical flow and altitude mode case.

EDIT3:
It doesn't take off because:
The position controller runs through velocity and position control at any time and if the states are not available/valid they get set to NAN here: https://github.com/PX4/Firmware/pull/9368/files#diff-fc77c3ef569029d45764664c75d4b0c1R389 and this NAN propagates through the controller to the output of x and y and the thrust length() calculation here https://github.com/PX4/Firmware/pull/9368/files#diff-d241541147a774870439717a5b9145e4R107 is NAN and no thurst gets executed.

I didn't have a valid local position with flow because my computer ran the simulation too slow and the data didn't pass the checks but the same problem applies if you loose your last position estimate source which is usually GPS. -> @Stifael That's a problem.

MaEtUgR · 2018-05-16T11:26:58Z

msg/vehicle_constraints.msg

+
+float32 yawspeed	# in radians/sec
+float32 speed_xy	# in meters/sec
+float32 speed_up	# in meters/sec


the setpoint which gets constrained is called vz so why not call the constraint constraint.vz_up?

MaEtUgR · 2018-05-16T11:34:19Z

src/modules/mc_pos_control/mc_pos_control_main.cpp

-
-	matrix::Dcmf _R_setpoint;
+	hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
+	static constexpr uint64_t IDLE_BOFORE_TAKEOFF_TIME_US =


Why should we wait for a hardcoded 2.5 seconds until the position controller realizes that the vehicle was armed?

MaEtUgR · 2018-05-16T12:06:59Z

src/modules/mc_pos_control/mc_pos_control_main.cpp

-		_vel_sp_prev(0) = _vel(0);
-		_vel_sp_prev(1) = _vel(1);
+			// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
+			_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());


thrustToAttitude() holds its own copy of the complete attitude setpoint message. That overhead can be saved by reseting the outside attitude setpoint message and passing a reference to the function such that it can fill the fields.

MaEtUgR · 2018-05-16T12:14:14Z

src/modules/mc_pos_control/Utility/ControlMath.cpp

 	matrix::Eulerf euler = R_sp;
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);

-	/* fill and publish att_sp message */
+	// fill and publish att_sp message


it's actually not published here...

Stifael · 2018-05-22T09:43:18Z

@MaEtUgR thanks for the testing and comments.
For future collaboration, I think it is most efficient if contributors are pushing directly onto this branch for smaller issues/fixes and open a PR for more involved issues.
I will now address your comments. In addition, I will add code to protect against invalid states that can occur due to commander-position-control race condition.

dagar · 2018-05-25T04:53:18Z

Rebase?

…ctive and below minimum distance to ground.

…not valid

…and sport

…hich corresponds to position lock.

…is enabled

Stifael · 2018-06-05T08:57:06Z

@santiago3dr @Avysuarez thanks again for testing.
The Stabilized takeoff should now be fixed. If there is still an issue, please can you try to the same with Manual takeoff. Although Manual should be the same as Stabilized, it is still worth to test.

Would it also be possible to test position control with flow enabled?

Avysuarez · 2018-06-05T20:51:45Z

couple flights with the new commit .
pixhawk 4 (V5).
https://review.px4.io/plot_app?log=25a00713-f2c9-4f85-88f5-143d586d93d8
took-off in stabilized mode and the throttle respond correctly
pixhawk 2.1 (V3).
https://review.px4.io/plot_app?log=56624300-632a-4aa0-8108-3a8bb8be1b56
took-off in stabilized mode and the throttle respond correctly

This is currently required because it can cause conflit with offboard attitude control where attitude setpoint are sent from mavlink_receiver and from mc_pos_control simultaneously.

MaEtUgR · 2018-06-20T06:36:00Z

I'm starting to rebase with a similar scheme like the flight task introduction pr from last time. We (@Stifael @RomanBapst) dicussed this yesterday in a call. The functionality should not change except for the yawspeed setpoint rescaling (see https://github.com/YUNEEC/Firmware/pull/2069/commits/500ffe5a622f85c0de409f0c9da233b8b5be3d7b for reference) which I will point out afterwards.

MaEtUgR · 2018-06-21T07:16:52Z

I pushed first rebase work to a new branch https://github.com/PX4/Firmware/tree/pr-flight-task-iteration to reduce confusion because the it's in terms of history quite different. The principle is the same as with the first flight task pr: I rebase all the files except for mc_pos_control_main.cpp because that is the file generating the huge pile of conflicts. Now I'll adjust that file to match the diff and not loose any of the changes in between.

MaEtUgR · 2018-06-21T15:11:48Z

Rebase pr with only small things open is now here: #9731

Stifael mentioned this pull request Apr 25, 2018

[WIP] Flight task auto #9253

Closed

1 task

Stifael force-pushed the Legacy-Auto-Manual-replacement branch from a3ff249 to 1bf9803 Compare April 25, 2018 09:23

Stifael force-pushed the Legacy-Auto-Manual-replacement branch 4 times, most recently from 64c4e35 to b261530 Compare May 1, 2018 17:09

MaEtUgR self-requested a review May 2, 2018 11:40

Stifael force-pushed the Legacy-Auto-Manual-replacement branch from 27c5079 to d04b1ae Compare May 4, 2018 05:31

Stifael added Admin: Enhancement (improvement) 💡 Multirotor 🛸 offboard Admin: Cleanup (housekeeping) 🧹 labels May 4, 2018

MaEtUgR reviewed May 14, 2018

View reviewed changes

MaEtUgR reviewed May 16, 2018

View reviewed changes

Stifael force-pushed the Legacy-Auto-Manual-replacement branch 2 times, most recently from 91205bb to 7952da6 Compare May 22, 2018 16:32

Stifael mentioned this pull request May 23, 2018

Optical Flow Use Improvements - supersedes #9435 #9509

Closed

Stifael added 20 commits June 5, 2018 10:54

FlightTaskManualAltitude: do terrain following when distance sensor a…

0720fe5

…ctive and below minimum distance to ground.

mc_pos_control: reset arm_hysteresis when disarmed

b0ee732

FlightTaskAuto: require valid position in 3D

983d1db

FlightTaskAuto: replace target with destination for checks

75c217e

mc_pos_control: comment fix

3840348

PositionControl: set vel_dot to 0 if NAN

08aca36

PositionControl: saturate thrust integral

3ac3f39

mc_pos_control: update controlblock derivative with 0 if velocity is …

a6ed830

…not valid

mc_pos_control: don't control position in xy during smooth takeoff

ee250d4

mc_pos_control: parameter for position control mode: default, smooth …

a4c258e

…and sport

ManualSmoothing: direction change with maximum acceleration

b91ff4b

ManualSmoothingXY: remove empty lines

0bcc818

FlightTaskAuto: take care of case when triplet.lat/lon are invalid, w…

fcf6d44

…hich corresponds to position lock.

FlightTaskAuto: reset lock position to NAN

7e2d338

mc_pos_control: start stablizied flightthask if manual OR stabilized …

fff6db0

…is enabled

FlightTasks: create struct that contains FlightTask and FlightTaskIndex

0b23cc4

FlightTasks: construct all tasks in constructor

9cf7cdb

mc_pos_control: move small code chunck to make it more readable

3e00f0e

mc_pos_control_param: activate manual smoothign by default

4bb36c4

mc_pos_control: always publish setpoint

45d5767

Stifael force-pushed the Legacy-Auto-Manual-replacement branch from d9e1e79 to 45d5767 Compare June 5, 2018 08:54

mc_pos_control: publish attitude only if there is a flight-task running.

9eb2dec

This is currently required because it can cause conflit with offboard attitude control where attitude setpoint are sent from mavlink_receiver and from mc_pos_control simultaneously.

MaEtUgR mentioned this pull request Jun 21, 2018

FlightTask Introduction Replace Legacy Multicopter Position Controller #9731

Merged

1 task

LorenzMeier closed this in #9731 Jul 20, 2018

MaEtUgR deleted the Legacy-Auto-Manual-replacement branch August 29, 2018 15:41

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] Legacy auto manual replacement #9368

[WIP] Legacy auto manual replacement #9368

Stifael commented Apr 25, 2018 •

edited by AuterionWrikeBot

Loading

LorenzMeier commented Apr 29, 2018

Stifael commented May 4, 2018 •

edited

Loading

Stifael commented May 4, 2018 •

edited

Loading

dagar commented May 4, 2018

Stifael commented May 4, 2018

MaEtUgR May 14, 2018 •

edited

Loading

MaEtUgR May 16, 2018

MaEtUgR commented May 16, 2018 •

edited

Loading

MaEtUgR May 16, 2018

MaEtUgR May 16, 2018

MaEtUgR May 16, 2018

MaEtUgR May 16, 2018

Stifael commented May 22, 2018

dagar commented May 25, 2018

Stifael commented Jun 5, 2018

Avysuarez commented Jun 5, 2018

MaEtUgR commented Jun 20, 2018 •

edited

Loading

MaEtUgR commented Jun 21, 2018

MaEtUgR commented Jun 21, 2018

[WIP] Legacy auto manual replacement #9368

[WIP] Legacy auto manual replacement #9368

Conversation

Stifael commented Apr 25, 2018 • edited by AuterionWrikeBot Loading

LorenzMeier commented Apr 29, 2018

Stifael commented May 4, 2018 • edited Loading

FlightTask

PositionControl

mc_pos_control_main

Flight-tests

Conventions

comments

parameter naming

Stifael commented May 4, 2018 • edited Loading

dagar commented May 4, 2018

Stifael commented May 4, 2018

MaEtUgR May 14, 2018 • edited Loading

Choose a reason for hiding this comment

MaEtUgR May 16, 2018

Choose a reason for hiding this comment

MaEtUgR commented May 16, 2018 • edited Loading

MaEtUgR May 16, 2018

Choose a reason for hiding this comment

MaEtUgR May 16, 2018

Choose a reason for hiding this comment

MaEtUgR May 16, 2018

Choose a reason for hiding this comment

MaEtUgR May 16, 2018

Choose a reason for hiding this comment

Stifael commented May 22, 2018

dagar commented May 25, 2018

Stifael commented Jun 5, 2018

Avysuarez commented Jun 5, 2018

MaEtUgR commented Jun 20, 2018 • edited Loading

MaEtUgR commented Jun 21, 2018

MaEtUgR commented Jun 21, 2018

Stifael commented Apr 25, 2018 •

edited by AuterionWrikeBot

Loading

Stifael commented May 4, 2018 •

edited

Loading

Stifael commented May 4, 2018 •

edited

Loading

MaEtUgR May 14, 2018 •

edited

Loading

MaEtUgR commented May 16, 2018 •

edited

Loading

MaEtUgR commented Jun 20, 2018 •

edited

Loading