motor shutoff issues during disconnect

[GriffinBonner]

I have successfully written, built, flashed, and tested my firmware application. Currently, the application checks if the log variable "radio.isConnected" to determine if the connection has been dropped during flight. If the crazyflie is disconnected the application uses the zrange variable from the laser range sensor on the flow deck v2 and the function setHoverSetpoint to send setpoints to the commander with negative z-axis velocity and land the crazyflie. The landing sequence has been tested while the crazyflie remains connected to the client and it works as expected, the program states have also been verified with the client debug tab. The only problem that I am experiencing is that there seems to be something in the firmware which automatically shuts off the motors when a disconnect is detected, so the crazyflie just falls out of the air and my landing sequence is never entered. Do you know if this can be disabled? or have any suggestions on how continue running my application task even when the core firmware detects that the crazyflie is no longer connected? I have attached my application code below.

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>

#include "app.h"            // entry to application layer
#include "commander.h"      // position commander
#include "FreeRTOS.h"       // FreeRTOS API
#include "task.h"           // RTOS system tasks
#include "debug.h"          // debugging
#include "log.h"            // internal access to logged variables
#include "param.h"          // internal access to run-time parameters

#include "crtp_commander_high_level.h"  // compute setpoints for smooth trajectory commands

#define DEBUG_MODULE "PUSH" 

// pack setpoint to descrease altitude
static void setHoverSetpoint(setpoint_t *setpoint, float vx, float vy, float vz, float yawrate){
    setpoint->mode.z = modeVelocity; 
    setpoint->velocity.z = vz;

    setpoint->mode.yaw = modeVelocity;
    setpoint->attitudeRate.yaw = yawrate;

    setpoint->mode.x = modeVelocity;
    setpoint->mode.y = modeVelocity;
    setpoint->velocity.x = vx;
    setpoint->velocity.y = vy;

    setpoint->velocity_body = true;
}

#define MAX(a,b) ((a>b)?a:b) // maximum macro
#define MIN(a,b) ((a<b)?a:b) // minimum macro

// deadman_land module state control
typedef enum{
    idle,
    connected,
    disconnected,
    rssi_low,
    landing
} dl_state;

// initialize system state
static dl_state state = idle;

// initialize system variables
static uint8_t prev_con = 0;
static uint8_t cur_con = 0;
static uint8_t rssi = 0;
static uint16_t zrange;
static const uint16_t ground_z = 35;
static const uint16_t max_z = 800;

//static const uint8_t max_rssi = 80;

