Push-based high-level commander #903

jpreiss · 2021-12-14T05:45:35Z

Implements push-based high-level commander as discussed in #861.

Summary of changes (HLcmd is short for high-level commander):

Adds a new priority level for HLcmd, lower than all but DISABLE.
Stabilizer loop steps HLcmd and pushes setpoints on its behalf.
(Initially I tried having a separate task, but it was messier due to needing a lock on the state estimate struct.)
Introduces a new planner state to distinguish between the conditions:
- HLcmd has been preempted
- HLcmd has finished landing and wants motors off
This is necessary (as far as I can tell) to preserve the correct leveling-off and emergency stop behaviors on timeout, while also allowing the high-level commander to immediately cut the motors after landing.
Switching from low-level to high-level is accomplished by modifying the priority of the current setpoint sitting in the queue. This means we no longer need the remainValidMillisecs argument on the notifySetpointsStop command.
The commander.enHighLevel param is no longer needed.

The priority switching mechanism still needs to "know about" the high-level commander (i.e. commander.c still includes crtp_commander_high_level.h) to switch it into the disabled state when it is preempted. I couldn't figure out a way around this.

Although the net lines of code is increased, notice the simplification of commander.c.

Flight tested in 2 scenarios:

takeoff
cmdFullState
goTo
cmdFullState
goTo
land

and

takeoff
cmdFullState
<wait for leveling and emergency stop>

jpreiss · 2021-12-14T06:37:40Z

also tested the scenario

takeoff
goTo with long duration
cmdFullState preempt (before goTo is done)

and verified that, once the cmdFullState packets cease, we get leveling followed by emergency stop (vs. jumping to the end of the goTo trajectory).

jonasdn · 2021-12-14T12:51:12Z

src/modules/src/commander.c

- */
-PARAM_ADD_CORE(PARAM_UINT8, enHighLevel, &enableHighLevel)
-
-PARAM_GROUP_STOP(commander)


Removing this would break a lot of code out there, right? Including some examples

yes indeed, in the cflib, cfclient and also some demo applications

Perhaps for this PR we should still have the param and the value, but perhaps print a warning once somebody tries to enable it, as that it is deprecated. Since it's a core parameter, maybe we need some kind of procedure if we want to remove these type of parameters.

Sorry about that! In Crazyswarm missing parameters only cause a warning. I didn't realize it could actually break code.

I will restore the old param and comment. All crazyflie-firmware maintainers should be able to push to this branch, so please edit the docstring as you see fit.

restored in 4109e90

knmcguire · 2021-12-14T14:08:49Z

src/modules/src/planner.c

@@ -88,6 +88,16 @@ bool plan_is_stopped(struct planner *p)
 	return p->state == TRAJECTORY_STATE_IDLE;
 }

+void plan_disable(struct planner *p)


So now that we have a plan_disable, is the plan_stop necessary in the grander scheme of things? We do have a stopping function in the crtp_commander_generic.

Isn't it better to put plan_disable() in the stop function of the generic commander, and use the more general stop command instead. Just to prevent the same functionality in multiple places.

I believe we still need both the stopped and disabled states in the planner.c state machine due to reason 3 in my original PR comment. However, with this PR crtpCommanderHighLevelStop is no longer called by other parts of the firmware. The only place it might still be used is due to radio commands. I am not sure but it seems like we could deprecate and re-route the high-level stop radio command; I can't see why it needs to be different from the low-level one.

I think we could simply get rid of crtpCommanderHighLevelStop.

Just mind that crtpCommanderHighLevelStop are being used in the the app api examples and the app implementation of the swarm_demo

so we just have to make sure to replace those. And the CI would catch it as well as we check all the app layer examples and api

Maybe we can do this in a separate commit/PR?

@knmcguire in the swarm demo example, I believe the crtpCommanderHighLevelStop is not needed...

It is used here after landing:

crazyflie-firmware/examples/demos/swarm_demo/app.c

Lines 395 to 401 in 250a9e5

case STATE_LANDING:

