Skip to content

Commit

Permalink
Merge pull request #1415 from bitcraze/1406-stabilizer-task-blocks-du…
Browse files Browse the repository at this point in the history 
…e-to-gyro-failure-freezing-flight-control

Stop motors when stabilizer task is blocking
  • Loading branch information
@gemenerik
gemenerik authored Oct 9, 2024
2 parents 3a35c64 + e9ac614 commit 3a91774
Show file tree
Hide file tree
Showing 2 changed files with 159 additions and 137 deletions.
265 changes: 134 additions & 131 deletions src/config/config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,45 +61,46 @@


// Task priorities. Higher number higher priority
#define PASSTHROUGH_TASK_PRI 5
#define STABILIZER_TASK_PRI 5
#define SENSORS_TASK_PRI 4
#define ADC_TASK_PRI 3
#define FLOW_TASK_PRI 3
#define MULTIRANGER_TASK_PRI 3
#define SYSTEM_TASK_PRI 2
#define CRTP_TX_TASK_PRI 2
#define CRTP_RX_TASK_PRI 2
#define EXTRX_TASK_PRI 2
#define ZRANGER_TASK_PRI 2
#define ZRANGER2_TASK_PRI 2
#define LOG_TASK_PRI 1
#define MEM_TASK_PRI 1
#define PARAM_TASK_PRI 1
#define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0
#define USDLOG_TASK_PRI 1
#define USDWRITE_TASK_PRI 0
#define PCA9685_TASK_PRI 2
#define CMD_HIGH_LEVEL_TASK_PRI 2
#define BQ_OSD_TASK_PRI 1
#define GTGPS_DECK_TASK_PRI 1
#define LIGHTHOUSE_TASK_PRI 3
#define LPS_DECK_TASK_PRI 3
#define OA_DECK_TASK_PRI 3
#define UART1_TEST_TASK_PRI 1
#define UART2_TEST_TASK_PRI 1
#define KALMAN_TASK_PRI 2
#define ERROR_UKF_TASK_PRI 2
#define LEDSEQCMD_TASK_PRI 1
#define FLAPPERDECK_TASK_PRI 2
#define SYSLINK_TASK_PRI 3
#define USBLINK_TASK_PRI 3
#define ACTIVE_MARKER_TASK_PRI 3
#define AI_DECK_TASK_PRI 1
#define UART2_TASK_PRI 3
#define CRTP_SRV_TASK_PRI 0
#define PLATFORM_SRV_TASK_PRI 0
#define PASSTHROUGH_TASK_PRI 5
#define STABILIZER_TASK_PRI 5
#define RATE_SUPERVISOR_TASK_PRI 5
#define SENSORS_TASK_PRI 4
#define ADC_TASK_PRI 3
#define FLOW_TASK_PRI 3
#define MULTIRANGER_TASK_PRI 3
#define SYSTEM_TASK_PRI 2
#define CRTP_TX_TASK_PRI 2
#define CRTP_RX_TASK_PRI 2
#define EXTRX_TASK_PRI 2
#define ZRANGER_TASK_PRI 2
#define ZRANGER2_TASK_PRI 2
#define LOG_TASK_PRI 1
#define MEM_TASK_PRI 1
#define PARAM_TASK_PRI 1
#define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0
#define USDLOG_TASK_PRI 1
#define USDWRITE_TASK_PRI 0
#define PCA9685_TASK_PRI 2
#define CMD_HIGH_LEVEL_TASK_PRI 2
#define BQ_OSD_TASK_PRI 1
#define GTGPS_DECK_TASK_PRI 1
#define LIGHTHOUSE_TASK_PRI 3
#define LPS_DECK_TASK_PRI 3
#define OA_DECK_TASK_PRI 3
#define UART1_TEST_TASK_PRI 1
#define UART2_TEST_TASK_PRI 1
#define KALMAN_TASK_PRI 2
#define ERROR_UKF_TASK_PRI 2
#define LEDSEQCMD_TASK_PRI 1
#define FLAPPERDECK_TASK_PRI 2
#define SYSLINK_TASK_PRI 3
#define USBLINK_TASK_PRI 3
#define ACTIVE_MARKER_TASK_PRI 3
#define AI_DECK_TASK_PRI 1
#define UART2_TASK_PRI 3
#define CRTP_SRV_TASK_PRI 0
#define PLATFORM_SRV_TASK_PRI 0

