Feature/First iteration on MC failsafe #5863

anton-matosov · 2016-11-15T09:29:27Z

Add failsafe tune
Fix LED blinking for Pixracer
Pass rc_loss_mode instead of simple bool flag
Add setting of failsafe PWM outputs in case of force_failsafe

LorenzMeier · 2016-11-15T09:37:11Z

Nice!

LorenzMeier · 2016-11-15T10:51:43Z

@kd0aij Can you test?

kd0aij · 2016-11-15T16:49:07Z

What is rc_loss_mode supposed to do? I guess this should fix the flight termination bug I mentioned in the RC loss PR... need to reference it from here.

kd0aij · 2016-11-16T00:44:01Z

what about #5388?

anton-matosov · 2016-11-16T01:43:50Z

Rc_loss_mode is enum that's is not used right now, but it should define behavior of RC loss failsafe action. With current code it switches to "land" mode, and there is no way to force termination
I took a brief look at your PR and it does some of what I am missing right now regarding applying failsafe during termination. But it also does some stuff a bit brute force (e.g. applying lockdown from commander state machine)
I will incorporate your fixes into my PR, so it will be a complete solution (right now it drops output to minimum pwm upon termination)

julianoes · 2016-11-16T06:05:46Z

src/modules/commander/commander.cpp

@@ -2881,7 +2881,7 @@ int commander_thread_main(int argc, char *argv[])
 						       _mission_result.stay_in_failsafe,
 						       &status_flags,
 						       land_detector.landed,
-						       (rc_loss_enabled > 0),
+						       rc_loss_mode,


I like this change but does it change the behaviour at all like this?

Note that the same param is used again in the navigator which is really quite ugly:
https://github.com/PX4/Firmware/blob/master/src/modules/commander/commander.cpp#L1276
https://github.com/PX4/Firmware/blob/master/src/modules/navigator/navigator_main.cpp#L162

No, it doesn't, yet.
I have never seen Navigator before. What is it used for?
It looks like it has very similar logic as commander for data/rc links and GPS, but handles it in its own way.

