diff --git a/src/config/config.h b/src/config/config.h index 6988267880..6975aa191d 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -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 @@ -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 diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 0a3c1d8cfd..5a1956c90d 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -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 @@ -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) { @@ -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). @@ -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 @@ -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();