motor shutoff issues during disconnect
Posted: Wed Oct 28, 2020 5:05 pm
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;
}
}
}