In my opinion, the handling of RC and DL loss should move back from the navigator to the state machine (as done here with offboard actions: #4655)

The navigation states itself can still implement complex logic as it is the case for things that got added for OBC 2014, but no separate/duplicate state switching

I read more commander and navigator code and now feel the difference of their responsibilities.
(Please correct me if I am wrong)
Navigator simply performs 'pieces' of the autonomous navigation, it doesn't make a decision.
Commander is that one that maintains the global state of the vehicle based on params, user and sensor inputs and makes all the decisions of what action to take in particular situation.

If my understanding is correct then I am 100% agree that rc_loss_mode should be handled in commander (SM helper), not navigator.

My only concern is that SM helper keeps growing and already has a big chunk of copy-pasted code. I feel a bit worried of breaking things by doing change to RC loss like this without having really good understanding and having unit tests covering the logic.

@anton-matosov I think you got it right, that's very much the way it was designed. If you're concerned about breaking the state machine helper the way to resolve that would be to add tests to and then extend or refactor it.

anton-matosov · 2016-11-17T02:18:07Z

Ok. I think that's what I am going to do then. I will write tests at least for code I am going to modify for SM Helper and navigator. Are there any existing tests?

dagar · 2016-11-17T02:25:52Z

@anton-matosov state machine tests here - https://github.com/PX4/Firmware/blob/master/src/modules/commander/commander_tests/state_machine_helper_test.cpp

Nothing really for navigator, unless you can fit it into a mission test. https://github.com/PX4/Firmware/tree/4453e4201b7a245cff52beeb38a293161aea4c48/integrationtests/python_src/px4_it/mavros

AndreasAntener · 2016-11-17T06:21:10Z

@anton-matosov what do you plan to change in the state machine helper? Because I'm reworking this at the moment and implemented a different way of how the failures are handled (more comprehensive), also adding unit tests.

anton-matosov · 2016-11-17T07:31:17Z

@AndreasAntener I am planning to refactor RC and Datalink loss code to get rid of the copy-paste and migrate RC/Data loss mode switching to commander out from Navigator the same way offboard loss modes are handled.
Also I will extend loss mode to force termination mode. (that is what I need for FPV racers)

anton-matosov · 2016-11-17T18:28:39Z

@AndreasAntener I am planning to refactor RC and Datalink loss code to get rid of the copy-paste and migrate RC/Data loss mode switching to commander out from Navigator the same way offboard loss modes are handled.
Also I will extend loss mode to force termination mode. (that is what I need for FPV racers)

anton-matosov · 2016-11-20T09:19:11Z

@AndreasAntener are you done with your refactoring of the commander's SM?
Which fork/branch are you working in? (so I can merge in your changes and keep going)

AndreasAntener · 2016-11-20T10:32:55Z

@anton-matosov I think we should separate the refactoring from your functionality change (termination handling). I'll push a branch in a sec that contains the move from navigator, after I can build upon that again.

AndreasAntener · 2016-11-20T10:53:59Z

@anton-matosov added a PR for you (#5884), please have a look. You can integrate the changes from there into your branch and delete it, or whatever is easier for you

anton-matosov · 2016-11-30T07:05:50Z

@AndreasAntener @LorenzMeier @kd0aij @Tumbili @julianoes PR is ready for review

LorenzMeier · 2016-11-30T07:23:32Z

Thanks! I did a first pass and it overall looks good. A couple of questions:

What was the driver behind adding manual_lockdown in addition to the lockdown flag? Was it necessary to have both?
Do you require manual RC input in SITL? I saw you added it back?
Can you explain more about the build system changes? They look ok, but I'd like to understand the use case.

Please try to describe this in general when submitting a PR, since those are changes that need context. Thanks!

anton-matosov · 2016-11-30T08:11:25Z

What was the driver behind adding manual_lockdown in addition to the lockdown flag? Was it necessary to have both?

If you have kill switch configured on RC, lockdown will be constantly reset to the killswitch position and it can't be used from other code.

Do you require manual RC input in SITL? I saw you added it back?

I was using manual RC for the RC loss testing and I made it off by default (configurable in CMakeLists). I will also need it for RC expo rates code testing in simulator.

Can you explain more about the build system changes? They look ok, but I'd like to understand the use case.

I have added wrapper for px4 that includes paths originally provided via the command line by cmake+sitl_run.sh. The original approach was not allowing easy debugging from IDE (I use CLion). With current approach you can simply chose simulator target you want to debug, e.g. px4_iris and simply hit debug button and it will build and start debug session with no need to configure bunch of run parameters.
I have also added IDE parameter to debuggers which is handled in a way of skipping launching px4 from sitl_run.sh that allows simple start/stop of px4 debugging from IDE without the need to restart viewer (I was using gazebo)

Please try to describe this in general when submitting a PR, since those are changes that need context. Thanks!

Sure. I will definitely do this next time

anton-matosov · 2016-12-02T07:45:16Z

@LorenzMeier any updates?

julianoes · 2016-12-02T10:07:44Z

@anton-matosov will review this weekend, thanks for your work.

anton-matosov · 2016-12-02T18:10:21Z

Thanks @julianoes

julianoes

Reviewed, this already looks cleaner but I need to review the failsafe nav state part in detail.

I'm a bit confused what the build system changes are for.

julianoes · 2016-12-03T10:40:02Z

Tools/sitl_run.sh

@@ -135,6 +135,15 @@ then
 elif [ "$debugger" == "valgrind" ]
 then
 	valgrind $sitl_command
+elif [ "$debugger" == "ide" ]


Make sure to document this somewhere.

Any suggestions on where to put docs for this?

julianoes · 2016-12-03T10:41:25Z

src/drivers/pwm_out_sim/pwm_out_sim.cpp

+			/* overwrite outputs in case of force_failsafe */
+			if (_failsafe) {
+				for (size_t i = 0; i < num_outputs; i++) {
+					outputs.output[i] = 600;


Why 600? That seems very low. Usually we have 900 in that case, and there should be a define.

I chose 600 as a magic for PWM Simulator failsafe as it doesn't read values from settings (might need to add this instead) and I needed some distinguishable value from disarmed to easily debug failsafe and not confuse with other states.

I have moved 600 and 900 to named constants.
It doesn't make sense to read params as they are not configured for any simulator

julianoes · 2016-12-03T10:44:02Z

src/firmware/posix/CMakeLists.txt

+				 FILES ${CMAKE_CURRENT_BINARY_DIR}/${APP_NAME}
+				 ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
+				 DEPENDS ${APP_NAME}
+				 DEST /home/linaro)


I'll need to test this on Snapdragon to make sure there are no regressions.

julianoes · 2016-12-03T10:46:24Z

src/modules/commander/commander.cpp

-						       offboard_loss_act,
-						       offboard_loss_rc_act);
+        /* now set navigation state according to failsafe and main state */
+        bool nav_state_changed = set_nav_state(&status,


Strange, so here the indent has changed but I think it was correct before.

Ok, will fix that

make format doesn't catch this.

Indent have changed from tabs to spaces.
Many files have mixed formatting...
What does the coding standard for px4 says about tab/spaces and what is the tab size used on the project (needed for mixed lines)?

Everything has to be tabs and default is 8 width (if you use Sublime, you get a decent default). Anything having mixed formatting is the result of style checking not being enforced yet (e.g. in the commander). We're cleaning up module-by-module and probably should enforce style when this PR is in.

julianoes · 2016-12-03T10:49:44Z

src/modules/commander/commander.cpp

+			}
+		} else {
+			/* armed, solid */
+			led_on(LED_BLUE);


Am I right that LED_GREEN is undefined in the non failsafe case and left at whatever it was toggled to last?

Most likely you are right. I will double check and fix. Good catch!

Added missing led_off(LED_GREEN);

julianoes · 2016-12-03T11:03:22Z

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -1040,6 +1040,63 @@ MulticopterAttitudeControl::task_main()
 					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
 				}
 			}