void appMain(){

    vTaskDelay(M2T(3000)); // block task for 3s ( depends on TICK_PERIOD_MS, one tick per period)

    static setpoint_t setpoint;

    uint16_t idConnection = logGetVarId("radio", "isConnected"); // id of connection variable
    uint16_t idRssi = logGetVarId("radio", "rssi"); // id of rssi variable
    uint16_t idZrange = logGetVarId("range","zrange"); // id of z-range from flow deck v2

    DEBUG_PRINT("deadman land module initialized, waiting for activation...\n"); // print debug message

    while(1){

        vTaskDelay(M2T(100)); // block task for 100 ms

        rssi = logGetUint(idRssi);
        cur_con = logGetUint(idConnection);
        zrange = logGetUint(idZrange);
        
        if (prev_con == 0 && cur_con == 0){
            state = idle;
        }else if(prev_con == 0 && cur_con == 1){
            state = connected;
        }else if(prev_con == 1 && cur_con == 0){
            state = disconnected;
        }

        switch (state){
            case idle:
                DEBUG_PRINT("state: idle, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                break;
            case connected:
                DEBUG_PRINT("state: connected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                break;
            case disconnected:
                DEBUG_PRINT("state: disconnected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                state = landing;
                zrange = logGetUint(idZrange);
                while(MAX(zrange,ground_z) == zrange){
                    setHoverSetpoint(&setpoint,0,0,-0.1,0);
                    commanderSetSetpoint(&setpoint,4);
                    vTaskDelay(M2T(10));
                    zrange = logGetUint(idZrange);
                }
                state = idle;
                break;
            case landing:
                DEBUG_PRINT("state: disconnected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                break;
            default:
                DEBUG_PRINT("rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                break;
        }
    }
}

[kristoffer]

Hi!
I think you are maybe hit by a safety mechanism that is designed to prevent a Crazyflie to fly away if connection is lost.
This would happen if you send set points from the PC and loose connection, is the case?

You can find the code here https://github.com/bitcraze/crazyflie-f ... #L103-L133

[GriffinBonner]

Yes, I am initially controlling the crazyflie from either a python script using the cflib or from the client using a manual controller. The goal of my application is to detect when the connection is lost (isConnected set from 1->0) and land the crazyflie. Will I be able to disable the condition you mention in your reply which prevents the crazyflie from flying away when disconnected and instead shuts off the motors?

[GriffinBonner]

My program starts sending the commander setpoints with decreased z-velocity after a disconnect is detected. So I want to disable the feature which shuts down the crazyflie if no setpoints are received, in commander.c, so that commander.c can start receiving the setpoints that my application posted above is sending.

[arnaud]

The commander subsystem has been designed for this kind of usecase and there is a priority value for each setpoint. Since you are using a priority higher than the radio setpoint (CRTP priority is 1, you are using 4), you should be able to take-over instantly the control bypassing the commander watchdog.

I have been looking around the firmware and I cannot see anything else that would cut the motors when the connection is lost. I also looked at the timing, and the radio disconnection is detected after 1 second, and the motor cut in the commander happens after 2 seconds, so you should definitely be able to start controlling the Crazyflie before the motors are cut.

This was not very helpful so far but I will continue looking around to see if I can find what is happening.

[GriffinBonner]

Thank you for looking into this. I have not been able to find the problem yet either. I think that the problem may be that my script is checking the log value "isConnected" which may be toggled after the timeout has already occurred and the motors shut off. This would mean my application code does not have sufficient time to pre-empt and assume control. I wrote my code under the assumption "isConnected" would be toggled before the timeout period, and this may not be the case.

[arnaud]

I already though about timing and the commander watchdog is of 2 seconds while the CRTP radio link watchdog you are looking at is 1 second, so you should be able to react 1 second before the motor are cut.

I tested your code and it seems to be a logic problem: when flying in hover mode and disconnecting the radio, then reconnecting to the Crazyflie and looking at the console I see that:

Code: Select all

state: connected, rssi: 35, connection: 1, z-range: 433
state: connected, rssi: 35, connection: 1, z-range: 435
state: connected, rssi: 35, connection: 1, z-range: 433
state: idle, rssi: 35, connection: 0, z-range: 428
state: idle, rssi: 35, connection: 0, z-range: 430
state: idle, rssi: 35, connection: 0, z-range: 426
state: idle, rssi: 35, connection: 0, z-range: 425

The state machine never goes to disconnected and then to landing state. The main problem is with the state transition, the state transition needs to be calculated using the current state, by moving all state transition in the switch(). A side thing is that having a while loop in a state is not consistent with a state machine implementation so I moved that in the landing state as well.

This implementation works:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>

#include "app.h"            // entry to application layer
#include "commander.h"      // position commander
#include "FreeRTOS.h"       // FreeRTOS API
#include "task.h"           // RTOS system tasks
#include "debug.h"          // debugging
#include "log.h"            // internal access to logged variables
#include "param.h"          // internal access to run-time parameters

#include "crtp_commander_high_level.h"  // compute setpoints for smooth trajectory commands

#define DEBUG_MODULE "PUSH" 

// pack setpoint to descrease altitude
static void setHoverSetpoint(setpoint_t *setpoint, float vx, float vy, float vz, float yawrate){
    setpoint->mode.z = modeVelocity; 
    setpoint->velocity.z = vz;

    setpoint->mode.yaw = modeVelocity;
    setpoint->attitudeRate.yaw = yawrate;

    setpoint->mode.x = modeVelocity;
    setpoint->mode.y = modeVelocity;
    setpoint->velocity.x = vx;
    setpoint->velocity.y = vy;

    setpoint->velocity_body = true;
}

#define MAX(a,b) ((a>b)?a:b) // maximum macro
#define MIN(a,b) ((a<b)?a:b) // minimum macro

// deadman_land module state control
typedef enum{
    idle,
    connected,
    disconnected,
    rssi_low,
    landing
} dl_state;

// initialize system state
static dl_state state = idle;

// initialize system variables
// static uint8_t prev_con = 0;
static uint8_t cur_con = 0;
static uint8_t rssi = 0;
static uint16_t zrange;
static const uint16_t ground_z = 35;
// static const uint16_t max_z = 800;

//static const uint8_t max_rssi = 80;

void appMain(){

    vTaskDelay(M2T(3000)); // block task for 3s ( depends on TICK_PERIOD_MS, one tick per period)

    static setpoint_t setpoint;

    uint16_t idConnection = logGetVarId("radio", "isConnected"); // id of connection variable
    uint16_t idRssi = logGetVarId("radio", "rssi"); // id of rssi variable
    uint16_t idZrange = logGetVarId("range","zrange"); // id of z-range from flow deck v2

    DEBUG_PRINT("deadman land module initialized, waiting for activation...\n"); // print debug message

    while(1){

        vTaskDelay(M2T(100)); // block task for 100 ms

        rssi = logGetUint(idRssi);
        cur_con = logGetUint(idConnection);
        zrange = logGetUint(idZrange);

        switch (state){
            case idle:
                DEBUG_PRINT("state: idle, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);

                if (cur_con == 1) {
                  state = connected;
                }

                break;
            case connected:
                DEBUG_PRINT("state: connected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);

                if (cur_con == 0) {
                  state = disconnected;
                }

                break;
            case disconnected:
                DEBUG_PRINT("state: disconnected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);

                state = landing;

                break;
            case landing:
                DEBUG_PRINT("state: disconnected, rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);

                setHoverSetpoint(&setpoint,0,0,-0.1,0);
                commanderSetSetpoint(&setpoint,4);

                if (zrange <= ground_z) {
                  // Motors should be cut there
                  state = idle;
                }

                break;
            default:
                DEBUG_PRINT("rssi: %i, connection: %i, z-range: %i\n",rssi,cur_con,zrange);
                break;
        }
    }
}

[GriffinBonner]

Thank you for your help, the application is working. I am very happy that I could contain all of the functionality within the out-of-tree build system as an application without modifying the Crazyflie source.