// Not compiled
#if 0
Expand All @@ -109,101 +110,103 @@


// Task names
#define SYSTEM_TASK_NAME "SYSTEM"
#define LEDSEQCMD_TASK_NAME "LEDSEQCMD"
#define ADC_TASK_NAME "ADC"
#define PM_TASK_NAME "PWRMGNT"
#define CRTP_TX_TASK_NAME "CRTP-TX"
#define CRTP_RX_TASK_NAME "CRTP-RX"
#define CRTP_RXTX_TASK_NAME "CRTP-RXTX"
#define LOG_TASK_NAME "LOG"
#define MEM_TASK_NAME "MEM"
#define PARAM_TASK_NAME "PARAM"
#define SENSORS_TASK_NAME "SENSORS"
#define STABILIZER_TASK_NAME "STABILIZER"
#define NRF24LINK_TASK_NAME "NRF24LINK"
#define ESKYLINK_TASK_NAME "ESKYLINK"
#define SYSLINK_TASK_NAME "SYSLINK"
#define USBLINK_TASK_NAME "USBLINK"
#define PROXIMITY_TASK_NAME "PROXIMITY"
#define EXTRX_TASK_NAME "EXTRX"
#define UART_RX_TASK_NAME "UART"
#define ZRANGER_TASK_NAME "ZRANGER"
#define ZRANGER2_TASK_NAME "ZRANGER2"
#define FLOW_TASK_NAME "FLOW"
#define USDLOG_TASK_NAME "USDLOG"
#define USDWRITE_TASK_NAME "USDWRITE"
#define PCA9685_TASK_NAME "PCA9685"
#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
#define MULTIRANGER_TASK_NAME "MR"
#define BQ_OSD_TASK_NAME "BQ_OSDTASK"
#define GTGPS_DECK_TASK_NAME "GTGPS"
#define LIGHTHOUSE_TASK_NAME "LH"
#define LPS_DECK_TASK_NAME "LPS"
#define OA_DECK_TASK_NAME "OA"
#define UART1_TEST_TASK_NAME "UART1TEST"
#define UART2_TEST_TASK_NAME "UART2TEST"
#define KALMAN_TASK_NAME "KALMAN"
#define ERROR_UKF_TASK_NAME "ERROR_UKF"
#define ACTIVE_MARKER_TASK_NAME "ACTIVEMARKER-DECK"
#define AI_DECK_GAP_TASK_NAME "AI-DECK-GAP"
#define AIDECK_ESP_TX_TASK_NAME "AI-DECK ESP TX"
#define AIDECK_ESP_RX_TASK_NAME "AI-DECK ESP RX"
#define UART2_TASK_NAME "UART2"
#define CRTP_SRV_TASK_NAME "CRTP-SRV"
#define PLATFORM_SRV_TASK_NAME "PLATFORM-SRV"
#define PASSTHROUGH_TASK_NAME "PASSTHROUGH"
#define CPX_RT_UART_TASK_NAME "ROUTER FROM UART2"
#define CPX_RT_INT_TASK_NAME "ROUTER FROM INTERNAL"
#define CPX_TASK_NAME "CPX"
#define APP_TASK_NAME "APP"
#define FLAPPERDECK_TASK_NAME "FLAPPERDECK"
#define SYSTEM_TASK_NAME "SYSTEM"
#define LEDSEQCMD_TASK_NAME "LEDSEQCMD"
#define ADC_TASK_NAME "ADC"
#define PM_TASK_NAME "PWRMGNT"
#define CRTP_TX_TASK_NAME "CRTP-TX"
#define CRTP_RX_TASK_NAME "CRTP-RX"
#define CRTP_RXTX_TASK_NAME "CRTP-RXTX"
#define LOG_TASK_NAME "LOG"
#define MEM_TASK_NAME "MEM"
#define PARAM_TASK_NAME "PARAM"
#define SENSORS_TASK_NAME "SENSORS"
#define STABILIZER_TASK_NAME "STABILIZER"
#define RATE_SUPERVISOR_TASK_NAME "RATE_SUPERVISOR"
#define NRF24LINK_TASK_NAME "NRF24LINK"
#define ESKYLINK_TASK_NAME "ESKYLINK"
#define SYSLINK_TASK_NAME "SYSLINK"
#define USBLINK_TASK_NAME "USBLINK"
#define PROXIMITY_TASK_NAME "PROXIMITY"
#define EXTRX_TASK_NAME "EXTRX"
#define UART_RX_TASK_NAME "UART"
#define ZRANGER_TASK_NAME "ZRANGER"
#define ZRANGER2_TASK_NAME "ZRANGER2"
#define FLOW_TASK_NAME "FLOW"
#define USDLOG_TASK_NAME "USDLOG"
#define USDWRITE_TASK_NAME "USDWRITE"
#define PCA9685_TASK_NAME "PCA9685"
#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
#define MULTIRANGER_TASK_NAME "MR"
#define BQ_OSD_TASK_NAME "BQ_OSDTASK"
#define GTGPS_DECK_TASK_NAME "GTGPS"
#define LIGHTHOUSE_TASK_NAME "LH"
#define LPS_DECK_TASK_NAME "LPS"
#define OA_DECK_TASK_NAME "OA"
#define UART1_TEST_TASK_NAME "UART1TEST"
#define UART2_TEST_TASK_NAME "UART2TEST"
#define KALMAN_TASK_NAME "KALMAN"
#define ERROR_UKF_TASK_NAME "ERROR_UKF"
#define ACTIVE_MARKER_TASK_NAME "ACTIVEMARKER-DECK"
#define AI_DECK_GAP_TASK_NAME "AI-DECK-GAP"
#define AIDECK_ESP_TX_TASK_NAME "AI-DECK ESP TX"
#define AIDECK_ESP_RX_TASK_NAME "AI-DECK ESP RX"
#define UART2_TASK_NAME "UART2"
#define CRTP_SRV_TASK_NAME "CRTP-SRV"
#define PLATFORM_SRV_TASK_NAME "PLATFORM-SRV"
#define PASSTHROUGH_TASK_NAME "PASSTHROUGH"
#define CPX_RT_UART_TASK_NAME "ROUTER FROM UART2"
#define CPX_RT_INT_TASK_NAME "ROUTER FROM INTERNAL"
#define CPX_TASK_NAME "CPX"
#define APP_TASK_NAME "APP"
#define FLAPPERDECK_TASK_NAME "FLAPPERDECK"


