Skip to content

Commit

Permalink
AP_HAL_ESP32: Migration of ESP32 targets from idf 4.4 to 5.3 consisti…
Browse files Browse the repository at this point in the history
…ng of:

- modification of CMakeLists to use new component names of idf 5.3;
- removing big sdkconfig, which changes a lot, when upgrading idf and to use sdkconfig.defaults, which contain only non default defines;
- Updated idf installation packages list, according to espressif documentation;
- Updated README.md to reflect changes in sdkconfig handling;
- Fixed WDT in Scheduler, it was broken with idf 5.3;
- fixed compilation issues with GCC 13 (which is used by idf 5.3);
- fixed bug in case when HAL_ESP32_WIFI defined as 0 (disable wifi)
- Added ESP32 targets sdkconfig (auto generated) to .gitignore
  • Loading branch information
arg7 authored and tpwrules committed Sep 30, 2024
1 parent a8ecc23 commit 4cf6f7e
Show file tree
Hide file tree
Showing 19 changed files with 195 additions and 2,449 deletions.
4 changes: 4 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -171,3 +171,7 @@ ENV/
env.bak/
venv.bak/
autotest_result_*_junit.xml

# Ignore ESP-IDF SDK defines
/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig
/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig
3 changes: 2 additions & 1 deletion libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,14 @@
#include <AP_HAL/SIMState.h>
#endif


static ESP32::UARTDriver cons(0);
#ifdef HAL_ESP32_WIFI
#if HAL_ESP32_WIFI == 1
static ESP32::WiFiDriver serial1Driver; //tcp, client should connect to 192.168.4.1 port 5760
#elif HAL_ESP32_WIFI == 2
static ESP32::WiFiUdpDriver serial1Driver; //udp
#else
static Empty::UARTDriver serial1Driver;
#endif
#else
static Empty::UARTDriver serial1Driver;
Expand Down
22 changes: 10 additions & 12 deletions libraries/AP_HAL_ESP32/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,11 @@ or
Tools/environment_install/install-prereqs-arch.sh
```

3. install esp-idf python deps:
2. install esp-idf python deps:

```
# from: https://docs.espressif.com/projects/esp-idf/en/latest/esp32/get-started/linux-setup.html
sudo apt-get install git wget flex bison gperf python-setuptools cmake ninja-build ccache libffi-dev libssl-dev dfu-util
sudo apt-get install git wget flex bison gperf cmake ninja-build ccache libffi-dev libssl-dev dfu-util
sudo apt-get install python3 python3-pip python3-setuptools
sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10
Expand Down Expand Up @@ -61,15 +61,20 @@ TIPS:

- we use toolchain and esp-idf from espressif , as a 'git submodule', so no need to preinstall etc.
https://docs.espressif.com/projects/esp-idf/en/latest/get-started/ -
(note we currently use https://github.com/espressif/esp-idf/tree/release/v4.0 )
(note we currently use https://github.com/espressif/esp-idf/tree/release/v5.3 )


- if you get compile error/s to do with CONFIG... such as
in expansion of macro 'configSUPPORT_STATIC_ALLOCATION'
warning: "CONFIG_SUPPORT_STATIC_ALLOCATION" is not defined

this means your 'sdkconfig' file that the IDF relies on is perhaps a bit out of date or out of sync with your IDF.
So double check you are using the correct IDF version ( buzz's branch uses v3.3 , sh83's probably does not.. and then if you are sure:

You can simply remove sdkconfig file and idf build system will recreate it using sdkconfig.defaults, which should fix the problem.

If you need to change sdkconfig, you can edit sdkconfig manually or to use ninja menuconfig:

So double check you are using the correct IDF version:
```
cd build/esp32{BOARD}/esp-idf_build
ninja menuconfig
Expand All @@ -81,11 +86,7 @@ press [tab] then enter on the [exit] box to exit the app
done. the 'sdkconfig' file in this folder should have been updated
cd ../../../..

OR locate the 'libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig' and delete it, as it should call back to the 'sdkconfig.defaults' file if its not there.

'cd libraries/AP_HAL_ESP32/targets/esp32/esp-idf ; idf.py defconfig' is the command that updates it, but that shouldn't be needed manually, we don't think.

... try ./waf plane"
If you want to make changes to sdkconfig (sdkconfig is in git ignore list) permanent and to commit them back in git, you should edit sdkconfig.defaults manually or to use ninja save-defconfig tool after menuconfig.

5. Recommanded way to flash the firmware :
```
Expand Down Expand Up @@ -114,9 +115,6 @@ ESPTOOL_BAUD=921600

You can find more info here : [ESPTOOL](https://github.com/espressif/esptool)

You can also find the cmake esp-idf project at `libraries/AP_HAL_ESP32/targets/esp32/esp-idf` for idf.py command. But see next section to understand how ardupilot is compiled on ESP32.


For flashing from another machine you need the following files:
```
build/<board>/esp-idf_build/bootloader/bootloader.bin
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_ESP32/SPIDevice.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ SPIDevice::SPIDevice(SPIBus &_bus, SPIDeviceDesc &_device_desc)
set_device_bus(bus.bus);
set_device_address(_device_desc.device);
set_speed(AP_HAL::Device::SPEED_LOW);
gpio_pad_select_gpio(device_desc.cs);
esp_rom_gpio_pad_select_gpio(device_desc.cs);
gpio_set_direction(device_desc.cs, GPIO_MODE_OUTPUT);
gpio_set_level(device_desc.cs, 1);

Expand Down
58 changes: 24 additions & 34 deletions libraries/AP_HAL_ESP32/Scheduler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,6 @@
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "soc/rtc_wdt.h"
#include "esp_int_wdt.h"
#include "esp_task_wdt.h"

#include <AP_HAL/AP_HAL.h>
Expand All @@ -38,39 +36,33 @@ using namespace ESP32;
extern const AP_HAL::HAL& hal;

bool Scheduler::_initialized = true;
TaskHandle_t idle_0 = NULL;
TaskHandle_t idle_1 = NULL;

Scheduler::Scheduler()
{
_initialized = false;
}

void disableCore0WDT()
Scheduler::~Scheduler()
{
idle_0 = xTaskGetIdleTaskHandleForCPU(0);
if (idle_0 == NULL || esp_task_wdt_delete(idle_0) != ESP_OK) {
//print("Failed to remove Core 0 IDLE task from WDT");
if (_initialized) {
esp_task_wdt_deinit();
}
}

void disableCore1WDT()
{
idle_1 = xTaskGetIdleTaskHandleForCPU(1);
if (idle_1 == NULL || esp_task_wdt_delete(idle_1) != ESP_OK) {
//print("Failed to remove Core 1 IDLE task from WDT");
}
}
void enableCore0WDT()
void Scheduler::wdt_init(uint32_t timeout, uint32_t core_mask)
{
if (idle_0 != NULL && esp_task_wdt_add(idle_0) != ESP_OK) {
//print("Failed to add Core 0 IDLE task to WDT");
esp_task_wdt_config_t config = {
.timeout_ms = timeout,
.idle_core_mask = core_mask,
.trigger_panic = true
};

if ( ESP_OK != esp_task_wdt_init(&config) ) {
printf("esp_task_wdt_init() failed\n");
}
}
void enableCore1WDT()
{
if (idle_1 != NULL && esp_task_wdt_add(idle_1) != ESP_OK) {
//print("Failed to add Core 1 IDLE task to WDT");

if (ESP_OK != esp_task_wdt_add(NULL)) {
printf("esp_task_wdt_add(NULL) failed");
}
}

Expand All @@ -81,11 +73,6 @@ void Scheduler::init()
printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__);
#endif

// disable wd while booting, as things like mounting the sd-card in the io thread can take a while, especially if there isn't hardware attached.
disableCore0WDT(); //FASTCPU
disableCore1WDT(); //SLOWCPU


hal.console->printf("%s:%d running with CONFIG_FREERTOS_HZ=%d\n", __PRETTY_FUNCTION__, __LINE__,CONFIG_FREERTOS_HZ);

// keep main tasks that need speed on CPU 0
Expand Down Expand Up @@ -140,11 +127,6 @@ void Scheduler::init()
}

// xTaskCreatePinnedToCore(_print_profile, "APM_PROFILE", IO_SS, this, IO_PRIO, nullptr,SLOWCPU);

hal.console->printf("OK Sched Init, enabling WD\n");
enableCore0WDT(); //FASTCPU
//enableCore1WDT(); //we don't enable WD on SLOWCPU right now.

}

template <typename T>
Expand Down Expand Up @@ -232,7 +214,7 @@ void IRAM_ATTR Scheduler::delay(uint16_t ms)
void IRAM_ATTR Scheduler::delay_microseconds(uint16_t us)
{
if (in_main_thread() && us < 100) {
ets_delay_us(us);
esp_rom_delay_us(us);
} else { // Minimum delay for FreeRTOS is 1ms
uint32_t tick = portTICK_PERIOD_MS * 1000;

Expand Down Expand Up @@ -571,6 +553,10 @@ void IRAM_ATTR Scheduler::_main_thread(void *arg)

sched->set_system_initialized();

//initialize WTD for current thread on FASTCPU, all cores will be (1 << CONFIG_FREERTOS_NUMBER_OF_CORES) - 1
wdt_init( TWDT_TIMEOUT_MS, 1 << FASTCPU ); // 3 sec


#ifdef SCHEDDEBUG
printf("%s:%d initialised\n", __PRETTY_FUNCTION__, __LINE__);
#endif
Expand All @@ -581,6 +567,10 @@ void IRAM_ATTR Scheduler::_main_thread(void *arg)
// run stats periodically
sched->print_stats();
sched->print_main_loop_rate();

if (ESP_OK != esp_task_wdt_reset()) {
printf("esp_task_wdt_reset() failed\n");
};
}
}

5 changes: 4 additions & 1 deletion libraries/AP_HAL_ESP32/Scheduler.h
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,15 @@

#define ESP32_SCHEDULER_MAX_TIMER_PROCS 10
#define ESP32_SCHEDULER_MAX_IO_PROCS 10

#define TWDT_TIMEOUT_MS 3000

/* Scheduler implementation: */
class ESP32::Scheduler : public AP_HAL::Scheduler
{

public:
Scheduler();
~Scheduler();
/* AP_HAL::Scheduler methods */
void init() override;
void set_callbacks(AP_HAL::HAL::Callbacks *cb)
Expand Down Expand Up @@ -130,6 +131,8 @@ class ESP32::Scheduler : public AP_HAL::Scheduler

static void test_esc(void* arg);

static void wdt_init(uint32_t timeout, uint32_t core_mask);

bool _in_timer_proc;
void _run_timers();
Semaphore _timer_sem;
Expand Down
8 changes: 2 additions & 6 deletions libraries/AP_HAL_ESP32/SdCard.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,6 @@
#include <sys/types.h>
#include "SPIDevice.h"

#include "soc/rtc_wdt.h"

#ifdef HAL_ESP32_SDCARD

#if CONFIG_IDF_TARGET_ESP32S2 ||CONFIG_IDF_TARGET_ESP32C3
Expand Down Expand Up @@ -254,8 +252,8 @@ void mount_sdcard()
{
mount_sdcard_spi();
}
#endif // end spi

#endif // end spi

bool sdcard_retry(void)
{
Expand All @@ -265,16 +263,14 @@ bool sdcard_retry(void)
return sdcard_running;
}


void unmount_sdcard()
{
if (card != nullptr) {
esp_vfs_fat_sdmmc_unmount();
esp_vfs_fat_sdcard_unmount( "/SDCARD", card);
}
sdcard_running = false;
}


#else
// empty impl's
void mount_sdcard()
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_ESP32/Semaphores.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ void BinarySemaphore::signal_ISR()
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
portYIELD_FROM_ISR_ARG(xHigherPriorityTaskWoken);
}

BinarySemaphore::~BinarySemaphore(void)
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_ESP32/UARTDriver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
uart_desc[uart_num].rx,
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
//uart_driver_install(p, 2*UART_FIFO_LEN, 0, 0, nullptr, 0);
uart_driver_install(p, 2*UART_FIFO_LEN, 0, 0, nullptr, 0);
uart_driver_install(p, 2*UART_HW_FIFO_LEN(p), 0, 0, nullptr, 0);
_readbuf.set_size(RX_BUF_SIZE);
_writebuf.set_size(TX_BUF_SIZE);

Expand Down
1 change: 1 addition & 0 deletions libraries/AP_HAL_ESP32/Util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "esp_heap_caps.h"
#include <AP_Common/ExpandingString.h>

#include "esp_mac.h"

extern const AP_HAL::HAL& hal;

Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_HAL_ESP32/WiFiDriver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ bool WiFiDriver::start_listen()
bool WiFiDriver::try_accept()
{
struct sockaddr_in sourceAddr;
uint addrLen = sizeof(sourceAddr);
socklen_t addrLen = sizeof(sourceAddr);
short i = available_socket();
if (i != WIFI_MAX_CONNECTION) {
socket_list[i] = accept(accept_socket, (struct sockaddr *)&sourceAddr, &addrLen);
Expand Down Expand Up @@ -246,7 +246,7 @@ static void _sta_event_handler(void* arg, esp_event_base_t event_base,
void WiFiDriver::initialize_wifi()
{
#ifndef WIFI_PWD
#default WIFI_PWD "ardupilot1"
#define WIFI_PWD "ardupilot1"
#endif
//Initialize NVS
esp_err_t ret = nvs_flash_init();
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
#include "lwip/sys.h"
#include "lwip/netdb.h"

#include "soc/rtc_wdt.h"
#include "freertos/idf_additions.h"

using namespace ESP32;

Expand Down Expand Up @@ -215,7 +215,7 @@ static void _sta_event_handler(void* arg, esp_event_base_t event_base,
void WiFiUdpDriver::initialize_wifi()
{
#ifndef WIFI_PWD
#default WIFI_PWD "ardupilot1"
#define WIFI_PWD "ardupilot1"
#endif
//Initialize NVS
esp_err_t ret = nvs_flash_init();
Expand Down
5 changes: 3 additions & 2 deletions libraries/AP_HAL_ESP32/i2c_sw.c
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,9 @@ void i2c_init(_i2c_bus_t* bus)
case 160: bus->delay = _i2c_delays[bus->speed][1]; break;
case 240: bus->delay = _i2c_delays[bus->speed][2]; break;
default : DEBUG("i2c I2C software implementation is not "
"supported for this CPU frequency: %d MHz\n",
ets_get_cpu_frequency());
"supported for this CPU frequency: %u MHz\n",
(unsigned int)ets_get_cpu_frequency()
);
return;
}

Expand Down
Loading

0 comments on commit 4cf6f7e

Please sign in to comment.