diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp index 66be0ef5ee..f4d26dde3a 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.cpp +++ b/libraries/AP_HAL_ESP32/Scheduler.cpp @@ -28,6 +28,7 @@ #include "esp_task_wdt.h" #include +#include #include //#define SCHEDULERDEBUG 1 @@ -50,6 +51,7 @@ void disableCore0WDT() //print("Failed to remove Core 0 IDLE task from WDT"); } } + void disableCore1WDT() { TaskHandle_t idle_1 = xTaskGetIdleTaskHandleForCPU(1); @@ -292,9 +294,14 @@ void Scheduler::_timer_thread(void *arg) printf("%s:%d start\n", __PRETTY_FUNCTION__, __LINE__); #endif Scheduler *sched = (Scheduler *)arg; + +#if HAL_INS_DEFAULT != HAL_INS_NONE + // wait to ensure INS system inits unless using HAL_INS_NONE while (!_initialized) { sched->delay_microseconds(1000); } +#endif + #ifdef SCHEDDEBUG printf("%s:%d initialised\n", __PRETTY_FUNCTION__, __LINE__); #endif @@ -507,17 +514,25 @@ void Scheduler::print_stats(void) // printf("loop_rate_hz: %d",get_loop_rate_hz()); } +// Run every 10s +void Scheduler::print_main_loop_rate(void) +{ + static int64_t last_run = 0; + if (AP_HAL::millis64() - last_run > 10000) { + last_run = AP_HAL::millis64(); + const float actual_loop_rate = AP::scheduler().get_filtered_loop_rate_hz(); + const uint16_t expected_loop_rate = AP::scheduler().get_loop_rate_hz(); + hal.console->printf("loop_rate: actual: %uHz, expected: %uHz\n", + (uint16_t)actual_loop_rate, (uint16_t)expected_loop_rate); + } +} + void IRAM_ATTR Scheduler::_main_thread(void *arg) { #ifdef SCHEDDEBUG printf("%s:%d start\n", __PRETTY_FUNCTION__, __LINE__); #endif Scheduler *sched = (Scheduler *)arg; - hal.serial(0)->begin(115200); - hal.serial(1)->begin(57600); - hal.serial(2)->begin(57600); - //hal.uartC->begin(921600); - hal.serial(3)->begin(115200); #ifndef HAL_DISABLE_ADC_DRIVER hal.analogin->init(); @@ -535,7 +550,9 @@ void IRAM_ATTR Scheduler::_main_thread(void *arg) sched->callbacks->loop(); sched->delay_microseconds(250); - sched->print_stats(); // only runs every 60 seconds. + // run stats periodically + sched->print_stats(); + sched->print_main_loop_rate(); } } diff --git a/libraries/AP_HAL_ESP32/Scheduler.h b/libraries/AP_HAL_ESP32/Scheduler.h index eb676b8bdb..237f9532ea 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.h +++ b/libraries/AP_HAL_ESP32/Scheduler.h @@ -49,6 +49,8 @@ public: bool is_system_initialized() override; void print_stats(void) ; + void print_main_loop_rate(void); + uint16_t get_loop_rate_hz(void); AP_Int16 _active_loop_rate_hz; AP_Int16 _loop_rate_hz; diff --git a/libraries/AP_HAL_ESP32/boards/esp32empty.h b/libraries/AP_HAL_ESP32/boards/esp32empty.h index 4ca5f00608..33f5d7c219 100644 --- a/libraries/AP_HAL_ESP32/boards/esp32empty.h +++ b/libraries/AP_HAL_ESP32/boards/esp32empty.h @@ -23,108 +23,128 @@ #define HAL_ESP32_BOARD_NAME "esp32-empty" -#define TRUE 1 -#define FALSE 0 +#define TRUE 1 +#define FALSE 0 //Protocols -//list of protocols/enum: ardupilot/libraries/AP_SerialManager/AP_SerialManager.h -//default protocols: ardupilot/libraries/AP_SerialManager/AP_SerialManager.cpp -//ESP32 serials: AP_HAL_ESP32/HAL_ESP32_Class.cpp +// list of protocols/enum: ardupilot/libraries/AP_SerialManager/AP_SerialManager.h +// default protocols: ardupilot/libraries/AP_SerialManager/AP_SerialManager.cpp +// ESP32 serials: AP_HAL_ESP32/HAL_ESP32_Class.cpp -//#define DEFAULT_SERIAL0_PROTOCOL SerialProtocol_MAVLink2 //A UART0: Always: Console, MAVLink2 -//#define DEFAULT_SERIAL0_BAUD AP_SERIALMANAGER_CONSOLE_BAUD/1000 //115200 +//#define DEFAULT_SERIAL0_PROTOCOL SerialProtocol_MAVLink2 //A UART0: Always: Console, MAVLink2 +//#define DEFAULT_SERIAL0_BAUD AP_SERIALMANAGER_CONSOLE_BAUD/1000 //115200 -//#define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_MAVLink2 //C WiFi: TCP, UDP, or disable (depends on HAL_ESP32_WIFI) -//#define DEFAULT_SERIAL1_BAUD AP_SERIALMANAGER_MAVLINK_BAUD/1000 //57600 +//#define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_MAVLink2 //C WiFi: TCP, UDP, or disable (depends on HAL_ESP32_WIFI) +//#define DEFAULT_SERIAL1_BAUD AP_SERIALMANAGER_MAVLINK_BAUD/1000 //57600 -#define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_MAVLink2 //D UART2: Always: MAVLink2 on ESP32 -//#define DEFAULT_SERIAL2_BAUD AP_SERIALMANAGER_MAVLINK_BAUD/1000 //57600 +#define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_MAVLink2 //D UART2 +#define DEFAULT_SERIAL2_BAUD AP_SERIALMANAGER_MAVLINK_BAUD/1000 //57600 -#define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_GPS //B UART1: GPS1 -//#define DEFAULT_SERIAL4_BAUD AP_SERIALMANAGER_GPS_BAUD/1000 //38400, Can not define default baudrate here (by config only) +#define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_GPS //B UART1: GPS1 +#define DEFAULT_SERIAL3_BAUD AP_SERIALMANAGER_GPS_BAUD/1000 //38400, Can not define default baudrate here (by config only) +//#define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None //B +//#define DEFAULT_SERIAL3_BAUD (115200/1000) -#define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_None //E -//#define DEFAULT_SERIAL4_BAUD AP_SERIALMANAGER_GPS_BAUD/1000 //38400, Can not define default baudrate here (by config only) +#define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_None //E +#define DEFAULT_SERIAL5_BAUD (115200/1000) -#define DEFAULT_SERIAL5_PROTOCOL SerialProtocol_None //F -#define DEFAULT_SERIAL5_BAUD (115200/1000) +#define DEFAULT_SERIAL5_PROTOCOL SerialProtocol_None //F +#define DEFAULT_SERIAL5_BAUD (115200/1000) -#define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_None //G -#define DEFAULT_SERIAL6_BAUD (115200/1000) +#define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_None //G +#define DEFAULT_SERIAL6_BAUD (115200/1000) -#define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_None //H -#define DEFAULT_SERIAL7_BAUD (115200/1000) +#define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_None //H +#define DEFAULT_SERIAL7_BAUD (115200/1000) -#define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_None //I -#define DEFAULT_SERIAL8_BAUD (115200/1000) +#define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_None //I +#define DEFAULT_SERIAL8_BAUD (115200/1000) -#define DEFAULT_SERIAL9_PROTOCOL SerialProtocol_None //J -#define DEFAULT_SERIAL9_BAUD (115200/1000) +#define DEFAULT_SERIAL9_PROTOCOL SerialProtocol_None //J +#define DEFAULT_SERIAL9_BAUD (115200/1000) //Inertial sensors -//#define HAL_INS_DEFAULT HAL_INS_MPU9250_I2C -//#define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) -//#define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensense, 0, 0x68, ROTATION_NONE) +#define HAL_INS_DEFAULT HAL_INS_NONE +//#define HAL_INS_DEFAULT HAL_INS_MPU9250_I2C +//#define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) +//#define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensense, 0, 0x68, ROTATION_NONE) //I2C Buses -#define HAL_ESP32_I2C_BUSES {.port=I2C_NUM_0, .sda=GPIO_NUM_13, .scl=GPIO_NUM_14, .speed=400*KHZ, .internal=true, .soft=true} +#define HAL_ESP32_I2C_BUSES {.port=I2C_NUM_0, .sda=GPIO_NUM_13, .scl=GPIO_NUM_14, .speed=400*KHZ, .internal=true, .soft=true} //SPI Buses -#define HAL_ESP32_SPI_BUSES {} +#define HAL_ESP32_SPI_BUSES {} //SPI Devices -#define HAL_ESP32_SPI_DEVICES {} +#define HAL_ESP32_SPI_DEVICES {} //RCIN -#define HAL_ESP32_RCIN GPIO_NUM_36 +#define HAL_ESP32_RCIN GPIO_NUM_36 //RCOUT -#define HAL_ESP32_RCOUT { GPIO_NUM_25, GPIO_NUM_27, GPIO_NUM_33, GPIO_NUM_32, GPIO_NUM_22, GPIO_NUM_21 } +#define HAL_ESP32_RCOUT {GPIO_NUM_25, GPIO_NUM_27, GPIO_NUM_33, GPIO_NUM_32, GPIO_NUM_22, GPIO_NUM_21} + +//AIRSPEED +#define AP_AIRSPEED_ENABLED 0 +#define AP_AIRSPEED_ANALOG_ENABLED 0 +#define AP_AIRSPEED_BACKEND_DEFAULT_ENABLED 0 //BAROMETER -#define HAL_BARO_ALLOW_INIT_NO_BARO 1 +#define HAL_BARO_ALLOW_INIT_NO_BARO 1 + +//IMU +// #define AP_INERTIALSENSOR_ENABLED 1 +// #define AP_INERTIALSENSOR_KILL_IMU_ENABLED 0 //COMPASS +#define AP_COMPASS_ENABLE_DEFAULT 0 #define ALLOW_ARM_NO_COMPASS +//See boards.py +#ifndef ENABLE_HEAP +#define ENABLE_HEAP 1 +#endif + //WIFI -#define HAL_ESP32_WIFI 1 //1-TCP, 2-UDP, comment this line = without wifi -#define WIFI_SSID "ardupilot-empty" -#define WIFI_PWD "ardupilot-empty" +#define HAL_ESP32_WIFI 1 //1-TCP, 2-UDP, comment this line = without wifi +#define WIFI_SSID "ardupilot-esp32" +#define WIFI_PWD "ardupilot-esp32" //UARTs +// UART_NUM_0 and UART_NUM_2 are configured to use defaults #define HAL_ESP32_UART_DEVICES \ {.port=UART_NUM_0, .rx=GPIO_NUM_3 , .tx=GPIO_NUM_1 },\ {.port=UART_NUM_1, .rx=GPIO_NUM_34, .tx=GPIO_NUM_18},\ - {.port=UART_NUM_2, .rx=GPIO_NUM_35, .tx=GPIO_NUM_19} + {.port=UART_NUM_2, .rx=GPIO_NUM_16, .tx=GPIO_NUM_17} //ADC -#define HAL_DISABLE_ADC_DRIVER 1 -#define HAL_USE_ADC 0 +#define HAL_DISABLE_ADC_DRIVER 1 +#define HAL_USE_ADC 0 //LED -#define DEFAULT_NTF_LED_TYPES Notify_LED_None +#define DEFAULT_NTF_LED_TYPES Notify_LED_None //RMT pin number -#define HAL_ESP32_RMT_RX_PIN_NUMBER 4 +#define HAL_ESP32_RMT_RX_PIN_NUMBER 4 //SD CARD -// Do u want to use mmc or spi mode for the sd card, this is board specific , -// as mmc uses specific pins but is quicker, -// and spi is more flexible pinouts.... dont forget vspi/hspi should be selected to NOT conflict with HAL_ESP32_SPI_BUSES +// Do u want to use mmc or spi mode for the sd card, this is board specific, +// as mmc uses specific pins but is quicker, +// and spi is more flexible pinouts.... +// dont forget vspi/hspi should be selected to NOT conflict with HAL_ESP32_SPI_BUSES //#define HAL_ESP32_SDCARD //after enabled, uncomment one of below //#define HAL_ESP32_SDMMC -//#define HAL_ESP32_SDSPI {.host=VSPI_HOST, .dma_ch=2, .mosi=GPIO_NUM_2, .miso=GPIO_NUM_15, .sclk=GPIO_NUM_26, .cs=GPIO_NUM_21} +//#define HAL_ESP32_SDSPI {.host=VSPI_HOST, .dma_ch=2, .mosi=GPIO_NUM_2, .miso=GPIO_NUM_15, .sclk=GPIO_NUM_26, .cs=GPIO_NUM_21} -#define HAL_LOGGING_FILESYSTEM_ENABLED 0 -#define HAL_LOGGING_DATAFLASH_ENABLED 0 -#define HAL_LOGGING_MAVLINK_ENABLED 0 +#define HAL_LOGGING_FILESYSTEM_ENABLED 0 +#define HAL_LOGGING_DATAFLASH_ENABLED 0 +#define HAL_LOGGING_MAVLINK_ENABLED 0 -#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" -#define HAL_BOARD_STORAGE_DIRECTORY "/SDCARD/APM/STORAGE" -#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" -#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_STORAGE_DIRECTORY "/SDCARD/APM/STORAGE" +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" -#define HAL_LOGGING_BACKENDS_DEFAULT 1 +#define HAL_LOGGING_BACKENDS_DEFAULT 1