//Task stack sizes
#define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
#define LEDSEQCMD_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ADC_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PM_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_TX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_RX_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
#define CRTP_RXTX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define LOG_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define MEM_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define PARAM_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define SENSORS_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define STABILIZER_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define NRF24LINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ESKYLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define SYSLINK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USBLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART_RX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ZRANGER_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ZRANGER2_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define FLOW_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USDLOG_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USDWRITE_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define PCA9685_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define CMD_HIGH_LEVEL_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define MULTIRANGER_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ACTIVEMARKER_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define AI_DECK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART2_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_SRV_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PLATFORM_SRV_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PASSTHROUGH_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define BQ_OSD_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define GTGPS_DECK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART1_TEST_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART2_TEST_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define LIGHTHOUSE_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define LPS_DECK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define OA_DECK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define KALMAN_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define FLAPPERDECK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ERROR_UKF_TASK_STACKSIZE (4 * configMINIMAL_STACK_SIZE)
#define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
#define LEDSEQCMD_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ADC_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PM_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_TX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_RX_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
#define CRTP_RXTX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define LOG_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define MEM_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define PARAM_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define SENSORS_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define STABILIZER_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define RATE_SUPERVISOR_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define NRF24LINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ESKYLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define SYSLINK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USBLINK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART_RX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define ZRANGER_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ZRANGER2_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define FLOW_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USDLOG_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define USDWRITE_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define PCA9685_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define CMD_HIGH_LEVEL_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define MULTIRANGER_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ACTIVEMARKER_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define AI_DECK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART2_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define CRTP_SRV_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PLATFORM_SRV_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define PASSTHROUGH_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define BQ_OSD_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define GTGPS_DECK_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART1_TEST_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART2_TEST_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define LIGHTHOUSE_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define LPS_DECK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define OA_DECK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define KALMAN_TASK_STACKSIZE (3 * configMINIMAL_STACK_SIZE)
#define FLAPPERDECK_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
#define ERROR_UKF_TASK_STACKSIZE (4 * configMINIMAL_STACK_SIZE)