if (crtpCommanderHighLevelIsTrajectoryFinished()) {

DEBUG_PRINT("Landed. Feed me!\n");

crtpCommanderHighLevelStop();

landingTimeCheckCharge = now + 3000;

state = STATE_CHECK_CHARGING;

}

but this same check and state transition will happen automatically within planner.c:

crazyflie-firmware/src/modules/src/planner.c

Lines 93 to 97 in 250a9e5

switch (p->state) {

case TRAJECTORY_STATE_LANDING:

if (plan_is_finished(p, t)) {

p->state = TRAJECTORY_STATE_IDLE;

}

(IIRC, this automatic state transition is the only reason we have a land command at all - otherwise we could just use goTo followed by a manual stop.)

It is used here when a crash is detected:

crazyflie-firmware/examples/demos/swarm_demo/app.c

Lines 425 to 427 in 250a9e5

case STATE_CRASHED:

crtpCommanderHighLevelStop();

break;

but the supervisor framework should be overriding the high-level commander anyway:

crazyflie-firmware/src/modules/src/supervisor.c

Lines 115 to 122 in 250a9e5

void supervisorUpdate(const sensorData_t *data)

{

isFlying = isFlyingCheck();

isTumbled = isTumbledCheck(data);

if (isTumbled && isFlying) {

stabilizerSetEmergencyStop();

}

I am not sure what's going on in the first link - it looks like it's just calling every API function in arbitrary order.

perhaps @krichardsson could take a look at the demo example to make sure it's not broken when we remove it. And then if in the same PR we also remove the app_api one as well, then it should be all good for the checks

I think we can remove it from the swarm demo.

Since crtpCommanderHighLevelStop() is part of the public API we should not remove it completely. I assume the correct way is to NOP it and make it deprecated. I think it should be kept in app_api.c for a while, whith a comment that it has been deprecated.

Could we mark it with __attribute__(deprecated)__?

https://gcc.gnu.org/onlinedocs/gcc-4.7.0/gcc/Function-Attributes.html

knmcguire · 2021-12-14T14:18:14Z

src/modules/src/stabilizer.c

@@ -256,6 +259,12 @@ static void stabilizerTask(void* param)
      stateEstimator(&state, tick);
      compressState();

+      if (RATE_DO_EXECUTE(RATE_HL_COMMANDER, tick)) {


Currently I'm wondering if this is the right place to handle the planner and the execution rate of the high level commander. The position and attitude rate are handled within the controllers so perhaps this part should be in commander.c ? Just thinking out loud how to improve the consistency of the code

OK, but wouldn't it go in crtp_commander_high_level.c?

In the long term I think we might want to move the rate-skipping code in stabilizer.c, because as we make more modules bind-able it would not make sense to include the rate limiting within the bound "library" part. But that is orthogonal to this PR.

moved in 65f3a41

yeah indeed, let's keep it in mind for future discussions!

whoenig

Very well done. I also love the code documentation surrounding it. Originally, I thought we would need another FreeRTOS task for pushing the HL commands, but I think the current solution is actually nicer.

whoenig · 2021-12-15T08:55:49Z

src/modules/src/crtp_commander_high_level.c

+LOG_GROUP_START(hlCommander)
+LOG_ADD(LOG_UINT8, pstate, &planner.state)
+LOG_GROUP_STOP(hlCommander)


Do we need this for production code or was that only for debugging? If needed, this should include more documentation, as with the other logging variables.

I used it for debugging but I doubt it will be a hot spot in the future. I'm fine with removing it.

It's not marked as an LOG_ADD_CORE, which means that it's not used in our examples or cfclient, so it's probably safe to remove.

removed in 74d3149, allowing a revert in 83f619f

whoenig · 2021-12-15T08:57:31Z

src/modules/src/planner.c

@@ -88,6 +88,16 @@ bool plan_is_stopped(struct planner *p)
 	return p->state == TRAJECTORY_STATE_IDLE;
 }

+void plan_disable(struct planner *p)


I think we could simply get rid of crtpCommanderHighLevelStop.

jpreiss · 2021-12-16T17:41:13Z

Marked as ready for review. The only outstanding comments are related to removing crtpCommanderHighLevelStop. If I understand the comments correctly, we are OK with deferring that to a separate PR.

Since this is a core change, it would be nice to get some other flight tests to corroborate mine. @jonasdn do you have the ability to do a flight test with low-to-high switching using the Bitcraze Python API?

knmcguire · 2021-12-20T13:22:52Z

Hi! @jonasdn is not able to come to the office for a bit so I did some flight testing with the following content of run_sequence:

  commander = cf.high_level_commander
  commander.takeoff(1.0, 2.0)
  time.sleep(3.0)
  cf.commander.send_position_setpoint(0,0.5,1,0)
  time.sleep(1)
  cf.commander.send_position_setpoint(0,0.0,1,0)
  time.sleep(1)
  commander.land(0.0, 2.0)
  time.sleep(2)
  commander.stop()

I guess the last one line doesn't do anything right now.

Do I need to also try out some rpyt setpoints in combination too @jpreiss ?

knmcguire · 2021-12-20T13:30:57Z

rypt from script is a bit difficult without an continuous controller input 😅 but at least the switch between position setpoint and high level command seems to do the trick. But will that be enough to know if this is all okay?

jpreiss · 2021-12-20T18:12:09Z

Sorry I've never used the Bitcraze API so I have some questions...

In this code:

cf.commander.send_position_setpoint(0,0.5,1,0)
time.sleep(1)

will the position setpoint be sent continuously over the 1 second sleep, or just once?

In this code:

cf.commander.send_position_setpoint(0,0.0,1,0)
time.sleep(1)
commander.land(0.0, 2.0)

there is no notifySetpointsStop packet sent, so the high-level setpoints will be rejected due to their lower priority because a streaming setpoint is still sitting in the queue. This should cause the CF to hold the last position setpoint for 0.5 seconds and then start drifting for 1.5 seconds due to the leveling behavior (based on the current values of COMMANDER_WDT_TIMEOUT_SHUTDOWN and COMMANDER_WDT_TIMEOUT_STABILIZE).

Then when the shutdown timeout finally happens, the HLcmd will already be at the end of the landing trajectory because it has 2second duration. It should be able to place a few setpoints at the end of the landing trajectory in the queue, but then it will start issuing motor stop setpoints. Therefore, I would expect the CF fall out of the sky almost immediately after the drifting period.

What do you observe?

jpreiss · 2021-12-20T18:12:54Z

No need to test rpyt setpoints - any switching between streaming setpoints and the high-level commander will test the mechanism.

knmcguire · 2021-12-21T11:35:40Z

here is a video of what I made yesterday. I won't come to the office again until early January unfortunately for more flight tests

It does not seem to hold the position for a long time... after the second position setpoint it reacts quite quickly to the HL land command..

cf_mixhlsetpoint.mp4

jpreiss · 2021-12-22T18:48:32Z

For other readers: the video above does not play correctly for me in macOS 10.14 on Firefox, but works in Safari.

jpreiss · 2021-12-22T19:21:55Z

Based on the code https://github.com/bitcraze/crazyflie-lib-python/blob/fbc491c811127785d3318e0ec45e264c055c1ca3/cflib/crazyflie/commander.py#L140-L144
I think the send_position_setpoint commands only send a single packet.

If so, then we should expect the CF to switch to leveling behavior 0.5 seconds into the sleep after the send_position_setpoint command.

If the white square on your floor corresponds to the coordinate (0, 0, 0), then drift from the leveling mechanism could explain why the landing is somewhat far to the right of the white square.

Then, the timeout-based switch to high-level for the landing would happen 1 second into the landing trajectory instead of at the end, so the CF has a chance to get close to z=0 before the motors are cut.

It's hard for me to tell what's happening in the video because the commands are of similar durations to the leveling/timeout mechanism. @knmcguire when you get back in the lab next year, could you try rerunning the same experiment but with much longer durations, like 4x everything?

knmcguire · 2021-12-22T19:28:05Z

Yes ofcourse! I'll try it once I'm back. I do have flowdeck with me but not a lot of space to crash, so the flight arena is necessary 😁

And perhaps we can also try to do a longer HL trajectory and in the middle of the duration send a position setpoint? That also seems like a good tryout for the priority switching.

jpreiss · 2021-12-22T19:40:08Z

Yes, that sounds like a good idea.

I was thinking it would be useful to collect these flight test scripts somewhere. Each script could have a comment telling the operator how the CF should behave in flight. When possible, we can have equivalent crazyflie-lib-python and crazyswarm versions of tests.

@jonasdn would this be in the scope of crazyflie-testing? Currently I only see tests that can run without flying, like the logging/param system.

jonasdn · 2021-12-23T06:08:06Z

Yes, that sounds like a good idea.

I was thinking it would be useful to collect these flight test scripts somewhere. Each script could have a comment telling the operator how the CF should behave in flight. When possible, we can have equivalent crazyflie-lib-python and crazyswarm versions of tests.

@jonasdn would this be in the scope of crazyflie-testing? Currently I only see tests that can run without flying, like the logging/param system.

It might be! I wamt to add som flying tests eventually, both supervised and un-supervised.

knmcguire · 2022-01-11T11:34:53Z

Alright here are some more flights:

I did first two scenarios:

take off in HL
position low level command 1.0 meter to left
wait for 4 seconds

and then

take off in HL
High level 1.0 forward
immediately a low level cmd to the left
wait for 4 seconds

In both scenarios it moved to the left and then the motors shut off after 2 seconds like this:

20220111_111952-converted.mp4

Then I tried it out during a planned trajectory

take off in HL command
start 8 trajectory
halfway, send a low level position command to middle
wait for 4 seconds

Then you see this:

20220111_112415-converted.mp4

What I do find strange, is that for the low level position setpoints (with #cf.commander.send_position_setpoint()
), it does not seem to really stop at those locations I've indicated but continues to glide in the same direction, but that is probably due to the fact that the motors go to 'hover' after 1 second and the crazyflie is no longer controlled.

But in general, it seems that this PR indeeds does the trick for switching from HL to low level

knmcguire · 2022-01-11T11:36:22Z

The experiment before the last one also did show, that after the low-level setpoint is given, it can switch to HL without any problem (if no low level setpoint is given at the same time)

jonasdn · 2022-01-12T07:56:06Z

Does this look mergable to you @jpreiss ?

jpreiss · 2022-01-13T21:24:19Z

I agree, in the most recent experiments the glide/drift past the position setpoint seems consistent with the leveling mechanism activating while the x/y velocity is nonzero.

In the earlier experiments, I agree it demonstrates low-to-high switching, but only in the form of a fallback after drifting the 0.5-second timeout period. To test instantaneous switching, you would need to implement the notifySetpointsStop packet in the Bitcraze Python API.

Since it works with Crazyswarm I doubt there's any problem on the firmware side. However, it seems less than ideal that a firmware feature would require Crazyswarm to fully test its functionality.

knmcguire · 2022-01-14T13:58:00Z

Yes that is true... we should implement that in the cflib first, but I've made a issue for it for now. Not sure if we manage to do this before the next immediate release within 1 or two weeks but at least we are reminded.

For now, I will trust that this works if you have already tried it out in crazyswarm :)

knmcguire · 2022-01-26T09:50:05Z

woops! forgot to merge this for the release.... we will do a few extra test to just double check this doesn't break anything.

* first draft of push-based high-level commander * fix missing extern in bindings build * restore enHighLevel param, now unused * move HLcmd rate limit out of stabilizer.c * remove debug log var in HLcmd * switch uint8_t to enum now that it's not logged

first draft of push-based high-level commander

628d1e3

jonasdn requested review from krichardsson, knmcguire, whoenig and ataffanel December 14, 2021 05:53

fix missing extern in bindings build

34a9436

jonasdn reviewed Dec 14, 2021

View reviewed changes

knmcguire reviewed Dec 14, 2021

View reviewed changes

restore enHighLevel param, now unused

4109e90

jpreiss force-pushed the priority3 branch from c7d8ad4 to 4109e90 Compare December 14, 2021 17:59

move HLcmd rate limit out of stabilizer.c

65f3a41

whoenig approved these changes Dec 15, 2021

View reviewed changes

jpreiss added 2 commits December 16, 2021 09:10

remove debug log var in HLcmd

74d3149

switch uint8_t to enum now that it's not logged

83f619f

jpreiss force-pushed the priority3 branch from 51e2852 to 83f619f Compare December 16, 2021 17:16

jpreiss marked this pull request as ready for review December 16, 2021 17:20

knmcguire mentioned this pull request Jan 14, 2022

Implement notifySetpointsStop packet bitcraze/crazyflie-lib-python#301

Closed

knmcguire approved these changes Jan 14, 2022

View reviewed changes

knmcguire added this to the 2022.01 milestone Jan 26, 2022

knmcguire merged commit 5bf6f11 into bitcraze:master Jan 26, 2022

jpreiss mentioned this pull request Jan 27, 2022

Overall commander architecture is complicated #861

Closed

knmcguire mentioned this pull request Mar 7, 2022

deprecate crtpCommanderHighLevelStop and make more generic alternative #980

Closed

This was referenced Apr 8, 2022

NotifySetpointStop odd behavior USC-ACTLab/crazyswarm#464

Closed

Document notifySetpointsStop USC-ACTLab/crazyswarm#625

Open

knmcguire mentioned this pull request Aug 11, 2022

Step by Step Swarm Guide bitcraze/crazyflie-lib-python#352

Merged

This was referenced Feb 15, 2023

Removed use of commander.enHighLevel bitcraze/crazyflie-lib-python#381

Merged

Deprecated enHighLevel #1230

Merged

knmcguire mentioned this pull request Apr 25, 2023

Extended supervisor #1272

Merged

	case STATE_LANDING:
	if (crtpCommanderHighLevelIsTrajectoryFinished()) {
	DEBUG_PRINT("Landed. Feed me!\n");
	crtpCommanderHighLevelStop();
	landingTimeCheckCharge = now + 3000;
	state = STATE_CHECK_CHARGING;
	}

	switch (p->state) {
	case TRAJECTORY_STATE_LANDING:
	if (plan_is_finished(p, t)) {
	p->state = TRAJECTORY_STATE_IDLE;
	}

	void supervisorUpdate(const sensorData_t *data)
	{
	isFlying = isFlyingCheck();

	isTumbled = isTumbledCheck(data);
	if (isTumbled && isFlying) {
	stabilizerSetEmergencyStop();
	}

Push-based high-level commander #903

Push-based high-level commander #903

Conversation

jpreiss commented Dec 14, 2021

jpreiss commented Dec 14, 2021

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

jpreiss Dec 15, 2021 • edited Loading

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

jonasdn Dec 20, 2021 • edited Loading

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

whoenig 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

jpreiss Dec 16, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jpreiss commented Dec 16, 2021

knmcguire commented Dec 20, 2021 • edited Loading

knmcguire commented Dec 20, 2021

jpreiss commented Dec 20, 2021 • edited Loading

jpreiss commented Dec 20, 2021

knmcguire commented Dec 21, 2021 • edited Loading

jpreiss commented Dec 22, 2021 • edited Loading

jpreiss commented Dec 22, 2021

knmcguire commented Dec 22, 2021

jpreiss commented Dec 22, 2021

jonasdn commented Dec 23, 2021

knmcguire commented Jan 11, 2022

knmcguire commented Jan 11, 2022

jonasdn commented Jan 12, 2022

jpreiss commented Jan 13, 2022

knmcguire commented Jan 14, 2022

knmcguire commented Jan 26, 2022

jpreiss Dec 15, 2021 •

edited

Loading

jonasdn Dec 20, 2021 •

edited

Loading

jpreiss Dec 16, 2021 •

edited

Loading

knmcguire commented Dec 20, 2021 •

edited

Loading

jpreiss commented Dec 20, 2021 •

edited

Loading

knmcguire commented Dec 21, 2021 •

edited

Loading

jpreiss commented Dec 22, 2021 •

edited

Loading