Extended supervisor

[krichardsson]

This PR improves the supervisor functionality to handle bigger, brushless platforms in a better way. It is still work in progress but the goal is to move handling of conditions such as arming, loss of connection, emegency stop and similar to the supervisor.

[knmcguire]

So these are some important references to emergency stop in the high level commander

This is the trajectory command, which we should probably still keep in here for Crazyswarm or others, but there should be some deprecation sign here? Or we change the functionality that only the planner is stopped but it's not changing the setpoint but instead sends a stop request.

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

Line 134 in 39cf2a2

COMMAND_STOP = 3,

When the planner is stopped, this logic will actually return a nullsetpoint (with the pose states to ModeDisable) such that all motors will stop when the stabilizer This should either be removed or changed as an emergency stop request:

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

Line 287 in 39cf2a2

if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, stabilizerStep)) {

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

Lines 319 to 323 in 39cf2a2

	if (plan_is_stopped(&planner)) {
	// Return a null setpoint - when the HLcommander is stopped, it wants the
	// motors to be off. Only reason they should be spinning is if the
	// HLcommander has been preempted by a streaming setpoint command.
	*setpoint = nullSetpoint;

This function belong so the COMMAND_STOP, my comments are the same as above

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

Lines 548 to 557 in 39cf2a2

	int stop(const struct data_stop* data)
	{
	int result = 0;
	if (isInGroup(data->groupMask)) {
	xSemaphoreTake(lockTraj, portMAX_DELAY);
	plan_stop(&planner);
	xSemaphoreGive(lockTraj);
	}
	return result;
	}

I think the issues is the following, as currently there are 2 trajectory or planner states on which the crtp_commander_high_level is responding on differently

• TRAJECTORY_STATE_IDLE (plan_stop and plan_is_stopped is reacting on those)
• TRAJECTORY_STATE_DISABLED ( plan_disable() and plane_is_disabled() )

So crtpCommanderHighLevelGetSetpoint sets the setpoint in case of TRAJECTORY_STATE_IDLE to a nullsetpoint, while it does not do that with TRAJECTORY_STATE_DISABLED. The latter was introduced with the push-based commander with mixing of HL and normale setpoints (see this PR) as you don't want the drone to fall out of the sky if it gets a lower level setpoint, but you do want to simple disable the planner (done with crtpCommanderHighLevelDisabled()

Anyway, we might need to take a good look at the logic of this to make sure it all goes okay.

[knmcguire]

This is the emergency stop handled by generic commander:

There is a packet type of the [crtp_commander_generic], on which the reaction is to do nothing, assuming the setpoint is already on zero upon initialization (which might not be the case with the HL commander coming in front) (https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_generic.c) that also handles stopping of motors

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

Line 66 in 39cf2a2

stopType = 0,

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

Lines 81 to 87 in 39cf2a2

	/* stopDecoder
	* Keeps setpoint to 0: stops the motors and fall
	*/
	static void stopDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
	{
	return;
	}

Here we should probably not assume that the setpoint is zero, and put out a request to stop for the supervisor instead perhaps?

[knmcguire]

Now for the localization services

I see that this is already in the first commits, but I'll add it anyway just for the sake of completion :)

Couple of localization services message types that are related:

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

Lines 296 to 301 in 39cf2a2

	case EMERGENCY_STOP:
	stabilizerSetEmergencyStop();
	break;
	case EMERGENCY_STOP_WATCHDOG:
	stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT);
	break;

And these are related to these functions in stabilizer loop:

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

Lines 340 to 355 in 39cf2a2

	void stabilizerSetEmergencyStop()
	{
	emergencyStop = true;
	}
	void stabilizerResetEmergencyStop()
	{
	emergencyStop = false;
	}
	void stabilizerSetEmergencyStopTimeout(int timeout)
	{
	emergencyStop = false;
	emergencyStopTimeout = timeout;
	}

I guess... the question is, what was the function of the emergencystoptimeout that I don't really understand yet

[knmcguire]

And the emergency stop from the stabilizer itself which can be enabled from a parameter.

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

Lines 371 to 374 in 39cf2a2

	/**
	* @brief If set to nonzero will turn off power
	*/
	PARAM_ADD_CORE(PARAM_UINT8, stop, &emergencyStop)

And the functions are the same that localization srvs are using.

I see that the first commits of this PR have already taken this into account

[knmcguire]

very cool! I like the approach. I'll do a more thorough review once you are done.

[krichardsson]

I think the high level commander looks OK and that both the stop and idle states are valid. To me it does not look like they are related to emergency stop, but rather just a normal stop.
Stop = stop the motors. I think this is a valid case, I can imagine my self wanting to stop motors without doing a land at some point
Disable = Not using the high level commander at all. Makes sense :-)

What I think is missing though, is a connection between the high level commander and the isFlying in the supervisor. Currently the isFlying is only checking if the motors are spinning. The problem is that the supervisor changes state (from flying to landed) based on isFlying and we might end up being landed in mid-air, if we temporarily stop the motors.

[krichardsson]

For the generic commander I think the same reasoning holds, the stop set point is probably not an emergency stop function, it simply means "stop the motors". @ataffanel Can you shed any light on the semantics?

I don't think we need to do anything here.

[krichardsson]

The functionality in localization services is the real emergency stop. This functionality has been implemented in the new supervisor and I don't think there is anything more here that must be added at this point.

The idea is that the emergency stop message stops the motors - pretty straight forward.

The timeout is related to making sure the emergency stop communication is working. An "emergency stop device" is expected to continuously send "heartbeats" to show that it is alive, if no heartbeat is received, the Crazyflie will assume the device is broken and do an emergency stop.

[krichardsson]

The stop parameter in stabilizer has now been re-implemented in the supervisor and should behave the same way as before.

[ataffanel]

For the generic commander I think the same reasoning holds, the stop set point is probably not an emergency stop function, it simply means "stop the motors". @ataffanel Can you shed any light on the semantics?

This is correct. The reason we have both a stop message and an emergency stop is that the stop message is intended for normal operation to stop the motors (for example when implementing a landing sequence controlled remotely). The e-stop is designed to be independent from the setpoint so that it can be activated independently from the normal control algorithms.

[tobbeanton]

canFly state becomes wrong if system is disarmed in ready to fly state.

[krichardsson]

@tobbeanton It looks like it should go back to supervisorStatePreFlChecksNotPassed

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

Lines 75 to 79 in 1fd1259

	{
	.newState = supervisorStatePreFlChecksNotPassed,
	.mustBeSet = SUPERVISOR_CB_NONE,
	.mustNotBeSet = SUPERVISOR_CB_ARMED
	},

This should set canFly to false

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

Line 215 in 1fd1259

this->canFly = supervisorAreMotorsAllowedToRun();

I assume something is broken here?

[tobbeanton]

I added a canArm log variable but I think we still need to discuss how we want things to work. It wasn't as easy as anticipated 😄