//The radio channel. From 0 to 125
#define RADIO_CHANNEL 80
Expand Down
31 changes: 25 additions & 6 deletions src/modules/src/stabilizer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ static ControllerType controllerType;
static STATS_CNT_RATE_DEFINE(stabilizerRate, 500);
static rateSupervisor_t rateSupervisorContext;
static bool rateWarningDisplayed = false;
SemaphoreHandle_t xRateSupervisorSemaphore;

static struct {
// position - mm
Expand Down Expand Up @@ -118,8 +119,10 @@ static struct {
} setpointCompressed;

STATIC_MEM_TASK_ALLOC(stabilizerTask, STABILIZER_TASK_STACKSIZE);
STATIC_MEM_TASK_ALLOC(rateSupervisorTask, RATE_SUPERVISOR_TASK_STACKSIZE);

static void stabilizerTask(void* param);
static void rateSupervisorTask(void* param);

static void calcSensorToOutputLatency(const sensorData_t *sensorData)
{
Expand Down Expand Up @@ -254,6 +257,25 @@ static void controlMotors(const control_t* control) {
setMotorRatios(&motorPwm);
}

void rateSupervisorTask(void *pvParameters) {
while (1) {
// Wait for the semaphore to be given by the stabilizerTask
if (xSemaphoreTake(xRateSupervisorSemaphore, M2T(2000)) == pdTRUE) {
// Validate the rate
if (!rateSupervisorValidate(&rateSupervisorContext, xTaskGetTickCount())) {
if (!rateWarningDisplayed) {
DEBUG_PRINT("WARNING: stabilizer loop rate is off (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
rateWarningDisplayed = true;
}
}
} else {
// Handle the case where the semaphore was not given within the timeout
DEBUG_PRINT("ERROR: stabilizerTask is blocking\n");
ASSERT(false); // For safety, assert if the stabilizer task is blocking to ensure motor shutdown
}
}
}

/* The stabilizer loop runs at 1kHz. It is the
* responsibility of the different functions to run slower by skipping call
* (ie. returning without modifying the output structure).
Expand All @@ -280,6 +302,8 @@ static void stabilizerTask(void* param)
systemWaitStart();
DEBUG_PRINT("Starting stabilizer loop\n");
rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1);
xRateSupervisorSemaphore = xSemaphoreCreateBinary();
STATIC_MEM_TASK_CREATE(rateSupervisorTask, rateSupervisorTask, RATE_SUPERVISOR_TASK_NAME, NULL, RATE_SUPERVISOR_TASK_PRI);

while(1) {
// The sensor should unlock at 1kHz
Expand Down Expand Up @@ -342,12 +366,7 @@ static void stabilizerTask(void* param)
stabilizerStep++;
STATS_CNT_RATE_EVENT(&stabilizerRate);

if (!rateSupervisorValidate(&rateSupervisorContext, xTaskGetTickCount())) {
if (!rateWarningDisplayed) {
DEBUG_PRINT("WARNING: stabilizer loop rate is off (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
rateWarningDisplayed = true;
}
}
xSemaphoreGive(xRateSupervisorSemaphore);
}
#ifdef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT
motorsBurstDshot();
Expand Down

0 comments on commit 3a91774

Please sign in to comment.