+
+			if (_v_control_mode.flag_control_termination_enabled) {


Shouldn't this be taken care off in the drivers instead of the controllers?

This where I have stared based on the FW_att_control but I couldn't stop motors from here.
For lockdown we don't really need anything, but for termination it is the a kind of only place to do something.
IMO drivers should not know about flight control modes and take actions based on this.

@anton-matosov The whole diff / PR makes sense, this is the only section I don't get yet entirely.

Are you trying to prevent an integrator windup here or is this needed to stop the props?

julianoes · 2016-12-03T11:05:00Z

src/modules/navigator/navigator_main.cpp

-				} else { /* if == 2 or unknown, RTL */
-					_navigation_mode = &_rtl;
-				}
+				_navigation_mode = &_rcLoss;


This is much cleaner and moves the responsibility to the commander. I think that's the right approach.

Cudos to @AndreasAntener

julianoes · 2016-12-03T11:06:50Z

src/modules/simulator/simulator_config.h.in

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 Anton Matosov. All rights reserved.


This should be PX4 Development Team, I think.

Emmm... I am not part of that team, so I don't think I should put your name on my changes.

It's just the default header in general, not sure.

julianoes · 2016-12-03T11:07:03Z

src/modules/simulator/simulator_mavlink.cpp

@@ -1,6 +1,7 @@
 /****************************************************************************
 *
 *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (c) 2016 Anton Matosov. All rights reserved.


And the same here actually.

julianoes · 2016-12-03T11:10:47Z

src/platforms/posix/sitl_runner_main.cpp.in

+        return errno;
+    }
+    const char *argsOverride[] = {argv[0], "@SITL_RUNNER_SOURCE_DIR@", "@SITL_RUNNER_MODEL_FILE@"};
+    return SITL_MAIN(sizeof(argsOverride) / sizeof(argsOverride[0]), (char**)argsOverride);


I'm gonna push back on this change, I think it conflicts heavily with upcoming changes in #5162.

I don't see any conflicts with your PR (on logic/behavior level, didn't dive in code yet)
But to make it cleaner I will separate build system/debugging changes from failsafe code and submit them as separate PR

That would be better, thanks.

julianoes · 2016-12-04T15:47:38Z

src/modules/commander/state_machine_helper.cpp

+	set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
+}
+
+void set_link_loss_nav_state(struct vehicle_status_s* status,


Oh I realize now what you are doing here. It seems quite complicated, in my opinion we need to simplify this all.

What exactly looks complicated here? This is mostly port of the link loss code from the navigator to commander + 2 new action types.
I have introduced enum for actions instead of using magic numbers and merged handlers for RC and data loss as the are the same except 1 nav state.

Got it. Were you already able to test most/all the cases?

I have not tested all the cases.

anton-matosov · 2016-12-08T04:14:06Z

@julianoes can you please take a look at the latest changes that address your comments?

anton-matosov · 2016-12-14T05:51:09Z

@LorenzMeier @julianoes I have applied suggested changes. Please review again.

@AndreasAntener

Move RC and DL failsafe actions handling from navigator to commander (credits to @AndreasAntener) Separate manual kill switch handling via manual_lockdown to prevent override and release of software lockdown by RC switch Other changes: Add failsafe tune Fix LED blinking for Pixracer Return back support for rc inputs in simulator but now it is configurable via cmake

LorenzMeier · 2016-12-16T20:03:36Z

@anton-matosov Thanks, much appreciated! I'm going to try to get this in tomorrow.

LorenzMeier · 2016-12-26T14:46:08Z

Awesome! This is ready to go, one last question. @anton-matosov Great work!

LorenzMeier · 2016-12-26T14:47:38Z

Rebased and continuing here:
#6177

anton-matosov mentioned this pull request Nov 15, 2016

Feature/TPA #5861

Closed

julianoes reviewed Nov 16, 2016

View reviewed changes

LorenzMeier assigned RomanBapst Nov 16, 2016

LorenzMeier added this to the Release v1.5.0 milestone Nov 19, 2016

julianoes modified the milestones: Release v1.5.1, Release v1.6.0 Nov 22, 2016

anton-matosov force-pushed the feature/MC-Failsafe-part1 branch from dd2a622 to a1cfc1a Compare November 30, 2016 06:21

anton-matosov mentioned this pull request Nov 30, 2016

Pixracer + S.Bus = Failsafe not working properly => fly away #5357

Closed

LorenzMeier assigned MaEtUgR Nov 30, 2016

julianoes requested changes Dec 3, 2016

View reviewed changes

julianoes reviewed Dec 4, 2016

View reviewed changes

anton-matosov force-pushed the feature/MC-Failsafe-part1 branch from 0cecdfa to d3fce07 Compare December 15, 2016 18:39

kd0aij mentioned this pull request Dec 17, 2016

WIP: Fix FMU MC failsafe on RC loss #5388

Closed

LorenzMeier mentioned this pull request Dec 26, 2016

Implement RC and DL failsafe action handling for multirotors #6177

Merged

LorenzMeier closed this Dec 26, 2016

Feature/First iteration on MC failsafe #5863

Feature/First iteration on MC failsafe #5863

Conversation

anton-matosov commented Nov 15, 2016

LorenzMeier commented Nov 15, 2016

LorenzMeier commented Nov 15, 2016

kd0aij commented Nov 15, 2016

kd0aij commented Nov 16, 2016

anton-matosov commented Nov 16, 2016

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

anton-matosov commented Nov 17, 2016

dagar commented Nov 17, 2016

AndreasAntener commented Nov 17, 2016

anton-matosov commented Nov 17, 2016

anton-matosov commented Nov 17, 2016

anton-matosov commented Nov 20, 2016

AndreasAntener commented Nov 20, 2016

AndreasAntener commented Nov 20, 2016 • edited

anton-matosov commented Nov 30, 2016

LorenzMeier commented Nov 30, 2016

anton-matosov commented Nov 30, 2016 • edited

anton-matosov commented Dec 2, 2016

julianoes commented Dec 2, 2016

anton-matosov commented Dec 2, 2016

julianoes left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

anton-matosov Dec 7, 2016 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

anton-matosov commented Dec 8, 2016

anton-matosov commented Dec 14, 2016

LorenzMeier commented Dec 16, 2016

LorenzMeier commented Dec 26, 2016

LorenzMeier commented Dec 26, 2016

AndreasAntener commented Nov 20, 2016 •

edited

anton-matosov commented Nov 30, 2016 •

edited

anton-matosov Dec 7, 2016 •

edited