From bb8998bdef24f81c27a6bee04a743bc3055312a2 Mon Sep 17 00:00:00 2001 From: Buzz Date: Wed, 27 Oct 2021 18:43:28 +1000 Subject: [PATCH] AP_HAL_ESP32: new HAL layer for esp32 see libraries/AP_HAL_ESP32/README.md for more. Author: Charles Villard Author: Buzz --- libraries/AP_HAL_ESP32/AP_HAL_ESP32.h | 7 + libraries/AP_HAL_ESP32/AnalogIn.cpp | 369 +++++++ libraries/AP_HAL_ESP32/AnalogIn.h | 104 ++ libraries/AP_HAL_ESP32/DeviceBus.cpp | 155 +++ libraries/AP_HAL_ESP32/DeviceBus.h | 55 + libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp | 114 ++ libraries/AP_HAL_ESP32/HAL_ESP32_Class.h | 27 + libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h | 22 + libraries/AP_HAL_ESP32/I2CDevice.cpp | 193 ++++ libraries/AP_HAL_ESP32/I2CDevice.h | 147 +++ libraries/AP_HAL_ESP32/OSD.cpp | 309 ++++++ libraries/AP_HAL_ESP32/Profile.cpp | 87 ++ libraries/AP_HAL_ESP32/Profile.h | 18 + libraries/AP_HAL_ESP32/RCInput.cpp | 129 +++ libraries/AP_HAL_ESP32/RCInput.h | 53 + libraries/AP_HAL_ESP32/RCOutput.cpp | 349 +++++++ libraries/AP_HAL_ESP32/RCOutput.h | 139 +++ libraries/AP_HAL_ESP32/README.md | 461 +++++++++ libraries/AP_HAL_ESP32/RmtSigReader.cpp | 76 ++ libraries/AP_HAL_ESP32/RmtSigReader.h | 42 + libraries/AP_HAL_ESP32/SPIDevice.cpp | 238 +++++ libraries/AP_HAL_ESP32/SPIDevice.h | 99 ++ libraries/AP_HAL_ESP32/Scheduler.cpp | 500 +++++++++ libraries/AP_HAL_ESP32/Scheduler.h | 122 +++ libraries/AP_HAL_ESP32/SdCard.cpp | 294 ++++++ libraries/AP_HAL_ESP32/SdCard.h | 20 + libraries/AP_HAL_ESP32/Semaphores.cpp | 75 ++ libraries/AP_HAL_ESP32/Semaphores.h | 36 + libraries/AP_HAL_ESP32/SoftSigReaderInt.cpp | 177 ++++ libraries/AP_HAL_ESP32/SoftSigReaderInt.h | 71 ++ libraries/AP_HAL_ESP32/SoftSigReaderRMT.cpp | 158 +++ libraries/AP_HAL_ESP32/SoftSigReaderRMT.h | 47 + libraries/AP_HAL_ESP32/Storage.cpp | 217 ++++ libraries/AP_HAL_ESP32/Storage.h | 68 ++ libraries/AP_HAL_ESP32/UARTDriver.cpp | 216 ++++ libraries/AP_HAL_ESP32/UARTDriver.h | 96 ++ libraries/AP_HAL_ESP32/Util.cpp | 296 ++++++ libraries/AP_HAL_ESP32/Util.h | 101 ++ libraries/AP_HAL_ESP32/WiFiDriver.cpp | 299 ++++++ libraries/AP_HAL_ESP32/WiFiDriver.h | 79 ++ libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp | 250 +++++ libraries/AP_HAL_ESP32/WiFiUdpDriver.h | 77 ++ libraries/AP_HAL_ESP32/boards/defaults.parm | 3 + libraries/AP_HAL_ESP32/boards/esp32buzz.h | 183 ++++ libraries/AP_HAL_ESP32/boards/esp32diy.h | 113 ++ libraries/AP_HAL_ESP32/boards/esp32icarus.h | 53 + .../AP_HAL_ESP32/hwdef/esp32buzz/hwdef.dat | 0 .../AP_HAL_ESP32/hwdef/esp32diy/hwdef.dat | 0 libraries/AP_HAL_ESP32/i2c_sw.c | 725 +++++++++++++ libraries/AP_HAL_ESP32/i2c_sw.h | 430 ++++++++ .../profile/LinkerScriptGenerator.kt | 94 ++ libraries/AP_HAL_ESP32/profile/PROF000.TXT | 0 libraries/AP_HAL_ESP32/profile/README.md | 39 + libraries/AP_HAL_ESP32/profile/ardusub.map | 0 libraries/AP_HAL_ESP32/system.cpp | 63 ++ libraries/AP_HAL_ESP32/targets/.gitignore | 2 + .../targets/esp-idf/CMakeLists.txt | 106 ++ libraries/AP_HAL_ESP32/targets/esp-idf/main.c | 14 + .../AP_HAL_ESP32/targets/esp-idf/sdkconfig | 970 ++++++++++++++++++ libraries/AP_HAL_ESP32/targets/partitions.csv | 5 + libraries/AP_HAL_ESP32/utils/FontConverter.kt | 78 ++ .../utils/profile/LinkerScriptGenerator.kt | 92 ++ .../AP_HAL_ESP32/utils/profile/README.md | 14 + 63 files changed, 9376 insertions(+) create mode 100644 libraries/AP_HAL_ESP32/AP_HAL_ESP32.h create mode 100644 libraries/AP_HAL_ESP32/AnalogIn.cpp create mode 100644 libraries/AP_HAL_ESP32/AnalogIn.h create mode 100644 libraries/AP_HAL_ESP32/DeviceBus.cpp create mode 100644 libraries/AP_HAL_ESP32/DeviceBus.h create mode 100644 libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp create mode 100644 libraries/AP_HAL_ESP32/HAL_ESP32_Class.h create mode 100644 libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h create mode 100644 libraries/AP_HAL_ESP32/I2CDevice.cpp create mode 100644 libraries/AP_HAL_ESP32/I2CDevice.h create mode 100644 libraries/AP_HAL_ESP32/OSD.cpp create mode 100644 libraries/AP_HAL_ESP32/Profile.cpp create mode 100644 libraries/AP_HAL_ESP32/Profile.h create mode 100644 libraries/AP_HAL_ESP32/RCInput.cpp create mode 100644 libraries/AP_HAL_ESP32/RCInput.h create mode 100644 libraries/AP_HAL_ESP32/RCOutput.cpp create mode 100644 libraries/AP_HAL_ESP32/RCOutput.h create mode 100644 libraries/AP_HAL_ESP32/README.md create mode 100644 libraries/AP_HAL_ESP32/RmtSigReader.cpp create mode 100644 libraries/AP_HAL_ESP32/RmtSigReader.h create mode 100644 libraries/AP_HAL_ESP32/SPIDevice.cpp create mode 100644 libraries/AP_HAL_ESP32/SPIDevice.h create mode 100644 libraries/AP_HAL_ESP32/Scheduler.cpp create mode 100644 libraries/AP_HAL_ESP32/Scheduler.h create mode 100644 libraries/AP_HAL_ESP32/SdCard.cpp create mode 100644 libraries/AP_HAL_ESP32/SdCard.h create mode 100644 libraries/AP_HAL_ESP32/Semaphores.cpp create mode 100644 libraries/AP_HAL_ESP32/Semaphores.h create mode 100644 libraries/AP_HAL_ESP32/SoftSigReaderInt.cpp create mode 100644 libraries/AP_HAL_ESP32/SoftSigReaderInt.h create mode 100644 libraries/AP_HAL_ESP32/SoftSigReaderRMT.cpp create mode 100644 libraries/AP_HAL_ESP32/SoftSigReaderRMT.h create mode 100644 libraries/AP_HAL_ESP32/Storage.cpp create mode 100644 libraries/AP_HAL_ESP32/Storage.h create mode 100644 libraries/AP_HAL_ESP32/UARTDriver.cpp create mode 100644 libraries/AP_HAL_ESP32/UARTDriver.h create mode 100644 libraries/AP_HAL_ESP32/Util.cpp create mode 100644 libraries/AP_HAL_ESP32/Util.h create mode 100644 libraries/AP_HAL_ESP32/WiFiDriver.cpp create mode 100644 libraries/AP_HAL_ESP32/WiFiDriver.h create mode 100644 libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp create mode 100644 libraries/AP_HAL_ESP32/WiFiUdpDriver.h create mode 100644 libraries/AP_HAL_ESP32/boards/defaults.parm create mode 100644 libraries/AP_HAL_ESP32/boards/esp32buzz.h create mode 100644 libraries/AP_HAL_ESP32/boards/esp32diy.h create mode 100644 libraries/AP_HAL_ESP32/boards/esp32icarus.h create mode 100644 libraries/AP_HAL_ESP32/hwdef/esp32buzz/hwdef.dat create mode 100644 libraries/AP_HAL_ESP32/hwdef/esp32diy/hwdef.dat create mode 100644 libraries/AP_HAL_ESP32/i2c_sw.c create mode 100644 libraries/AP_HAL_ESP32/i2c_sw.h create mode 100644 libraries/AP_HAL_ESP32/profile/LinkerScriptGenerator.kt create mode 100644 libraries/AP_HAL_ESP32/profile/PROF000.TXT create mode 100644 libraries/AP_HAL_ESP32/profile/README.md create mode 100644 libraries/AP_HAL_ESP32/profile/ardusub.map create mode 100644 libraries/AP_HAL_ESP32/system.cpp create mode 100644 libraries/AP_HAL_ESP32/targets/.gitignore create mode 100644 libraries/AP_HAL_ESP32/targets/esp-idf/CMakeLists.txt create mode 100644 libraries/AP_HAL_ESP32/targets/esp-idf/main.c create mode 100644 libraries/AP_HAL_ESP32/targets/esp-idf/sdkconfig create mode 100644 libraries/AP_HAL_ESP32/targets/partitions.csv create mode 100644 libraries/AP_HAL_ESP32/utils/FontConverter.kt create mode 100644 libraries/AP_HAL_ESP32/utils/profile/LinkerScriptGenerator.kt create mode 100644 libraries/AP_HAL_ESP32/utils/profile/README.md diff --git a/libraries/AP_HAL_ESP32/AP_HAL_ESP32.h b/libraries/AP_HAL_ESP32/AP_HAL_ESP32.h new file mode 100644 index 0000000000..13c0bc6a79 --- /dev/null +++ b/libraries/AP_HAL_ESP32/AP_HAL_ESP32.h @@ -0,0 +1,7 @@ +#pragma once + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + + +#include "HAL_ESP32_Class.h" diff --git a/libraries/AP_HAL_ESP32/AnalogIn.cpp b/libraries/AP_HAL_ESP32/AnalogIn.cpp new file mode 100644 index 0000000000..95ced8ef9a --- /dev/null +++ b/libraries/AP_HAL_ESP32/AnalogIn.cpp @@ -0,0 +1,369 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Charles Villard + */ +#include +#include + +#include +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" +#include "driver/adc.h" +#include "esp_adc_cal.h" +#include "soc/adc_channel.h" + +#if HAL_USE_ADC == TRUE && !defined(HAL_DISABLE_ADC_DRIVER) + +#include "AnalogIn.h" + +#ifndef ESP32_ADC_MAVLINK_DEBUG +// this allows the first 6 analog channels to be reported by mavlink for debugging purposes +#define ESP32_ADC_MAVLINK_DEBUG 0 +#endif + +#include + +#define ANALOGIN_DEBUGGING 0 + +// base voltage scaling for 12 bit 3.3V ADC +#define VOLTAGE_SCALING (3.3f/4096.0f) + +#if ANALOGIN_DEBUGGING +# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +# define Debug(fmt, args ...) +#endif + +// we are limited to using adc1, and it supports 8 channels max, on gpio, in this order: +// ADC1_CH0=D36,ADC1_CH1=D37,ADC1_CH2=D38,ADC1_CH3=D39,ADC1_CH4=D32,ADC1_CH5=D33,ADC1_CH6=D34,ADC1_CH7=D35 +// this driver will only configure the ADCs from a subset of these that the board exposes on pins. + + +extern const AP_HAL::HAL &hal; + +using namespace ESP32; + +/* + scaling table between ADC count and actual input voltage, to account + for voltage dividers on the board. + */ +const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ESP32_ADC_PINS; + +#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE(AnalogIn::pin_config) + + +#define DEFAULT_VREF 1100 //Use adc2_vref_to_gpio() to obtain a better estimate +#define NO_OF_SAMPLES 256 //Multisampling + +static const adc_atten_t atten = ADC_ATTEN_DB_11; + +//ardupin is the ardupilot assigned number, starting from 1-8(max) +// 'pin' and _pin is a macro like 'ADC1_GPIO35_CHANNEL' from board config .h +AnalogSource::AnalogSource(int16_t ardupin,int16_t pin,float scaler, float initial_value, uint8_t unit) : + + _unit(unit), + _ardupin(ardupin), + _pin(pin), + _scaler(scaler), + _value(initial_value), + _latest_value(initial_value), + _sum_count(0), + _sum_value(0) +{ + printf("AnalogIn: adding ardupin:%d-> which is adc1_offset:%d\n", _ardupin,_pin); + + // init the pin now if possible, otherwise doo it later from set_pin + if ( _ardupin != ANALOG_INPUT_NONE ) { + + // dertermine actial gpio from adc offset and configure it + gpio_num_t gpio; + //Configure ADC + if (unit == 1) { + adc1_config_channel_atten((adc1_channel_t)_pin, atten); + adc1_pad_get_io_num((adc1_channel_t)_pin, &gpio); + } else { + adc2_config_channel_atten((adc2_channel_t)_pin, atten); + } + + esp_adc_cal_characteristics_t adc_chars; + esp_adc_cal_characterize(ADC_UNIT_1, atten, ADC_WIDTH_BIT_12, DEFAULT_VREF, &adc_chars); + printf("AnalogIn: determined actual gpio as: %d\n", gpio); + + _gpio = gpio;// save it for later + + } +} + + +float AnalogSource::read_average() +{ + if ( _ardupin == ANALOG_INPUT_NONE ) { + return 0.0f; + } + + WITH_SEMAPHORE(_semaphore); + + if (_sum_count == 0) { + uint32_t adc_reading = 0; + //Multisampling + for (int i = 0; i < NO_OF_SAMPLES; i++) { + if (_unit == 1) { + adc_reading += adc1_get_raw((adc1_channel_t)_pin); + } else { + int raw; + adc2_get_raw((adc2_channel_t)_pin, ADC_WIDTH_BIT_12, &raw); + adc_reading += raw; + } + } + adc_reading /= NO_OF_SAMPLES; + return adc_reading; + } + + _value = _sum_value / _sum_count; + _sum_value = 0; + _sum_count = 0; + + return _value; +} + +float AnalogSource::read_latest() +{ + return _latest_value; +} + +//_scaler scaling from ADC count to Volts + +/* + return voltage in Volts + */ +float AnalogSource::voltage_average() +{ + return _scaler * read_average(); +} + +/* + return voltage in Volts + */ +float AnalogSource::voltage_latest() +{ + return _scaler * read_latest(); +} + +float AnalogSource::voltage_average_ratiometric() +{ + return _scaler * read_latest(); +} + +// ardupin +bool AnalogSource::set_pin(uint8_t ardupin) +{ + + if (_ardupin == ardupin) { + return true; + } + + int8_t pinconfig_offset = AnalogIn::find_pinconfig(ardupin); + if (pinconfig_offset == -1 ) { + hal.console->printf("AnalogIn: sorry set_pin() can't determine ADC1 offset from ardupin : %d \n",ardupin); + return false; + } + + int16_t newgpioAdcPin = AnalogIn::pin_config[(uint8_t)pinconfig_offset].channel; + float newscaler = AnalogIn::pin_config[(uint8_t)pinconfig_offset].scaling; + + if (_pin == newgpioAdcPin) { + return true; + } + + WITH_SEMAPHORE(_semaphore); + + // init the target pin now if possible + if ( ardupin != ANALOG_INPUT_NONE ) { + + gpio_num_t gpio; // new gpio + //Configure ADC + if (_unit == 1) { + adc1_config_channel_atten((adc1_channel_t)newgpioAdcPin, atten); + adc1_pad_get_io_num((adc1_channel_t)newgpioAdcPin, &gpio); + } else { + adc2_config_channel_atten((adc2_channel_t)newgpioAdcPin, atten); + } + + esp_adc_cal_characteristics_t adc_chars; + esp_adc_cal_characterize(ADC_UNIT_1, atten, ADC_WIDTH_BIT_12, DEFAULT_VREF, &adc_chars); + printf("AnalogIn: Adding gpio on: %d\n", gpio); + + hal.console->printf("AnalogIn: set_pin() FROM (ardupin:%d adc1_offset:%d gpio:%d) TO (ardupin:%d adc1_offset:%d gpio:%d)\n", \ + _ardupin,_pin,_gpio,ardupin,newgpioAdcPin,gpio); + _pin = newgpioAdcPin; + _ardupin = ardupin; + _gpio = gpio; + _scaler = newscaler; + + } + + _sum_value = 0; + _sum_count = 0; + _latest_value = 0; + _value = 0; + + return true; + +} + +/* + apply a reading in ADC counts + */ +void AnalogSource::_add_value() +{ + if ( _ardupin == ANALOG_INPUT_NONE ) { + return; + } + + WITH_SEMAPHORE(_semaphore); + + int value = 0; + if (_unit == 1) { + value = adc1_get_raw((adc1_channel_t)_pin); + } else { + adc2_get_raw((adc2_channel_t)_pin, ADC_WIDTH_BIT_12, &value); + } + + _latest_value = value; + _sum_value += value; + _sum_count++; + + if (_sum_count == 254) { + _sum_value /= 2; + _sum_count /= 2; + } +} + +static void check_efuse() +{ + //Check TP is burned into eFuse + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK) { + printf("AnalogIn: eFuse Two Point: Supported\n"); + } else { + printf("AnalogIn: eFuse Two Point: NOT supported\n"); + } + + //Check Vref is burned into eFuse + if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK) { + printf("AnalogIn: eFuse Vref: Supported\n"); + } else { + printf("AnalogIn: eFuse Vref: NOT supported\n"); + } +} + +/* + setup adc peripheral to capture samples with DMA into a buffer + */ +void AnalogIn::init() +{ + check_efuse(); + + adc1_config_width(ADC_WIDTH_BIT_12); +} + +/* + called at 1kHz +*/ +void AnalogIn::_timer_tick() +{ + for (uint8_t j = 0; j < ANALOG_MAX_CHANNELS; j++) { + ESP32::AnalogSource *c = _channels[j]; + if (c != nullptr) { + // add a value + //c->_add_value(); + } + } + +#if ESP32_ADC_MAVLINK_DEBUG + static uint8_t count; + if (AP_HAL::millis() > 5000 && count++ == 10) { + count = 0; + uint16_t adc[6] {}; + uint8_t n = ADC_GRP1_NUM_CHANNELS; + if (n > 6) { + n = 6; + } + for (uint8_t i = 0; i < n; i++) { + adc[i] = buf_adc[i]; + } + mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4], + adc[5]); + } +#endif + +} + +//positive array index (zero is ok), or -1 on error +int8_t AnalogIn::find_pinconfig(int16_t ardupin) +{ + // from ardupin, lookup which adc gpio that is.. + for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) { + if (pin_config[j].ardupin == ardupin) { + return j; + } + } + // can't find a match in definitons + return -1; + +} + +// +AP_HAL::AnalogSource *AnalogIn::channel(int16_t ardupin) +{ + int8_t pinconfig_offset = find_pinconfig(ardupin); + + int16_t gpioAdcPin = -1; + float scaler = -1; + + if ((ardupin != ANALOG_INPUT_NONE) && (pinconfig_offset == -1 )) { + hal.console->printf("AnalogIn: sorry channel() can't determine ADC1 offset from ardupin : %d \n",ardupin); + ardupin = ANALOG_INPUT_NONE; // default it to this not terrible value and allow to continue + } + // although ANALOG_INPUT_NONE=255 is not a valid pin, we let it through here as + // a special case, so that it can be changed with set_pin(..) later. + if (ardupin != ANALOG_INPUT_NONE) { + gpioAdcPin = pin_config[(uint8_t)pinconfig_offset].channel; + scaler = pin_config[(uint8_t)pinconfig_offset].scaling; + } + + for (uint8_t j = 0; j < ANALOG_MAX_CHANNELS; j++) { + if (_channels[j] == nullptr) { + _channels[j] = new AnalogSource(ardupin,gpioAdcPin, scaler,0.0f,1); + + if (ardupin != ANALOG_INPUT_NONE) { + hal.console->printf("AnalogIn: channel:%d attached to ardupin:%d at adc1_offset:%d on gpio:%d\n",\ + j,ardupin, gpioAdcPin, _channels[j]->_gpio); + } + + if (ardupin == ANALOG_INPUT_NONE) { + hal.console->printf("AnalogIn: channel:%d created but using delayed adc and gpio pin configuration\n",j ); + } + + return _channels[j]; + } + } + hal.console->printf("AnalogIn: out of channels\n"); + return nullptr; +} + +#endif // HAL_USE_ADC diff --git a/libraries/AP_HAL_ESP32/AnalogIn.h b/libraries/AP_HAL_ESP32/AnalogIn.h new file mode 100644 index 0000000000..2814e57790 --- /dev/null +++ b/libraries/AP_HAL_ESP32/AnalogIn.h @@ -0,0 +1,104 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Charles Villard + */ + +#pragma once + +#include "AP_HAL_ESP32.h" + +#if HAL_USE_ADC == TRUE && !defined(HAL_DISABLE_ADC_DRIVER) + +#define ANALOG_MAX_CHANNELS 8 + +namespace ESP32 +{ + +class AnalogSource : public AP_HAL::AnalogSource +{ +public: + friend class AnalogIn; + AnalogSource(int16_t ardupin,int16_t pin,float scaler, float initial_value, uint8_t unit); + float read_average() override; + float read_latest() override; + bool set_pin(uint8_t p) override; + float voltage_average() override; + float voltage_latest() override; + float voltage_average_ratiometric() override; + void set_stop_pin(uint8_t p) {} + void set_settle_time(uint16_t settle_time_ms) {} + +private: + //ADC number (1 or 2). ADC2 is unavailable when WIFI on + uint8_t _unit; + + //adc Pin number (1-8) + // gpio-adc lower level pin name + int16_t _pin; + + //human readable Pin number used in ardu params + int16_t _ardupin; + //scaling from ADC count to Volts + int16_t _scaler; + // gpio pin number on esp32: + gpio_num_t _gpio; + + //Current computed value (average) + float _value; + //Latest fetched raw value from the sensor + float _latest_value; + //Number of fetched value since average + uint8_t _sum_count; + //Sum of fetched values + float _sum_value; + + void _add_value(); + + HAL_Semaphore _semaphore; +}; + +class AnalogIn : public AP_HAL::AnalogIn +{ +public: + friend class AnalogSource; + + void init() override; + AP_HAL::AnalogSource* channel(int16_t pin) override; + void _timer_tick(); + float board_voltage() override + { + return _board_voltage; + } + static int8_t find_pinconfig(int16_t ardupin); + +private: + ESP32::AnalogSource* _channels[ANALOG_MAX_CHANNELS]; // list of pointers to active individual AnalogSource objects or nullptr + + uint32_t _last_run; + float _board_voltage; + + struct pin_info { + uint8_t channel; // adc1 pin offset + float scaling; + uint8_t ardupin; // eg 3 , as typed into an ardupilot parameter + uint8_t gpio; // eg 32 for D32 esp pin number/s + }; + + static const pin_info pin_config[]; +}; + +} + +#endif // HAL_USE_ADC diff --git a/libraries/AP_HAL_ESP32/DeviceBus.cpp b/libraries/AP_HAL_ESP32/DeviceBus.cpp new file mode 100644 index 0000000000..5e99e4ac95 --- /dev/null +++ b/libraries/AP_HAL_ESP32/DeviceBus.cpp @@ -0,0 +1,155 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "DeviceBus.h" + +#include +#include +#include + +#include "Scheduler.h" +#include "Semaphores.h" + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + + +using namespace ESP32; + +extern const AP_HAL::HAL& hal; + +DeviceBus::DeviceBus(uint8_t _thread_priority) : + thread_priority(_thread_priority), semaphore() +{ +#ifdef BUSDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif +} + +/* + per-bus callback thread +*/ +void IRAM_ATTR DeviceBus::bus_thread(void *arg) +{ +#ifdef BUSDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + struct DeviceBus *binfo = (struct DeviceBus *)arg; + + while (true) { + uint64_t now = AP_HAL::micros64(); + DeviceBus::callback_info *callback; + + // find a callback to run + for (callback = binfo->callbacks; callback; callback = callback->next) { + if (now >= callback->next_usec) { + while (now >= callback->next_usec) { + callback->next_usec += callback->period_usec; + } + // call it with semaphore held + if (binfo->semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + callback->cb(); + binfo->semaphore.give(); + } + } + } + + // work out when next loop is needed + uint64_t next_needed = 0; + now = AP_HAL::micros64(); + + for (callback = binfo->callbacks; callback; callback = callback->next) { + if (next_needed == 0 || + callback->next_usec < next_needed) { + next_needed = callback->next_usec; + if (next_needed < now) { + next_needed = now; + } + } + } + + // delay for at most 50ms, to handle newly added callbacks + uint32_t delay = 50000; + if (next_needed >= now && next_needed - now < delay) { + delay = next_needed - now; + } + // don't delay for less than 100usec, so one thread doesn't + // completely dominate the CPU + if (delay < 100) { + delay = 100; + } + hal.scheduler->delay_microseconds(delay); + } + return; +} + +AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device) +{ +#ifdef BUSDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (!thread_started) { + thread_started = true; + hal_device = _hal_device; + // setup a name for the thread + char name[configMAX_TASK_NAME_LEN]; + switch (hal_device->bus_type()) { + case AP_HAL::Device::BUS_TYPE_I2C: + snprintf(name, sizeof(name), "APM_I2C:%u", + hal_device->bus_num()); + break; + + case AP_HAL::Device::BUS_TYPE_SPI: + snprintf(name, sizeof(name), "APM_SPI:%u", + hal_device->bus_num()); + break; + default: + break; + } +#ifdef BUSDEBUG + printf("%s:%d Thread Start\n", __PRETTY_FUNCTION__, __LINE__); +#endif + xTaskCreate(DeviceBus::bus_thread, name, Scheduler::DEVICE_SS, + this, thread_priority, &bus_thread_handle); + } + DeviceBus::callback_info *callback = new DeviceBus::callback_info; + if (callback == nullptr) { + return nullptr; + } + callback->cb = cb; + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + // add to linked list of callbacks on thread + callback->next = callbacks; + callbacks = callback; + + return callback; +} + +/* + * Adjust the timer for the next call: it needs to be called from the bus + * thread, otherwise it will race with it + */ +bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + if (xTaskGetCurrentTaskHandle() != bus_thread_handle) { + return false; + } + DeviceBus::callback_info *callback = static_cast(h); + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + return true; +} diff --git a/libraries/AP_HAL_ESP32/DeviceBus.h b/libraries/AP_HAL_ESP32/DeviceBus.h new file mode 100644 index 0000000000..f18eb16b60 --- /dev/null +++ b/libraries/AP_HAL_ESP32/DeviceBus.h @@ -0,0 +1,55 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include "Semaphores.h" +#include "AP_HAL_ESP32.h" +#include "Scheduler.h" + + +namespace ESP32 +{ + +class DeviceBus +{ +public: + DeviceBus(uint8_t _thread_priority); + + struct DeviceBus *next; + Semaphore semaphore; + + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device); + bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec); + static void bus_thread(void *arg); + +private: + struct callback_info { + struct callback_info *next; + AP_HAL::Device::PeriodicCb cb; + uint32_t period_usec; + uint64_t next_usec; + } *callbacks; + uint8_t thread_priority; + void* bus_thread_handle; + bool thread_started; + AP_HAL::Device *hal_device; +}; + +} + + diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp new file mode 100644 index 0000000000..b4d246ee3b --- /dev/null +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp @@ -0,0 +1,114 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include +#include + +#include "HAL_ESP32_Class.h" +#include "Scheduler.h" +#include "I2CDevice.h" +#include "SPIDevice.h" +#include "I2CDevice.h" +#include "UARTDriver.h" +#include "WiFiDriver.h" +#include "WiFiUdpDriver.h" +#include "RCInput.h" +#include "RCOutput.h" +#include "Storage.h" +#include "RCOutput.h" +#include "Storage.h" +#include "RCOutput.h" +#include "AnalogIn.h" +#include "Util.h" + + +static Empty::UARTDriver uartADriver; +static ESP32::UARTDriver cons(0); +static ESP32::UARTDriver uartBDriver(1); +#ifdef HAL_ESP32_WIFI +#if HAL_ESP32_WIFI == 1 +static ESP32::WiFiDriver uartCDriver; //tcp, client should connect to 192.168.4.1 port 5760 +#elif HAL_ESP32_WIFI == 2 +static ESP32::WiFiUdpDriver uartCDriver; //udp +#endif +#else +static Empty::UARTDriver uartCDriver; +#endif +static ESP32::UARTDriver uartDDriver(2); +static Empty::UARTDriver uartEDriver; +static Empty::UARTDriver uartFDriver; +static Empty::UARTDriver uartGDriver; +static Empty::UARTDriver uartHDriver; +static Empty::UARTDriver uartIDriver; + +static Empty::DSP dspDriver; + +static ESP32::I2CDeviceManager i2cDeviceManager; +static ESP32::SPIDeviceManager spiDeviceManager; +#ifndef HAL_DISABLE_ADC_DRIVER +static ESP32::AnalogIn analogIn; +#else +static Empty::AnalogIn analogIn; +#endif +static ESP32::Storage storageDriver; +static Empty::GPIO gpioDriver; +static ESP32::RCOutput rcoutDriver; +static ESP32::RCInput rcinDriver; +static ESP32::Scheduler schedulerInstance; +static ESP32::Util utilInstance; +static Empty::OpticalFlow opticalFlowDriver; +static Empty::Flash flashDriver; + +extern const AP_HAL::HAL& hal; + +HAL_ESP32::HAL_ESP32() : + AP_HAL::HAL( + &cons, //Console/mavlink + &uartBDriver, //GPS 1 + &uartCDriver, //Telem 1 + &uartDDriver, //Telem 2 + &uartEDriver, //GPS 2 + &uartFDriver, //Extra 1 + &uartGDriver, //Extra 2 + &uartHDriver, //Extra 3 + &uartIDriver, //Extra 4 + &i2cDeviceManager, + &spiDeviceManager, + nullptr, + &analogIn, + &storageDriver, + &cons, + &gpioDriver, + &rcinDriver, + &rcoutDriver, + &schedulerInstance, + &utilInstance, + &opticalFlowDriver, + &flashDriver, + &dspDriver, + nullptr + ) +{} + +void HAL_ESP32::run(int argc, char * const argv[], Callbacks* callbacks) const +{ + ((ESP32::Scheduler *)hal.scheduler)->set_callbacks(callbacks); + hal.scheduler->init(); +} + +void AP_HAL::init() +{ +} + diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.h b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.h new file mode 100644 index 0000000000..59b866716c --- /dev/null +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.h @@ -0,0 +1,27 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include + +class HAL_ESP32 : public AP_HAL::HAL +{ +public: + HAL_ESP32(); + void run(int argc, char* const* argv, Callbacks* callbacks) const override; +}; diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h new file mode 100644 index 0000000000..d59f080c0c --- /dev/null +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h @@ -0,0 +1,22 @@ +#pragma once + +namespace ESP32 +{ +class UARTDriver; +class WiFiDriver; +class WiFiUdpDriver; +class Scheduler; +class EEPROMStorage; +class AnalogIn; +class RCInput; +class RCOutput; +class ADCSource; +class RCInput; +class Util; +class Semaphore; +class Semaphore_Recursive; +class GPIO; +class DigitalSource; +class Storage; +class RmtSigReader; +} // namespace ESP32 diff --git a/libraries/AP_HAL_ESP32/I2CDevice.cpp b/libraries/AP_HAL_ESP32/I2CDevice.cpp new file mode 100644 index 0000000000..e1b20497a6 --- /dev/null +++ b/libraries/AP_HAL_ESP32/I2CDevice.cpp @@ -0,0 +1,193 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "I2CDevice.h" + +#include +#include +#include +#include "Scheduler.h" + +using namespace ESP32; + +#define MHZ (1000U*1000U) +#define KHZ (1000U) + +I2CBusDesc i2c_bus_desc[] = { HAL_ESP32_I2C_BUSES }; + +I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(i2c_bus_desc)]; + +I2CDeviceManager::I2CDeviceManager(void) +{ + for (uint8_t i=0; i +I2CDeviceManager::get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock, + bool use_smbus, + uint32_t timeout_ms) +{ + if (bus >= ARRAY_SIZE(i2c_bus_desc)) { + return AP_HAL::OwnPtr(nullptr); + } + auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms)); + return dev; +} + +/* + get mask of bus numbers for all configured I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask(void) const +{ + return ((1U << ARRAY_SIZE(i2c_bus_desc)) - 1); +} + +/* + get mask of bus numbers for all configured internal I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_internal(void) const +{ + uint32_t result = 0; + for (size_t i = 0; i < ARRAY_SIZE(i2c_bus_desc); i++) { + if (i2c_bus_desc[i].internal) { + result |= (1u << i); + } + } + return result; +} + +/* + get mask of bus numbers for all configured external I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_external(void) const +{ + return get_bus_mask() & ~get_bus_mask_internal(); +} diff --git a/libraries/AP_HAL_ESP32/I2CDevice.h b/libraries/AP_HAL_ESP32/I2CDevice.h new file mode 100644 index 0000000000..099de86d33 --- /dev/null +++ b/libraries/AP_HAL_ESP32/I2CDevice.h @@ -0,0 +1,147 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include + +#include +#include +#include + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +#include "driver/i2c.h" +#include "i2c_sw.h" + +namespace ESP32 +{ + +struct I2CBusDesc { + i2c_port_t port; + gpio_num_t sda; + gpio_num_t scl; + uint32_t speed; + bool internal; + bool soft; +}; + +class I2CBus : public DeviceBus +{ +public: + I2CBus():DeviceBus(Scheduler::I2C_PRIORITY) {}; + i2c_port_t port; + uint32_t bus_clock; + _i2c_bus_t sw_handle; + bool soft; +}; + +class I2CDevice : public AP_HAL::I2CDevice +{ +public: + static I2CDevice *from(AP_HAL::I2CDevice *dev) + { + return static_cast(dev); + } + + I2CDevice(uint8_t bus, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms); + ~I2CDevice(); + + /* See AP_HAL::I2CDevice::set_address() */ + void set_address(uint8_t address) override + { + _address = address; + } + + /* See AP_HAL::I2CDevice::set_retries() */ + void set_retries(uint8_t retries) override + { + _retries = retries; + } + + /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ + bool set_speed(enum Device::Speed speed) override + { + return true; + } + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, + uint32_t recv_len, uint8_t times) override + { + return false; + }; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + /* See AP_HAL::Device::adjust_periodic_callback() */ + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override //TODO check all + { + // if asking for invalid bus number use bus 0 semaphore + return &bus.semaphore; + } + +protected: + I2CBus &bus; + uint8_t _retries; + uint8_t _address; + char *pname; + +}; + +class I2CDeviceManager : public AP_HAL::I2CDeviceManager +{ +public: + friend class I2CDevice; + + static I2CBus businfo[]; + + // constructor + I2CDeviceManager(); + + static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr) + { + return static_cast(i2c_mgr); + } + + AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock=400000, + bool use_smbus = false, + uint32_t timeout_ms=4) override; + + /* + get mask of bus numbers for all configured I2C buses + */ + uint32_t get_bus_mask(void) const override; + + /* + get mask of bus numbers for all configured external I2C buses + */ + uint32_t get_bus_mask_external(void) const override; + + /* + get mask of bus numbers for all configured internal I2C buses + */ + uint32_t get_bus_mask_internal(void) const override; +}; +} diff --git a/libraries/AP_HAL_ESP32/OSD.cpp b/libraries/AP_HAL_ESP32/OSD.cpp new file mode 100644 index 0000000000..ee7da71441 --- /dev/null +++ b/libraries/AP_HAL_ESP32/OSD.cpp @@ -0,0 +1,309 @@ +#include + +#ifdef WITH_INT_OSD + +#include "freertos/FreeRTOS.h" + +#include "soc/mcpwm_struct.h" +#include "soc/mcpwm_reg.h" +#include "soc/i2s_struct.h" +#include "soc/i2s_reg.h" + +#include "driver/mcpwm.h" +#include "driver/i2s.h" + +#include +#include + +#include + +#define LINE_S 22 +#define LINE_F 310 + +uint32_t *osd_buffer_mask; +uint32_t *osd_buffer_levl; +uint32_t line; + +//We use assembly cause high priority interrupt can not be written in C. +//C reference version is below. +__asm__(R"( + .data +_l5_intr_stack: + .space 36 + .section .iram1,"ax",@progbits + .literal_position + .literal .LC100, 1073078548 + .literal .LC102, 1073078524 + .literal .LC103, line + .literal .LC104, osd_buffer_mask + .literal .LC105, osd_buffer_levl + .literal .LC106, 1073016840 + .literal .LC107, 1073139720 + .literal .LC108, 1073016832 + .literal .LC109, 1073139712 + .literal .LC110, 1073078556 + .align 4 + .global xt_highint5 + .type xt_highint5, @function +xt_highint5: + movi a0, _l5_intr_stack + s32i a8, a0, 0 + s32i a9, a0, 4 + s32i a10, a0, 8 + s32i a11, a0, 12 + s32i a12, a0, 16 + s32i a13, a0, 20 + s32i a14, a0, 24 + s32i a15, a0, 28 + s32i a2, a0, 32 + + l32r a8, .LC100 + l32i.n a8, a8, 0 + bbci a8, 27, .L103 + l32r a8, .LC102 + movi a10, 0x62 + l32i.n a9, a8, 0 + movi a8, -0x12d + add.n a8, a9, a8 + bltu a10, a8, .L104 + l32r a10, .LC103 + l32i.n a8, a10, 0 + addi.n a9, a8, 1 + s32i.n a9, a10, 0 + addi a8, a8, -21 + movi a10, 0x11f + bltu a10, a8, .L103 + l32r a8, .LC104 + l32r a14, .LC106 + l32i.n a10, a8, 0 + l32r a8, .LC105 + movi.n a12, 0 + l32r a13, .LC107 + l32i.n a8, a8, 0 + s32i.n a12, a14, 0 + s32i.n a12, a13, 0 + movi.n a11, 1 + s32i.n a11, a14, 0 + s32i.n a11, a13, 0 + addx2 a9, a9, a9 + movi a11, -0x108 + addx4 a9, a9, a11 + s32i.n a12, a14, 0 + slli a9, a9, 2 + s32i.n a12, a13, 0 + add.n a10, a10, a9 + l32r a11, .LC108 + l32i.n a15, a10, 0 + add.n a8, a8, a9 + s32i.n a15, a11, 0 + l32r a9, .LC109 + l32i.n a2, a8, 0 + movi.n a15, 0x10 + s32i.n a2, a9, 0 + s32i.n a15, a14, 0 + s32i.n a15, a13, 0 + l32i.n a13, a10, 4 + s32i.n a13, a11, 0 + l32i.n a13, a8, 4 + s32i.n a13, a9, 0 + l32i.n a13, a10, 8 + s32i.n a13, a11, 0 + l32i.n a13, a8, 8 + s32i.n a13, a9, 0 + l32i.n a13, a10, 12 + s32i.n a13, a11, 0 + l32i.n a13, a8, 12 + s32i.n a13, a9, 0 + l32i.n a13, a10, 16 + s32i.n a13, a11, 0 + l32i.n a13, a8, 16 + s32i.n a13, a9, 0 + l32i.n a13, a10, 20 + s32i.n a13, a11, 0 + l32i.n a13, a8, 20 + s32i.n a13, a9, 0 + l32i.n a13, a10, 24 + s32i.n a13, a11, 0 + l32i.n a13, a8, 24 + s32i.n a13, a9, 0 + l32i.n a13, a10, 28 + s32i.n a13, a11, 0 + l32i.n a13, a8, 28 + s32i.n a13, a9, 0 + l32i.n a13, a10, 32 + s32i.n a13, a11, 0 + l32i.n a13, a8, 32 + s32i.n a13, a9, 0 + l32i.n a13, a10, 36 + s32i.n a13, a11, 0 + l32i.n a13, a8, 36 + s32i.n a13, a9, 0 + l32i.n a13, a10, 40 + s32i.n a13, a11, 0 + l32i.n a13, a8, 40 + s32i.n a13, a9, 0 + l32i.n a10, a10, 44 + s32i.n a10, a11, 0 + l32i.n a8, a8, 44 + s32i.n a8, a9, 0 + s32i.n a12, a11, 0 + s32i.n a12, a9, 0 + j .L103 +.L104: + movi a8, -0x7d1 + add.n a9, a9, a8 + movi a8, 0x1f2 + bltu a8, a9, .L103 + l32r a8, .LC103 + movi.n a9, 0 + s32i.n a9, a8, 0 +.L103: + l32r a8, .LC110 + movi.n a9, -1 + s32i.n a9, a8, 0 + + movi a0, _l5_intr_stack + l32i a8, a0, 0 + l32i a9, a0, 4 + l32i a10, a0, 8 + l32i a11, a0, 12 + l32i a12, a0, 16 + l32i a13, a0, 20 + l32i a14, a0, 24 + l32i a15, a0, 28 + l32i a2, a0, 32 + rsync + memw + rsr a0, 213 + rfi 5 +)"); + +inline bool normal_sync(uint32_t sync) { + return (sync > 300) && (sync < 400); +} + +inline bool long_sync(uint32_t sync) { + return (sync > 2000) && (sync < 2500); +} + +//This function is not linked in the final binary, +//it exist only to generate xt_highint5 above. +//In xt_hightint we have fixed prolog and epilog and we removed +//some 'memw' instructions, otherwise they are the same. +void IRAM_ATTR __attribute__((optimize("O3"))) osd_mcpwm_isr(void *) { + if ((READ_PERI_REG(MCMCPWM_INT_RAW_MCPWM_REG(0)) & MCPWM_CAP0_INT_RAW_M) + != 0) { + uint32_t sync = READ_PERI_REG(MCPWM_CAP_CH0_REG(0)); + if (normal_sync(sync)) { + line++; + if (line >= LINE_S && line < LINE_F) { + int disp = (line - LINE_S) * (AP_OSD_INT::video_x / 32); + uint32_t *tmp_mask = osd_buffer_mask + disp; + uint32_t *tmp_levl = osd_buffer_levl + disp; + + WRITE_PERI_REG(I2S_CONF_REG(0), 0); + WRITE_PERI_REG(I2S_CONF_REG(1), 0); + + WRITE_PERI_REG(I2S_CONF_REG(0), I2S_TX_RESET_M); + WRITE_PERI_REG(I2S_CONF_REG(1), I2S_TX_RESET_M); + + WRITE_PERI_REG(I2S_CONF_REG(0), 0); + WRITE_PERI_REG(I2S_CONF_REG(1), 0); + + WRITE_PERI_REG(REG_I2S_BASE(0), tmp_mask[0]); + WRITE_PERI_REG(REG_I2S_BASE(1), tmp_levl[0]); + + WRITE_PERI_REG(I2S_CONF_REG(0), I2S_TX_START_M); + WRITE_PERI_REG(I2S_CONF_REG(1), I2S_TX_START_M); + + for (int ix = 1; ix < AP_OSD_INT::video_x / 32; ix++) { + WRITE_PERI_REG(REG_I2S_BASE(0), tmp_mask[ix]); + WRITE_PERI_REG(REG_I2S_BASE(1), tmp_levl[ix]); + } + + WRITE_PERI_REG(REG_I2S_BASE(0), 0); + WRITE_PERI_REG(REG_I2S_BASE(1), 0); + + } + } else if (long_sync(sync)) { + line = 0; + } + } + WRITE_PERI_REG(MCMCPWM_INT_CLR_MCPWM_REG(0), 0xFFFFFFFF); +} + +void config_mcpwm() +{ + periph_module_enable(PERIPH_PWM0_MODULE); + MCPWM0.cap_timer_cfg.timer_en = 1; + MCPWM0.cap_timer_cfg.synci_en = 1; + MCPWM0.cap_timer_cfg.synci_sel = 4; //SYNC0 + MCPWM0.cap_timer_phase = 0; + MCPWM0.int_ena.cap0_int_ena = 1; + MCPWM0.cap_cfg_ch[0].en = 1; + MCPWM0.cap_cfg_ch[0].mode = (1 << MCPWM_NEG_EDGE); + MCPWM0.cap_cfg_ch[0].prescale = 0; +} + +void config_i2s(i2s_dev_t *I2S) { + if (I2S == &I2S1) { + periph_module_enable(PERIPH_I2S1_MODULE); + } else { + periph_module_enable(PERIPH_I2S0_MODULE); + } + + I2S->conf2.val = 0; + + I2S->pdm_conf.val = 0; + + I2S->conf_chan.tx_chan_mod = 0; + + I2S->fifo_conf.tx_fifo_mod = 0; + I2S->fifo_conf.dscr_en = 0; //no dma + I2S->fifo_conf.tx_fifo_mod_force_en = 1; + + I2S->conf.val = 0; + + I2S->clkm_conf.clka_en = 0; + I2S->clkm_conf.clkm_div_a = 63; + I2S->clkm_conf.clkm_div_b = 0; + I2S->clkm_conf.clkm_div_num = 2; + + I2S->sample_rate_conf.tx_bck_div_num = 12; + I2S->sample_rate_conf.tx_bits_mod = 16; +} + +void config_gpio() +{ + gpio_set_direction(OSD_SYNC_PIN, GPIO_MODE_INPUT); + gpio_set_direction(OSD_MASK_PIN, GPIO_MODE_OUTPUT); + gpio_set_direction(OSD_LEVL_PIN, GPIO_MODE_OUTPUT); + + gpio_matrix_in(OSD_SYNC_PIN, PWM0_SYNC0_IN_IDX, true); + gpio_matrix_in(OSD_SYNC_PIN, PWM0_CAP0_IN_IDX, true); + + gpio_matrix_out(OSD_MASK_PIN, I2S0O_DATA_OUT23_IDX, true, false); + gpio_matrix_out(OSD_LEVL_PIN, I2S1O_DATA_OUT23_IDX, false, false); +} + +void config_isr() +{ + esp_err_t err = esp_intr_alloc(ETS_PWM0_INTR_SOURCE, ESP_INTR_FLAG_IRAM|ESP_INTR_FLAG_LEVEL5, + nullptr, nullptr, nullptr); + printf("alloc intr error code %d\n", err); +} + +void osd_setup(AP_OSD_INT *d) +{ + printf("osd setup start %d\n", xPortGetCoreID()); + osd_buffer_mask = &(d->frame_mask[0][0]); + osd_buffer_levl = &(d->frame_levl[0][0]); + config_mcpwm(); + config_i2s(&I2S0); + config_i2s(&I2S1); + config_gpio(); + config_isr(); + printf("osd setup finish\n"); +} + +#endif diff --git a/libraries/AP_HAL_ESP32/Profile.cpp b/libraries/AP_HAL_ESP32/Profile.cpp new file mode 100644 index 0000000000..c87e18e250 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Profile.cpp @@ -0,0 +1,87 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#ifdef ENABLE_PROFILE +#include +#include +#include + +#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) + +struct FI { + uint32_t address; + uint32_t count; + struct FI * next; +}; + +FI* ht[1024]; + +extern "C" __attribute__((section(".iram1"))) __attribute__((no_instrument_function)) +void _mcount() +{ + void *a = __builtin_return_address(0); + a = __builtin_extract_return_addr(a); + uint32_t address = (uint32_t) a; + uint32_t hash = address; + hash = ((hash >> 16)^hash)*0x45d9f3b; + hash = ((hash >> 16)^hash)*0x45d9f3b; + uint32_t index = ((hash >> 16)^hash)%ARRAY_SIZE(ht); + FI **current = &(ht[index]); + while (*current != nullptr && (*current)->address != address) { + current = &((*current)->next); + } + if (*current != nullptr) { + (*current)->count++; + } else { + FI* next = (FI *)calloc(1, sizeof(FI)); + next->address = address; + next->count = 1; + next->next = nullptr; + *current = next; + } +} + +void print_profile() +{ + static int64_t last_run = 0; + static int counter = 0; + if (AP_HAL::millis64() - last_run > 60000) { + //char fname[50]; + //snprintf(fname, sizeof(fname), "/SDCARD/APM/PROF%03d.TXT", counter); + ++counter; + //FILE *f = fopen(fname, "w"); + // if (f != nullptr) { + for (size_t i=0; i < ARRAY_SIZE(ht); i++) { + for (FI *current = ht[i]; current != nullptr; current = current->next) { + if (current->count != 0) { + printf("0x%016x 0x%x\n", current->address, current->count); + current->count = 0; + } + } + } + // fclose(f); + printf("------- profile dumped %d\n", counter); + /*} else { + printf("profile open error %d %d\n", counter, errno); + }*/ + last_run = AP_HAL::millis64(); + } +} +#else +void print_profile() +{ +} +#endif + diff --git a/libraries/AP_HAL_ESP32/Profile.h b/libraries/AP_HAL_ESP32/Profile.h new file mode 100644 index 0000000000..b0c66c99b1 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Profile.h @@ -0,0 +1,18 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +void print_profile(); diff --git a/libraries/AP_HAL_ESP32/RCInput.cpp b/libraries/AP_HAL_ESP32/RCInput.cpp new file mode 100644 index 0000000000..721e137bb0 --- /dev/null +++ b/libraries/AP_HAL_ESP32/RCInput.cpp @@ -0,0 +1,129 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include +#include + +#include "AP_HAL_ESP32.h" +#include "RCInput.h" + +using namespace ESP32; + +void RCInput::init() +{ + if (_init) { + return; + } +#ifndef HAL_BUILD_AP_PERIPH + AP::RC().init(); +#endif + +#ifdef HAL_ESP32_RCIN + sig_reader.init(); +#endif + _init = true; +} + +bool RCInput::new_input() +{ + if (!_init) { + return false; + } + bool valid; + { + WITH_SEMAPHORE(rcin_mutex); + valid = _rcin_timestamp_last_signal != _last_read; + _last_read = _rcin_timestamp_last_signal; + } + + return valid; +} + +uint8_t RCInput::num_channels() +{ + if (!_init) { + return 0; + } + return _num_channels; +} + +uint16_t RCInput::read(uint8_t channel) +{ + if (!_init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, _num_channels))) { + return 0; + } + uint16_t v; + { + WITH_SEMAPHORE(rcin_mutex); + v = _rc_values[channel]; + } + return v; +} + +uint8_t RCInput::read(uint16_t* periods, uint8_t len) +{ + if (!_init) { + return false; + } + + if (len > RC_INPUT_MAX_CHANNELS) { + len = RC_INPUT_MAX_CHANNELS; + } + { + WITH_SEMAPHORE(rcin_mutex); + memcpy(periods, _rc_values, len*sizeof(periods[0])); + } + return len; +} + +void RCInput::_timer_tick(void) +{ + if (!_init) { + return; + } + + AP_RCProtocol &rcprot = AP::RC(); + +#ifdef HAL_ESP32_RCIN + uint32_t width_s0, width_s1; + while (sig_reader.read(width_s0, width_s1)) { + rcprot.process_pulse(width_s0, width_s1); + + } + +#ifndef HAL_NO_UARTDRIVER + const char *rc_protocol = nullptr; +#endif + + if (rcprot.new_input()) { + WITH_SEMAPHORE(rcin_mutex); + _rcin_timestamp_last_signal = AP_HAL::micros(); + _num_channels = rcprot.num_channels(); + _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); + rcprot.read(_rc_values, _num_channels); + _rssi = rcprot.get_RSSI(); +#ifndef HAL_NO_UARTDRIVER + rc_protocol = rcprot.protocol_name(); +#endif + } + +#ifndef HAL_NO_UARTDRIVER + if (rc_protocol && rc_protocol != last_protocol) { + last_protocol = rc_protocol; + gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol); + } +#endif + +#endif +} diff --git a/libraries/AP_HAL_ESP32/RCInput.h b/libraries/AP_HAL_ESP32/RCInput.h new file mode 100644 index 0000000000..370753d07f --- /dev/null +++ b/libraries/AP_HAL_ESP32/RCInput.h @@ -0,0 +1,53 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +#include "AP_HAL_ESP32.h" +#include "Semaphores.h" +#include "RmtSigReader.h" +#include + + +#ifndef RC_INPUT_MAX_CHANNELS +#define RC_INPUT_MAX_CHANNELS 18 +#endif + +class ESP32::RCInput : public AP_HAL::RCInput +{ +public: + void init() override; + bool new_input() override; + uint8_t num_channels() override; + uint16_t read(uint8_t ch) override; + uint8_t read(uint16_t* periods, uint8_t len) override; + void _timer_tick(void); + + const char *protocol() const override + { + return last_protocol; + } +private: + uint16_t _rc_values[RC_INPUT_MAX_CHANNELS] = {0}; + uint64_t _last_read; + uint8_t _num_channels; + Semaphore rcin_mutex; + int16_t _rssi = -1; + uint32_t _rcin_timestamp_last_signal; + bool _init; + const char *last_protocol; + + ESP32::RmtSigReader sig_reader; +}; diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp new file mode 100644 index 0000000000..20d8630194 --- /dev/null +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -0,0 +1,349 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt + * + */ + +#include "RCOutput.h" + +#include +#include +#include +#include + +#include "driver/rtc_io.h" + +#include + +extern const AP_HAL::HAL& hal; + +using namespace ESP32; + +#ifdef HAL_ESP32_RCOUT + +gpio_num_t outputs_pins[] = HAL_ESP32_RCOUT; + +//If the RTC source is not required, then GPIO32/Pin12/32K_XP and GPIO33/Pin13/32K_XN can be used as digital GPIOs. + +#else +gpio_num_t outputs_pins[] = {}; + +#endif + +#define MAX_CHANNELS ARRAY_SIZE(outputs_pins) + +struct RCOutput::pwm_out RCOutput::pwm_group_list[MAX_CHANNELS]; + +void RCOutput::init() +{ + _max_channels = MAX_CHANNELS; + + + //32 and 33 are special as they dont default to gpio, but can be if u disable their rtc setup: + rtc_gpio_deinit(GPIO_NUM_32); + rtc_gpio_deinit(GPIO_NUM_33); + + printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n"); + printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS); + printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n"); + + static const mcpwm_io_signals_t signals[] = { + MCPWM0A, + MCPWM0B, + MCPWM1A, + MCPWM1B, + MCPWM2A, + MCPWM2B + }; + static const mcpwm_timer_t timers[] = { + MCPWM_TIMER_0, + MCPWM_TIMER_1, + MCPWM_TIMER_2 + + }; + static const mcpwm_unit_t units[] = { + MCPWM_UNIT_0, + MCPWM_UNIT_1 + }; + static const mcpwm_operator_t operators[] = { + MCPWM_OPR_A, + MCPWM_OPR_B + }; + + for (uint8_t i = 0; i < MAX_CHANNELS; ++i) { + auto unit = units[i/6]; + auto signal = signals[i % 6]; + auto timer = timers[i/2]; + + //Save struct infos + pwm_out &out = pwm_group_list[i]; + out.gpio_num = outputs_pins[i]; + out.unit_num = unit; + out.timer_num = timer; + out.io_signal = signal; + out.op = operators[i%2]; + out.chan = i; + + //Setup gpio + mcpwm_gpio_init(unit, signal, outputs_pins[i]); + //Setup MCPWM module + mcpwm_config_t pwm_config; + pwm_config.frequency = 50; //frequency = 50Hz, i.e. for every servo motor time period should be 20ms + pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 0 + pwm_config.cmpr_b = 0; //duty cycle of PWMxb = 0 + pwm_config.counter_mode = MCPWM_UP_COUNTER; + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; + mcpwm_init(unit, timer, &pwm_config); + mcpwm_start(unit, timer); + } + + _initialized = true; +} + + + +void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) +{ + if (!_initialized) { + return; + } + + for (uint8_t i = 0; i < MAX_CHANNELS; i++) { + if (chmask & 1 << i) { + pwm_out &out = pwm_group_list[i]; + mcpwm_set_frequency(out.unit_num, out.timer_num, freq_hz); + } + } +} + +void RCOutput::set_default_rate(uint16_t freq_hz) +{ + if (!_initialized) { + return; + } + set_freq(0xFFFFFFFF, freq_hz); +} + +uint16_t RCOutput::get_freq(uint8_t chan) +{ + if (!_initialized || chan >= MAX_CHANNELS) { + return 50; + } + + pwm_out &out = pwm_group_list[chan]; + return mcpwm_get_frequency(out.unit_num, out.timer_num); +} + +void RCOutput::enable_ch(uint8_t chan) +{ + if (!_initialized || chan >= MAX_CHANNELS) { + return; + } + + pwm_out &out = pwm_group_list[chan]; + mcpwm_start(out.unit_num, out.timer_num); +} + +void RCOutput::disable_ch(uint8_t chan) +{ + if (!_initialized || chan >= MAX_CHANNELS) { + return; + } + + write(chan, 0); + pwm_out &out = pwm_group_list[chan]; + mcpwm_stop(out.unit_num, out.timer_num); +} + +void RCOutput::write(uint8_t chan, uint16_t period_us) +{ + if (!_initialized || chan >= MAX_CHANNELS) { + return; + } + + if (_corked) { + _pending[chan] = period_us; + _pending_mask |= (1U<= MAX_CHANNELS || !_initialized) { + return 0; + } + + pwm_out &out = pwm_group_list[chan]; + double freq = mcpwm_get_frequency(out.unit_num, out.timer_num); + double dprc = mcpwm_get_duty(out.unit_num, out.timer_num, out.op); + return (1000000.0 * (dprc / 100.)) / freq; +} + +void RCOutput::read(uint16_t *period_us, uint8_t len) +{ + for (int i = 0; i < MIN(len, _max_channels); i++) { + period_us[i] = read(i); + } +} + +void RCOutput::cork() +{ + _corked = true; +} + +void RCOutput::push() +{ + if (!_corked) { + return; + } + + bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; + + for (uint8_t i = 0; i < MAX_CHANNELS; i++) { + if ((1U<= MAX_CHANNELS) { + return; + } + + bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; + if (safety_on && !(safety_mask & (1U<<(chan)))) { + // safety is on, overwride pwm + period_us = safe_pwm[chan]; + } + + pwm_out &out = pwm_group_list[chan]; + mcpwm_set_duty_in_us(out.unit_num, out.timer_num, out.op, period_us); +} + +/* + get safety switch state for Util.cpp + */ +AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) +{ + if (!hal.util->was_watchdog_reset()) { + hal.util->persistent_data.safety_state = safety_state; + } + return safety_state; +} + +/* + force the safety switch on, disabling PWM output from the IO board +*/ +bool RCOutput::force_safety_on(void) +{ + safety_state = AP_HAL::Util::SAFETY_DISARMED; + return true; +} + +/* + force the safety switch off, enabling PWM output from the IO board +*/ +void RCOutput::force_safety_off(void) +{ + safety_state = AP_HAL::Util::SAFETY_ARMED; +} + +/* + set PWM to send to a set of channels when the safety switch is + in the safe state +*/ +void RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us) +{ + for (uint8_t i=0; i<16; i++) { + if (chmask & (1U<get_safety_mask(); + } + +#ifdef HAL_GPIO_PIN_SAFETY_IN + //TODO replace palReadLine + // handle safety button + bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_IN); + if (safety_pressed) { + AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton(); + if (safety_press_count < 255) { + safety_press_count++; + } + if (brdconfig && brdconfig->safety_button_handle_pressed(safety_press_count)) { + if (safety_state ==AP_HAL::Util::SAFETY_ARMED) { + safety_state = AP_HAL::Util::SAFETY_DISARMED; + } else { + safety_state = AP_HAL::Util::SAFETY_ARMED; + } + } + } else { + safety_press_count = 0; + } +#endif + +#ifdef HAL_GPIO_PIN_LED_SAFETY + led_counter = (led_counter+1) % 16; + const uint16_t led_pattern = safety_state==AP_HAL::Util::SAFETY_DISARMED?0x5500:0xFFFF; + palWriteLine(HAL_GPIO_PIN_LED_SAFETY, (led_pattern & (1U << led_counter))?0:1); + //TODO replace palReadwrite +#endif +} + +/* + set PWM to send to a set of channels if the FMU firmware dies +*/ +void RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us) +{ + //RIP (not the pointer) +} diff --git a/libraries/AP_HAL_ESP32/RCOutput.h b/libraries/AP_HAL_ESP32/RCOutput.h new file mode 100644 index 0000000000..9e3bb3d8b4 --- /dev/null +++ b/libraries/AP_HAL_ESP32/RCOutput.h @@ -0,0 +1,139 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt + * + */ + +#pragma once + + +#include +#include "HAL_ESP32_Namespace.h" +#include "driver/mcpwm.h" +#define HAL_PARAM_DEFAULTS_PATH nullptr +#include + +namespace ESP32 +{ + +class RCOutput : public AP_HAL::RCOutput +{ +public: + RCOutput() {}; + + ~RCOutput() {}; + + static RCOutput *from(AP_HAL::RCOutput *rcoutput) + { + return static_cast(rcoutput); + } + + void init() override; + + void set_freq(uint32_t chmask, uint16_t freq_hz) override; + uint16_t get_freq(uint8_t chan) override; + + void enable_ch(uint8_t chan) override; + void disable_ch(uint8_t chan) override; + + void write(uint8_t chan, uint16_t period_us) override; + uint16_t read(uint8_t ch) override; + void read(uint16_t* period_us, uint8_t len) override; + + void cork() override; + void push() override; + + void set_default_rate(uint16_t rate_hz) override; + + /* + force the safety switch on, disabling PWM output from the IO board + */ + bool force_safety_on() override; + + /* + force the safety switch off, enabling PWM output from the IO board + */ + void force_safety_off() override; + + /* + set PWM to send to a set of channels when the safety switch is + in the safe state + */ + void set_safety_pwm(uint32_t chmask, uint16_t period_us) ; + + /* + get safety switch state, used by Util.cpp + */ + AP_HAL::Util::safety_state _safety_switch_state(); + + /* + set PWM to send to a set of channels if the FMU firmware dies + */ + void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override; + + /* + set safety mask for IOMCU + */ + void set_safety_mask(uint16_t mask) + { + safety_mask = mask; + } + + + void timer_tick() override; + + +private: + struct pwm_out { + int gpio_num; + mcpwm_unit_t unit_num; + mcpwm_timer_t timer_num; + mcpwm_io_signals_t io_signal; + mcpwm_operator_t op; + uint8_t chan; + }; + + + void write_int(uint8_t chan, uint16_t period_us); + + static pwm_out pwm_group_list[]; + + bool _corked; + uint16_t _pending[12]; //Max channel with 2 unit MCPWM + uint32_t _pending_mask; + + uint16_t safe_pwm[16]; // pwm to use when safety is on + + uint16_t _max_channels; + + // safety switch state + AP_HAL::Util::safety_state safety_state; + uint32_t safety_update_ms; + uint8_t led_counter; + int8_t safety_button_counter; + uint8_t safety_press_count; // 0.1s units + + // mask of channels to allow when safety on + uint16_t safety_mask; + + // update safety switch and LED + void safety_update(void); + + + bool _initialized; + +}; + +} diff --git a/libraries/AP_HAL_ESP32/README.md b/libraries/AP_HAL_ESP32/README.md new file mode 100644 index 0000000000..56092d3070 --- /dev/null +++ b/libraries/AP_HAL_ESP32/README.md @@ -0,0 +1,461 @@ + +# Ardupilot port to the esp32 series mcu + + +## Building instructions +0. Build currently tested on linux +1. Checkout this branch https://github.com/ardupilot/ardupilot/tree/master +2. Use this to install ardupilot requirements: +``` +Tools/environment_install/install-prereqs-ubuntu.sh +``` +or +``` +Tools/environment_install/install-prereqs-arch.sh +``` + +3. 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 python3 python3-pip python3-setuptools +sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 + +#or +sudo pacman -S --needed gcc git make flex bison gperf python-pip cmake ninja ccache dfu-util libusb + + +cd ardupilot +git submodule update --init --recursive +cd modules/esp_idf +./install.sh +unset IDF_PATH +source ./export.sh +cd ../../.. + +``` + +4. Configure and run build: +```bash +cd ardupilot +./waf configure +./waf configure --board=esp32buzz --debug +[ or ./waf configure --board=esp32diy --debug ] +./waf plane +or +./waf copter +``` + +``` +Do NOT use "./waf build", it's broken right now. +``` + +TIPS: + + - ninja: error: loading 'build.ninja': No such file or directory + You need to run a plain './waf configure' FIRST, THEN run it with the --board type you want. + + - 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 ) + + + - 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: +``` +cd build/esp32{BOARD}/esp-idf_build +ninja menuconfig +``` +navigate to [save] (tab,tab,tab,enter) +press [tab] then [ok] to update the sdkconfig file +'config written' press [enter] to exit this dialog +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/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/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" + +5. Recommanded way to flash the firmware : +``` +ESPBAUD=921600 ./waf plane --upload +``` + +6. The espressif esp-idf original project is built at `cd build/esp32{BOARD}/esp-idf_build/`. +You can use your default build system (make or ninja) to build other esp-idf target. + +For example : +- ninja flash +- ninja monitor + +If you want to specify the port, specify before any command: +``` +ESPTOOL_PORT=/dev/ttyUSB1 +``` + +If you want to specify a wanted baudrate : +``` +ESPTOOL_BAUD=57600 +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/esp-idf` for idf.py command. But see next section to understand how ardupilot is compiled on ESP32. + +--- +OLD + +Alternatively, the "./waf plane' build outputs a python command that y can cut-n-paste to flash... buzz found that but using that command with a slower baudrate of 921600 instead of its recommended 2000000 worked for him: +cd ardupilot +python ./modules/esp_idf/components/esptool_py/esptool/esptool.py --chip esp32 --port /dev/ttyUSB0 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size detect 0xf000 ./build/esp32buzz/idf-plane/ota_data_initial.bin 0x1000 ./build/esp32buzz/idf-plane/bootloader/bootloader.bin 0x20000 ./build/esp32buzz/idf-plane/arduplane.bin 0x8000 ./build/esp32buzz/idf-plane/partitions.bin +--- + +## How is compiled Ardupilot on esp32 + +The ESP-IDF os is compiled as a library to the project. + +First waf configure the esp-idf cmake and run a target to get all esp-idf includes. + +Waf compile all ardupilot parts in libardusub.a and libArduSub_libs.a (for example for subs, but similarly for planes, copter, rover). + +Then waf execute the esp-idf cmake to compile esp-idf itself. +And the cmake link the final ardupilot.elf against previous built libraries. + +The ardupilot.elf contains all symbol. The cmake provide a stripped version as ardupilot.bin. + + +## Test hardware +Currently esp32 dev board with connected gy-91 10dof sensor board is supported. Pinout (consult UARTDriver.cpp and SPIDevice.cpp for reference): + +### Uart connecion/s +Internally connected on most devboards, just for reference. + +After flashing the esp32 , u can connect with a terminal app of your preference to the same COM port ( eg /dev/ttyUSB0) at a baud rate of 115200, software flow control, 8N1 common uart settings, and get to see the output from hal.console->printf("...") and other console messages. + +### Console/usb/boot-messages/mavlink telem aka serial0/uart0: + +| ESP32 | CP2102 | +| --- | --- | +| GPIO3 | UART_TX | AKA UART0_RX | +| GPIO1 | UART_RX | AKA UART0_TX | + +### GPS aka serial1/uart1: + +| ESP32 | GPS | +| --- | --- | +| GPIO17 (TX) | GPS (RX) | +| GPIO16 (RX) | GPS (TX) | +| GND | GND | +| 5v | Pwr | + +### RC reciever connection: + +|ESP32| RCRECIEVER | +| --- | --- | +| D4 | CPPM-out | +| GND | GND | +| 5v | Pwr | + + +### I2C connection ( for gps with leds/compass-es/etc onboard, or digital airspeed sensorrs, etc): + +| ESP32 | I2C | +| --- | --- | +| GPIO12 | SCL | +| GPIO13 | SDA | +| GND | GND | +| 5v | Pwr | + + +### Compass (using i2c) + - u need to set the ardupilot params, and connected a GPS that has at least one i2c compass on it.. tested this with a HMC5883 and/or LIS3MDL +COMPASS_ENABLE=1 +COMPASS_EXTERNAL=1 +COMPASS_EXTERN2=1 +COMPASS_EXTERN3=1 + +### Analog input/s + +2nd column is the ardupilot _PIN number and matches what u specify in the third column of HAL_ESP32_ADC_PINS #define elsewhere : + +if HAL_ESP32_ADC_PINS == HAL_ESP32_ADC_PINS_OPTION1: +| ESP32 | AnalogIn | +| --- | --- | +| GPIO35 | 1 | +| GPIO34 | 2 | +| GPIO39 | 3 | +| GPIO36 | 4 | +| GND | GND | + +eg, set ardupilot params like this: +RSSI_ANA_PIN = 3 - and it will attempt to read the adc value on GPIO39 for rssi data +BATT_CURR_PIN = 2 - and it will attempt to read the adc value on GPIO34 for battery current +BATT_VOLT_PIN = 1 - and it will attempt to read the adc value on GPIO35 for battery voltage +ARSPD_PIN = 4 - and it will attempt to read the adc value on GPIO36 for analog airspeed data + + +if HAL_ESP32_ADC_PINS == HAL_ESP32_ADC_PINS_OPTION2: +| ESP32 | AnalogIn | +| --- | --- | +| GPIO35 | 35 | +| GPIO34 | 34 | +| GPIO39 | 39 | +| GPIO36 | 36 | +| GND | GND | + +eg, set ardupilot params like this: +RSSI_ANA_PIN = 39 - and it will attempt to read the adc value on GPIO39 for rssi data +BATT_CURR_PIN = 34 - and it will attempt to read the adc value on GPIO34 for battery current +BATT_VOLT_PIN = 35 - and it will attempt to read the adc value on GPIO35 for battery voltage +ARSPD_PIN = 36 - and it will attempt to read the adc value on GPIO36 for analog airspeed data + + + +### RC Servo connection/s + +| BuzzsPcbHeader|ESP32| RCOUT |TYPICAL | +| --- | --- | --- | --- | +| servo1 |PIN25|SERVO-OUT1|AILERON | +| servo2 |PIN27|SERVO-OUT2|ELEVATOR| +| servo3 |PIN33|SERVO-OUT3|THROTTLE| +| servo4 |PIN32|SERVO-OUT4| RUDDER | +| servo5 |PIN22|SERVO-OUT5| avail | +| servo6 |PIN21|SERVO-OUT6| avail | + +If you don't get any PWM output on any/some/one of the pins while ardupilot is running, be sure you have set all of these params: +//ail +SERVO1_FUNCTION = 4 +// ele +SERVO2_FUNCTION = 19 +//thr +SERVO3_FUNCTION = 70 +//rud +SERVO4_FUNCTION = 21 +// for now make it a copy of ail, in case u have two ail servos and no Y lead +SERVO5_FUNCTION = 4 +// for now make it a copy of ail,for testing etc. +SERVO6_FUNCTION = 4 +// right now, we only have 6 channels of output due to pin limitations.. + + +(If the RTC source is not required, then Pin12 32K_XP and Pin13 32K_XN can be used as digital GPIOs, so we do, and it works) + + +### GY-91 connection +This is how buzz has the GY91 wired ATM, but its probable that connecting external 3.3V supply to the VIN is better than connecting a 5V supply, and then the 3V3 pin on the sensor board can be left disconnected, as it's actually 3.3v out from the LDO onboard. + +|ESP32|GY-91| +|---|---| +|GND|GND| +|5V|VIN| +|3.3V|3V3| +|IO5|NCS| +|IO23|SDA| +|IO19|SDO/SAO| +|IO18|SCL| +|IO26|CSB| + +## debugger connection +Currently used debugger is called a 'TIAO USB Multi Protocol Adapter' which is a red PCB with a bunch of jtag headers on it and doesn't cost too much. https://www.amazon.com/TIAO-Multi-Protocol-Adapter-JTAG-Serial/dp/B0156ML5LY + +|ESP32| 20PINJTAG| +| --- | --- | +|D12 | TDI(PIN5)| +|D13 | SWCLK/TCLK(PIN9)| +|D14 | SWDIO/TMS(PIN7)| +|D15 | SWO/TDO(PIN13)| +|3.3v | -- ( powered via usb, not programmer, or PIN1)| +|GND | GND(any of PIN4,PIN6,or PIN8 , all GND)| +|EN | TRST(PIN3)| + +## SDCARD connection + +|ESP32| SDCARD | +| --- | --- | +|D2 | D0/PIN7 | +|D14 | CLK/PIN5 | +|D15 | CMD/PIN2 | +|GND | Vss1/PIN3 and Vss2/PIN6 | +|3.3v | Vcc/PIN4 | + +## Current progress +### Main tasks +- [x] Build system +- [x] Scheduler and semaphores +- [x] SPI driver +- [x] WiFi driver ( connect with MissionPlanner or mavproxy on TCP to host: 192.168.4.1 and port: 5760 ) +- [x] Uart driver ( non-mavlink console messages and ardupilot Serial0 with mavlink or gps ) +- [X] RCIN driver ( PPMSUM INPUT on GPIO4 ) +- [X] GPS testing/integration ( Serial ublox GPS, as ardupilot SERIAL0 on RX2/TX2 aka GPIO16 and GPIO17 ) +- [X] PWM output driver +- [x] RCOUT driver ( 4 channels working right now) +- [x] I2C driver +- [x] Storage +- [X] OTA update of the fw +- [X] SdCard +- [x] Mavlink on console/usb as well as wifi. +- [x] UDP mavlink over wifi +- [x] TCP mavlink over wifi (choose tcp or udp at compile time , not both) +- [x] parameter storage in a esp32 flash partition area +- [x] Custom boards build +- [x] Perfomance optimization +- [x] SdCard mounts but ardupilot logging to SD does not +- [x] waf can upload to your board with --upload now +- [X] PWM output driver works, but it appears that throttle supression when disarmed does not. +- [X] AnalogIn driver - partial progress not really tested or documented +- [X] Finish waf commands to build seamlessly and wrap all function of esp-idf without command voodoo + +### Known Bugs + +- [ ] slow but functional wifi implementation for tcp & udp. + +### Future development +- [ ] Automatic board header listing +- [ ] UDP mavlink over wifi does not automatically stream to client/s when they connect to wifi AP. +- [ ] Fix parameters loading in wifi both udp and tcp (slow and not reliable) +- [ ] Pin remapping via parameters +- [ ] GPIO driver +- [ ] DShot driver +- [ ] 4way pass +- [ ] esc telemetry +- [ ] ws2812b ws2815 led strip +- [ ] INA219 driver +- [ ] GSD +- [ ] Buzzer + + +### analysing a 'coredump' from the uart... + +Save the log where coredump appears to a file, i'll call it core.txt +================= CORE DUMP START ================= + +================= CORE DUMP END =================== +cat > core.txt +... +ctrl-d +The CORE DUMP START and CORE DUMP END lines must not be included in core dump text file. + +cp build/esp32buzz/idf-plane/arduplane.elf . +espcoredump.py dbg_corefile arduplane.elf -c core.txt -t b64 + +.. this will give u a 'gdb' of the core dump, from which you can type.. +bt +..short for 'backtrace of current thread' or .. +thread apply all bt full +..short for 'backtrace of all threads' or .. +info var +.. which lists "All global and static variable names". +info locals +.. lists "All global and static variable names".. + info args +..to list "Arguments of the current stack frame" (names and values).. + +other things to sue.. +print varname +ptype varname +select-frame 5 + +ctrl-c to exit gdb + +# storage tips - not generally needed, as u can update params with missionplanenner over mavlink etc. + + +# determine offset and size of 'storage' partition in flash +parttool.py --partition-table-file partitions.csv get_partition_info --partition-name storage +>0x3e0000 0x20000 + +# then backup ardupilot 'storage' area (its a partition, see partitions.csv) to a file on disk: +esptool.py read_flash 0x3e0000 0x20000 storage.bin + +# restore the storage.bin to your device... basiclly the same flash command as used in esp32.py but different offset and file: +esptool.py --chip esp32 --baud 921600 --before default_reset --after hard_reset write_flash -z --flash_mode dio --flash_freq 80m --flash_size detect 0x3e0000 storage.bin + + +### example log of boot messages: + +[22:51:20:226] ets Jun 8 2016 00:22:57 +[22:51:20:228] +[22:51:20:228] rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT) +[22:51:20:234] configsip: 0, SPIWP:0xee +[22:51:20:236] clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00 +[22:51:20:242] mode:DIO, clock div:1 +[22:51:20:244] load:0x3fff0018,len:4 +[22:51:20:248] load:0x3fff001c,len:6784 +[22:51:20:248] ho 0 tail 12 room 4 +[22:51:20:252] load:0x40078000,len:12116 +[22:51:20:252] load:0x40080400,len:7420 +[22:51:20:255] entry 0x40080784 +[22:51:20:260] <0x1b>[0;32mI (30) boot: ESP-IDF v3.3-beta2-152-geed94b87e 2nd stage bootloader<0x1b>[0m +[22:51:20:267] <0x1b>[0;32mI (30) boot: compile time 21:12:33<0x1b>[0m +[22:51:20:269] <0x1b>[0;32mI (40) boot: Enabling RNG early entropy source...<0x1b>[0m +[22:51:20:275] <0x1b>[0;32mI (40) qio_mode: Enabling QIO for flash chip WinBond<0x1b>[0m +[22:51:20:281] <0x1b>[0;32mI (41) boot: SPI Speed : 80MHz<0x1b>[0m +[22:51:20:286] <0x1b>[0;32mI (46) boot: SPI Mode : QIO<0x1b>[0m +[22:51:20:290] <0x1b>[0;32mI (50) boot: SPI Flash Size : 4MB<0x1b>[0m +[22:51:20:294] <0x1b>[0;32mI (54) boot: Partition Table:<0x1b>[0m +[22:51:20:297] <0x1b>[0;32mI (57) boot: ## Label Usage Type ST Offset Length<0x1b>[0m +[22:51:20:303] <0x1b>[0;32mI (65) boot: 0 nvs WiFi data 01 02 00009000 00006000<0x1b>[0m +[22:51:20:309] <0x1b>[0;32mI (72) boot: 1 phy_init RF data 01 01 0000f000 00001000<0x1b>[0m +[22:51:20:317] <0x1b>[0;32mI (79) boot: 2 factory factory app 00 00 00010000 00300000<0x1b>[0m +[22:51:20:324] <0x1b>[0;32mI (87) boot: 3 storage unknown 45 00 00310000 00040000<0x1b>[0m +[22:51:20:334] <0x1b>[0;32mI (94) boot: 4 coredump Unknown data 01 03 00350000 00010000<0x1b>[0m +[22:51:20:339] <0x1b>[0;32mI (102) boot: End of partition table<0x1b>[0m +[22:51:20:343] <0x1b>[0;32mI (106) esp_image: segment 0: paddr=0x00010020 vaddr=0x3f400020 size=0x367d4 (223188) map<0x1b>[0m +[22:51:20:400] <0x1b>[0;32mI (174) esp_image: segment 1: paddr=0x000467fc vaddr=0x3ffb0000 size=0x030fc ( 12540) load<0x1b>[0m +[22:51:20:409] <0x1b>[0;32mI (178) esp_image: segment 2: paddr=0x00049900 vaddr=0x40080000 size=0x00400 ( 1024) load<0x1b>[0m +[22:51:20:418] <0x1b>[0;32mI (181) esp_image: segment 3: paddr=0x00049d08 vaddr=0x40080400 size=0x06308 ( 25352) load<0x1b>[0m +[22:51:20:427] <0x1b>[0;32mI (198) esp_image: segment 4: paddr=0x00050018 vaddr=0x400d0018 size=0x128738 (1214264) map<0x1b>[0m +[22:51:20:746] <0x1b>[0;32mI (520) esp_image: segment 5: paddr=0x00178758 vaddr=0x40086708 size=0x0b3fc ( 46076) load<0x1b>[0m +[22:51:20:771] <0x1b>[0;32mI (545) boot: Loaded app from partition at offset 0x10000<0x1b>[0m +[22:51:20:778] <0x1b>[0;32mI (546) boot: Disabling RNG early entropy source...<0x1b>[0m +[22:51:20:788] <0x1b>[0;32mI (546) cpu_start: Pro cpu up.<0x1b>[0m +[22:51:20:788] <0x1b>[0;32mI (550) cpu_start: Application information:<0x1b>[0m +[22:51:20:796] <0x1b>[0;32mI (554) cpu_start: Project name: arduplane<0x1b>[0m +[22:51:20:802] <0x1b>[0;32mI (560) cpu_start: App version: APMrover2-3.1.0-10435-g68525424<0x1b>[0m +[22:51:20:806] <0x1b>[0;32mI (567) cpu_start: Compile time: Apr 5 2019 22:41:05<0x1b>[0m +[22:51:20:811] <0x1b>[0;32mI (573) cpu_start: ELF file SHA256: 3fcbd70b13aca47c...<0x1b>[0m +[22:51:20:816] <0x1b>[0;32mI (579) cpu_start: ESP-IDF: v3.3-beta2-152-geed94b87e<0x1b>[0m +[22:51:20:822] <0x1b>[0;32mI (585) cpu_start: Starting app cpu, entry point is 0x400811d4<0x1b>[0m +[22:51:20:829] <0x1b>[0;32mI (0) cpu_start: App cpu up.<0x1b>[0m +[22:51:20:833] <0x1b>[0;32mI (596) heap_init: Initializing. RAM available for dynamic allocation:<0x1b>[0m +[22:51:20:840] <0x1b>[0;32mI (602) heap_init: At 3FFAE6E0 len 00001920 (6 KiB): DRAM<0x1b>[0m +[22:51:20:846] <0x1b>[0;32mI (609) heap_init: At 3FFC1FF8 len 0001E008 (120 KiB): DRAM<0x1b>[0m +[22:51:20:852] <0x1b>[0;32mI (615) heap_init: At 3FFE0440 len 00003AE0 (14 KiB): D/IRAM<0x1b>[0m +[22:51:20:858] <0x1b>[0;32mI (621) heap_init: At 3FFE4350 len 0001BCB0 (111 KiB): D/IRAM<0x1b>[0m +[22:51:20:865] <0x1b>[0;32mI (628) heap_init: At 40091B04 len 0000E4FC (57 KiB): IRAM<0x1b>[0m +[22:51:20:871] <0x1b>[0;32mI (634) cpu_start: Pro cpu start user code<0x1b>[0m +[22:51:20:880] <0x1b>[0;32mI (206) cpu_start: Starting scheduler on PRO CPU.<0x1b>[0m +[22:51:20:885] <0x1b>[0;32mI (0) cpu_start: Starting scheduler on APP CPU.<0x1b>[0m +[22:51:20:904] I (21) wifi: wifi driver task: 3ffce8ec, prio:23, stack:3584, core=0 +[22:51:20:910] I (253) wifi: wifi firmware version: 9b1a13b +[22:51:20:915] I (253) wifi: config NVS flash: disabled +[22:51:20:922] I (255) wifi: config nano formating: disabled +[22:51:20:923] <0x1b>[0;32mI (259) system_api: Base MAC address is not set, read default base MAC address from BLK0 of EFUSE<0x1b>[0m +[22:51:20:931] <0x1b>[0;32mI (269) system_api: Base MAC address is not set, read default base MAC address from BLK0 of EFUSE<0x1b>[0m +[22:51:20:941] I (279) wifi: Init dynamic tx buffer num: 16 +[22:51:20:946] I (282) wifi: Init data frame dynamic rx buffer num: 16 +[22:51:20:951] I (287) wifi: Init management frame dynamic rx buffer num: 16 +[22:51:20:955] I (293) wifi: Init management short buffer num: 32 +[22:51:20:960] I (297) wifi: Init static rx buffer size: 1600 +[22:51:20:964] I (301) wifi: Init static rx buffer num: 2 +[22:51:20:969] I (305) wifi: Init dynamic rx buffer num: 16 +[22:51:21:701] <0x1b>[0;32mI (1049) phy: phy_version: 4100, 6fa5e27, Jan 25 2019, 17:02:06, 0, 0<0x1b>[0m +[22:51:21:710] I (1050) wifi: mode : softAP (24:0a:c4:32:48:f5) +[22:51:21:712] I (1051) wifi: Init max length of beacon: 752/752 +[22:51:21:716] I (1054) wifi: Init max length of beacon: 752/752 +[22:51:22:023] spi device constructed SPI:BMP280:0:2 +[22:51:22:041] MS4525: no sensor found +[22:51:22:044] spi device constructed SPI:MPU9250:0:1 +[22:51:23:878] Sensor failure: INS: unable to initialise driver +[22:51:26:878] Sensor failure: INS: unable to initialise driver + + diff --git a/libraries/AP_HAL_ESP32/RmtSigReader.cpp b/libraries/AP_HAL_ESP32/RmtSigReader.cpp new file mode 100644 index 0000000000..c7d73b4032 --- /dev/null +++ b/libraries/AP_HAL_ESP32/RmtSigReader.cpp @@ -0,0 +1,76 @@ +#include +#include "RmtSigReader.h" + +#ifdef HAL_ESP32_RCIN + +using namespace ESP32; + +void RmtSigReader::init() +{ + rmt_config_t config; + config.rmt_mode = RMT_MODE_RX; + config.channel = RMT_CHANNEL_0; + config.clk_div = 80; //80MHZ APB clock to the 1MHZ target frequency + config.gpio_num = HAL_ESP32_RCIN; + config.mem_block_num = 2; //each block could store 64 pulses + config.rx_config.filter_en = true; + config.rx_config.filter_ticks_thresh = 8; + config.rx_config.idle_threshold = idle_threshold; + + rmt_config(&config); + rmt_driver_install(config.channel, max_pulses * 8, 0); + rmt_get_ringbuf_handle(config.channel, &handle); + rmt_rx_start(config.channel, true); +} + +bool RmtSigReader::add_item(uint32_t duration, bool level) +{ + bool has_more = true; + if (duration == 0) { + has_more = false; + duration = idle_threshold; + } + if (level) { + if (last_high == 0) { + last_high = duration; + } + } else { + if (last_high != 0) { + ready_high = last_high; + ready_low = duration; + pulse_ready = true; + last_high = 0; + } + } + return has_more; +} + +bool RmtSigReader::read(uint32_t &width_high, uint32_t &width_low) +{ + if (item == nullptr) { + item = (rmt_item32_t*) xRingbufferReceive(handle, &item_size, 0); + item_size /= 4; + current_item = 0; + } + if (item == nullptr) { + return false; + } + bool buffer_empty = (current_item == item_size); + buffer_empty = buffer_empty || + !add_item(item[current_item].duration0, item[current_item].level0); + buffer_empty = buffer_empty || + !add_item(item[current_item].duration1, item[current_item].level1); + current_item++; + if (buffer_empty) { + vRingbufferReturnItem(handle, (void*) item); + item = nullptr; + } + if (pulse_ready) { + width_high = ready_high; + width_low = ready_low; + pulse_ready = false; + return true; + } + return false; +} +#endif diff --git a/libraries/AP_HAL_ESP32/RmtSigReader.h b/libraries/AP_HAL_ESP32/RmtSigReader.h new file mode 100644 index 0000000000..25ef14584d --- /dev/null +++ b/libraries/AP_HAL_ESP32/RmtSigReader.h @@ -0,0 +1,42 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +#include +#include "AP_HAL_ESP32.h" +#include "driver/rmt.h" + +class ESP32::RmtSigReader +{ +public: + static const int frequency = 1000000; //1MHZ + static const int max_pulses = 128; + static const int idle_threshold = 3000; //we require at least 3ms gap between frames + void init(); + bool read(uint32_t &width_high, uint32_t &width_low); +private: + bool add_item(uint32_t duration, bool level); + + RingbufHandle_t handle; + rmt_item32_t* item; + size_t item_size; + size_t current_item; + + uint32_t last_high; + uint32_t ready_high; + uint32_t ready_low; + bool pulse_ready; +}; diff --git a/libraries/AP_HAL_ESP32/SPIDevice.cpp b/libraries/AP_HAL_ESP32/SPIDevice.cpp new file mode 100644 index 0000000000..f62f3c13ec --- /dev/null +++ b/libraries/AP_HAL_ESP32/SPIDevice.cpp @@ -0,0 +1,238 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "SPIDevice.h" + +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" +#include + +using namespace ESP32; + +#define MHZ (1000U*1000U) +#define KHZ (1000U) + +//#define SPIDEBUG 1 + +SPIDeviceDesc device_desc[] = {HAL_ESP32_SPI_DEVICES}; +SPIBusDesc bus_desc[] = {HAL_ESP32_SPI_BUSES}; + +SPIBus::SPIBus(uint8_t _bus): + DeviceBus(Scheduler::SPI_PRIORITY), bus(_bus) +{ + +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + spi_bus_config_t config = { + .mosi_io_num = bus_desc[_bus].mosi, + .miso_io_num = bus_desc[_bus].miso, + .sclk_io_num = bus_desc[_bus].sclk, + .quadwp_io_num = -1, + .quadhd_io_num = -1 + }; + spi_bus_initialize(bus_desc[_bus].host, &config, bus_desc[_bus].dma_ch); +} + +SPIDevice::SPIDevice(SPIBus &_bus, SPIDeviceDesc &_device_desc) + : bus(_bus) + , device_desc(_device_desc) +{ + +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + 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); + gpio_set_direction(device_desc.cs, GPIO_MODE_OUTPUT); + gpio_set_level(device_desc.cs, 1); + + spi_device_interface_config_t cfg_low; + memset(&cfg_low, 0, sizeof(cfg_low)); + cfg_low.mode = _device_desc.mode; + cfg_low.clock_speed_hz = _device_desc.lspeed; + cfg_low.spics_io_num = -1; + cfg_low.queue_size = 5; + spi_bus_add_device(bus_desc[_bus.bus].host, &cfg_low, &low_speed_dev_handle); + + if (_device_desc.hspeed != _device_desc.lspeed) { + spi_device_interface_config_t cfg_high; + memset(&cfg_high, 0, sizeof(cfg_high)); + cfg_high.mode = _device_desc.mode; + cfg_high.clock_speed_hz = _device_desc.hspeed; + cfg_high.spics_io_num = -1; + cfg_high.queue_size = 5; + spi_bus_add_device(bus_desc[_bus.bus].host, &cfg_high, &high_speed_dev_handle); + } + + + asprintf(&pname, "SPI:%s:%u:%u", + device_desc.name, 0, (unsigned)device_desc.device); + printf("spi device constructed %s\n", pname); +} + +SPIDevice::~SPIDevice() +{ + free(pname); +} + +spi_device_handle_t SPIDevice::current_handle() +{ + if (speed == AP_HAL::Device::SPEED_HIGH && high_speed_dev_handle != nullptr) { + return high_speed_dev_handle; + } + return low_speed_dev_handle; +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed _speed) +{ +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + speed = _speed; + return true; +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (!send || !recv) { + // simplest cases + transfer_fullduplex(send, recv, recv_len?recv_len:send_len); + return true; + } + uint8_t buf[send_len+recv_len]; + if (send_len > 0) { + memcpy(buf, send, send_len); + } + if (recv_len > 0) { + memset(&buf[send_len], 0, recv_len); + } + transfer_fullduplex(buf, buf, send_len+recv_len); + if (recv_len > 0) { + memcpy(recv, &buf[send_len], recv_len); + } + return true; +} + +bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) +{ +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + acquire_bus(true); + spi_transaction_t t; + memset(&t, 0, sizeof(t)); + t.length = len*8; + t.tx_buffer = send; + t.rxlength = len*8; + t.rx_buffer = recv; + spi_device_transmit(current_handle(), &t); + acquire_bus(false); + return true; +} + +void SPIDevice::acquire_bus(bool accuire) +{ +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (accuire) { + spi_device_acquire_bus(current_handle(), portMAX_DELAY); + gpio_set_level(device_desc.cs, 0); + } else { + gpio_set_level(device_desc.cs, 1); + spi_device_release_bus(current_handle()); + } +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ +#ifdef SPIDEBUG + rintf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + return &bus.semaphore; +} + +AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ +#ifdef SPIDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + return bus.register_periodic_callback(period_usec, cb, this); +} + +bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ +#ifdef SPIDEBUG + printf("%s:%d %s\n", __PRETTY_FUNCTION__, __LINE__, name); +#endif + uint8_t i; + for (i = 0; i(nullptr); + } + SPIDeviceDesc &desc = device_desc[i]; + +#ifdef SPIDEBUG + printf("%s:%d 222\n", __PRETTY_FUNCTION__, __LINE__); +#endif + // find the bus + SPIBus *busp; + for (busp = buses; busp; busp = (SPIBus *)busp->next) { + if (busp->bus == desc.bus) { + break; + } + } + +#ifdef SPIDEBUG + printf("%s:%d 333\n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (busp == nullptr) { + // create a new one + busp = new SPIBus(desc.bus); + if (busp == nullptr) { + return nullptr; + } + busp->next = buses; + busp->bus = desc.bus; + buses = busp; + } + +#ifdef SPIDEBUG + printf("%s:%d 444\n", __PRETTY_FUNCTION__, __LINE__); +#endif + + return AP_HAL::OwnPtr(new SPIDevice(*busp, desc)); +} + diff --git a/libraries/AP_HAL_ESP32/SPIDevice.h b/libraries/AP_HAL_ESP32/SPIDevice.h new file mode 100644 index 0000000000..da5dc07bb4 --- /dev/null +++ b/libraries/AP_HAL_ESP32/SPIDevice.h @@ -0,0 +1,99 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include "AP_HAL_ESP32.h" + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +#include "driver/spi_master.h" +#include "driver/spi_common.h" +#include "driver/gpio.h" + + +namespace ESP32 +{ + +struct SPIDeviceDesc { + const char *name; + uint8_t bus; + uint8_t device; + gpio_num_t cs; + uint16_t mode; + uint32_t lspeed; + uint32_t hspeed; +}; + +struct SPIBusDesc { + spi_host_device_t host; + int dma_ch; + gpio_num_t mosi; + gpio_num_t miso; + gpio_num_t sclk; + gpio_num_t cs; +}; + +class SPIBus : public DeviceBus +{ +public: + SPIBus(uint8_t _bus); + uint8_t bus; +}; + +class SPIDevice : public AP_HAL::SPIDevice +{ +public: + SPIDevice(SPIBus &_bus, SPIDeviceDesc &_device_desc); + virtual ~SPIDevice(); + + bool set_speed(AP_HAL::Device::Speed speed) override; + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, + uint32_t len) override; + AP_HAL::Semaphore *get_semaphore() override; + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + +private: + SPIBus &bus; + SPIDeviceDesc &device_desc; + Speed speed; + char *pname; + spi_device_handle_t low_speed_dev_handle; + spi_device_handle_t high_speed_dev_handle; + spi_device_handle_t current_handle(); + void acquire_bus(bool accuire); +}; + +class SPIDeviceManager : public AP_HAL::SPIDeviceManager +{ +public: + friend class SPIDevice; + + AP_HAL::OwnPtr get_device(const char *name) override; + +private: + SPIBus *buses; +}; +} + diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp new file mode 100644 index 0000000000..96e13a49c6 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Scheduler.cpp @@ -0,0 +1,500 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "AP_HAL_ESP32/Scheduler.h" +#include "AP_HAL_ESP32/RCInput.h" +#include "AP_HAL_ESP32/AnalogIn.h" +#include "AP_Math/AP_Math.h" +#include "SdCard.h" +#include "Profile.h" + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "soc/rtc_wdt.h" +#include "esp_int_wdt.h" +#include "esp_task_wdt.h" + +#include +#include + +//#define SCHEDULERDEBUG 1 + +using namespace ESP32; + +extern const AP_HAL::HAL& hal; + +bool Scheduler::_initialized = true; + +Scheduler::Scheduler() +{ + _initialized = false; +} + +void disableCore0WDT() +{ + TaskHandle_t 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"); + } +} +void disableCore1WDT() +{ + TaskHandle_t 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 Scheduler::init() +{ + +#ifdef SCHEDDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + + //xTaskCreatePinnedToCore(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle, 0); + xTaskCreate(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle); + xTaskCreate(_timer_thread, "APM_TIMER", TIMER_SS, this, TIMER_PRIO, &_timer_task_handle); + xTaskCreate(_rcout_thread, "APM_RCOUT", RCOUT_SS, this, RCOUT_PRIO, &_rcout_task_handle); + xTaskCreate(_rcin_thread, "APM_RCIN", RCIN_SS, this, RCIN_PRIO, &_rcin_task_handle); + xTaskCreate(_uart_thread, "APM_UART", UART_SS, this, UART_PRIO, &_uart_task_handle); + xTaskCreate(_io_thread, "APM_IO", IO_SS, this, IO_PRIO, &_io_task_handle); + xTaskCreate(_storage_thread, "APM_STORAGE", STORAGE_SS, this, STORAGE_PRIO, &_storage_task_handle); //no actual flash writes without this, storage kinda appears to work, but does an erase on every boot and params don't persist over reset etc. + + // xTaskCreate(_print_profile, "APM_PROFILE", IO_SS, this, IO_PRIO, nullptr); + + //disableCore0WDT(); + //disableCore1WDT(); + +} + +template +void executor(T oui) +{ + oui(); +} + +void Scheduler::thread_create_trampoline(void *ctx) +{ + AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx; + (*t)(); + free(t); +} + +/* + create a new thread +*/ +bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) +{ +#ifdef SCHEDDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + + // take a copy of the MemberProc, it is freed after thread exits + AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)calloc(1, sizeof(proc)); + if (!tproc) { + return false; + } + *tproc = proc; + + uint8_t thread_priority = IO_PRIO; + static const struct { + priority_base base; + uint8_t p; + } priority_map[] = { + { PRIORITY_BOOST, IO_PRIO}, + { PRIORITY_MAIN, MAIN_PRIO}, + { PRIORITY_SPI, SPI_PRIORITY}, + { PRIORITY_I2C, I2C_PRIORITY}, + { PRIORITY_CAN, IO_PRIO}, + { PRIORITY_TIMER, TIMER_PRIO}, + { PRIORITY_RCIN, RCIN_PRIO}, + { PRIORITY_IO, IO_PRIO}, + { PRIORITY_UART, UART_PRIO}, + { PRIORITY_STORAGE, STORAGE_PRIO}, + { PRIORITY_SCRIPTING, IO_PRIO}, + }; + for (uint8_t i=0; i= ESP32_SCHEDULER_MAX_TIMER_PROCS) { + printf("Out of timer processes\n"); + return; + } + _timer_sem.take_blocking(); + _timer_proc[_num_timer_procs] = proc; + _num_timer_procs++; + _timer_sem.give(); +} + +void Scheduler::register_io_process(AP_HAL::MemberProc proc) +{ +#ifdef SCHEDDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + _io_sem.take_blocking(); + for (uint8_t i = 0; i < _num_io_procs; i++) { + if (_io_proc[i] == proc) { + _io_sem.give(); + return; + } + } + if (_num_io_procs < ESP32_SCHEDULER_MAX_IO_PROCS) { + _io_proc[_num_io_procs] = proc; + _num_io_procs++; + } else { + printf("Out of IO processes\n"); + } + _io_sem.give(); +} + +void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) +{ + _failsafe = failsafe; +} + +void Scheduler::reboot(bool hold_in_bootloader) +{ + printf("Restarting now...\n"); + hal.rcout->force_safety_on(); + unmount_sdcard(); + esp_restart(); +} + +bool Scheduler::in_main_thread() const +{ + return _main_task_handle == xTaskGetCurrentTaskHandle(); +} + +void Scheduler::set_system_initialized() +{ +#ifdef SCHEDDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (_initialized) { + AP_HAL::panic("PANIC: scheduler::system_initialized called more than once"); + } + + _initialized = true; +} + +bool Scheduler::is_system_initialized() +{ + return _initialized; +} + +void Scheduler::_timer_thread(void *arg) +{ +#ifdef SCHEDDEBUG + printf("%s:%d start\n", __PRETTY_FUNCTION__, __LINE__); +#endif + Scheduler *sched = (Scheduler *)arg; + while (!_initialized) { + sched->delay_microseconds(1000); + } +#ifdef SCHEDDEBUG + printf("%s:%d initialised\n", __PRETTY_FUNCTION__, __LINE__); +#endif + while (true) { + sched->delay_microseconds(1000); + sched->_run_timers(); + //analog in +#ifndef HAL_DISABLE_ADC_DRIVER + ((AnalogIn*)hal.analogin)->_timer_tick(); +#endif + } +} + +void Scheduler::_rcout_thread(void* arg) +{ + Scheduler *sched = (Scheduler *)arg; + while (!_initialized) { + sched->delay_microseconds(1000); + } + + while (true) { + sched->delay_microseconds(4000); + // process any pending RC output requests + hal.rcout->timer_tick(); + } +} + +void Scheduler::_run_timers() +{ +#ifdef SCHEDULERDEBUG + printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (_in_timer_proc) { + return; + } +#ifdef SCHEDULERDEBUG + printf("%s:%d _in_timer_proc \n", __PRETTY_FUNCTION__, __LINE__); +#endif + _in_timer_proc = true; + + int num_procs = 0; + + _timer_sem.take_blocking(); + num_procs = _num_timer_procs; + _timer_sem.give(); + + // now call the timer based drivers + for (int i = 0; i < num_procs; i++) { + if (_timer_proc[i]) { + _timer_proc[i](); + } + } + + // and the failsafe, if one is setup + if (_failsafe != nullptr) { + _failsafe(); + } + + _in_timer_proc = false; +} + +void Scheduler::_rcin_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + while (!_initialized) { + sched->delay_microseconds(20000); + } + hal.rcin->init(); + while (true) { + sched->delay_microseconds(1000); + ((RCInput *)hal.rcin)->_timer_tick(); + } +} + +void Scheduler::_run_io(void) +{ +#ifdef SCHEDULERDEBUG + printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); +#endif + if (_in_io_proc) { + return; + } +#ifdef SCHEDULERDEBUG + printf("%s:%d initialised \n", __PRETTY_FUNCTION__, __LINE__); +#endif + _in_io_proc = true; + + int num_procs = 0; + _io_sem.take_blocking(); + num_procs = _num_io_procs; + _io_sem.give(); + // now call the IO based drivers + for (int i = 0; i < num_procs; i++) { + if (_io_proc[i]) { + _io_proc[i](); + } + } + _in_io_proc = false; +} + +void Scheduler::_io_thread(void* arg) +{ +#ifdef SCHEDDEBUG + printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); +#endif + mount_sdcard(); + Scheduler *sched = (Scheduler *)arg; + while (!sched->_initialized) { + sched->delay_microseconds(1000); + } +#ifdef SCHEDDEBUG + printf("%s:%d initialised \n", __PRETTY_FUNCTION__, __LINE__); +#endif + uint32_t last_sd_start_ms = AP_HAL::millis(); + while (true) { + sched->delay_microseconds(1000); + // run registered IO processes + sched->_run_io(); + + if (!hal.util->get_soft_armed()) { + // if sdcard hasn't mounted then retry it every 3s in the IO + // thread when disarmed + uint32_t now = AP_HAL::millis(); + if (now - last_sd_start_ms > 3000) { + last_sd_start_ms = now; + sdcard_retry(); + } + } + } +} + + +void Scheduler::_storage_thread(void* arg) +{ +#ifdef SCHEDDEBUG + printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); +#endif + Scheduler *sched = (Scheduler *)arg; + while (!_initialized) { + sched->delay_microseconds(10000); + } +#ifdef SCHEDDEBUG + printf("%s:%d initialised \n", __PRETTY_FUNCTION__, __LINE__); +#endif + while (true) { + sched->delay_microseconds(1000); + // process any pending storage writes + hal.storage->_timer_tick(); + //print_profile(); + } +} + +void Scheduler::_print_profile(void* arg) +{ + Scheduler *sched = (Scheduler *)arg; + while (!sched->_initialized) { + sched->delay_microseconds(10000); + } + + while (true) { + sched->delay(10000); + print_profile(); + } + +} + +void Scheduler::_uart_thread(void *arg) +{ +#ifdef SCHEDDEBUG + printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); +#endif + Scheduler *sched = (Scheduler *)arg; + while (!_initialized) { + sched->delay_microseconds(2000); + } +#ifdef SCHEDDEBUG + printf("%s:%d initialised\n", __PRETTY_FUNCTION__, __LINE__); +#endif + while (true) { + sched->delay_microseconds(1000); + for (uint8_t i=0; i_timer_tick(); + } + hal.console->_timer_tick(); + } +} + + +// get the active main loop rate +uint16_t Scheduler::get_loop_rate_hz(void) +{ + if (_active_loop_rate_hz == 0) { + _active_loop_rate_hz = _loop_rate_hz; + } + return _active_loop_rate_hz; +} + +// once every 60 seconds, print some stats... +void Scheduler::print_stats(void) +{ + static int64_t last_run = 0; + if (AP_HAL::millis64() - last_run > 60000) { + char buffer[1024]; + vTaskGetRunTimeStats(buffer); + printf("\n\n%s\n", buffer); + heap_caps_print_heap_info(0); + last_run = AP_HAL::millis64(); + } + + // printf("loop_rate_hz: %d",get_loop_rate_hz()); +} + +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(); +#endif + hal.rcout->init(); + + sched->callbacks->setup(); + + sched->set_system_initialized(); + +#ifdef SCHEDDEBUG + printf("%s:%d initialised\n", __PRETTY_FUNCTION__, __LINE__); +#endif + while (true) { + sched->callbacks->loop(); + sched->delay_microseconds(250); + + sched->print_stats(); // only runs every 60 seconds. + } +} + diff --git a/libraries/AP_HAL_ESP32/Scheduler.h b/libraries/AP_HAL_ESP32/Scheduler.h new file mode 100644 index 0000000000..6978ef818e --- /dev/null +++ b/libraries/AP_HAL_ESP32/Scheduler.h @@ -0,0 +1,122 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once +#include +#include +#include "HAL_ESP32_Namespace.h" + +#define ESP32_SCHEDULER_MAX_TIMER_PROCS 10 +#define ESP32_SCHEDULER_MAX_IO_PROCS 10 + + +/* Scheduler implementation: */ +class ESP32::Scheduler : public AP_HAL::Scheduler +{ + +public: + Scheduler(); + /* AP_HAL::Scheduler methods */ + void init() override; + void set_callbacks(AP_HAL::HAL::Callbacks *cb) + { + callbacks = cb; + }; + void delay(uint16_t ms) override; + void delay_microseconds(uint16_t us) override; + void register_timer_process(AP_HAL::MemberProc) override; + void register_io_process(AP_HAL::MemberProc) override; + void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override; + void reboot(bool hold_in_bootloader) override; + bool in_main_thread() const override; + // check and set the startup state + void set_system_initialized() override; + bool is_system_initialized() override; + + void print_stats(void) ; + uint16_t get_loop_rate_hz(void); + AP_Int16 _active_loop_rate_hz; + AP_Int16 _loop_rate_hz; + + static void thread_create_trampoline(void *ctx); + bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override; + + static const int SPI_PRIORITY = 40; // if your primary imu is spi, this should be above the i2c value, spi is better. + static const int MAIN_PRIO = 10; + static const int I2C_PRIORITY = 5; // if your primary imu is i2c, this should be above the spi value, i2c is not preferred. + static const int TIMER_PRIO = 15; + static const int RCIN_PRIO = 15; + static const int RCOUT_PRIO = 10; + static const int WIFI_PRIO = 7; + static const int UART_PRIO = 6; + static const int IO_PRIO = 5; + static const int STORAGE_PRIO = 4; + + static const int TIMER_SS = 4096; + static const int MAIN_SS = 4096; + static const int RCIN_SS = 4096; + static const int RCOUT_SS = 4096; + static const int WIFI_SS = 4096; + static const int UART_SS = 1024; + static const int DEVICE_SS = 4096; + static const int IO_SS = 4096; + static const int STORAGE_SS = 4096; + +private: + AP_HAL::HAL::Callbacks *callbacks; + AP_HAL::Proc _failsafe; + + AP_HAL::MemberProc _timer_proc[ESP32_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_timer_procs; + + AP_HAL::MemberProc _io_proc[ESP32_SCHEDULER_MAX_IO_PROCS]; + uint8_t _num_io_procs; + + static bool _initialized; + + + + void *_main_task_handle; + void *_timer_task_handle; + void *_rcin_task_handle; + void *_rcout_task_handle; + void *_uart_task_handle; + void *_io_task_handle; + void *test_task_handle; + void *_storage_task_handle; + + static void _main_thread(void *arg); + static void _timer_thread(void *arg); + static void _rcout_thread(void *arg); + static void _rcin_thread(void *arg); + static void _uart_thread(void *arg); + static void _io_thread(void *arg); + static void _storage_thread(void *arg); + + static void set_position(void* arg); + + + static void _print_profile(void* arg); + + static void test_esc(void* arg); + + bool _in_timer_proc; + void _run_timers(); + Semaphore _timer_sem; + + bool _in_io_proc; + void _run_io(); + Semaphore _io_sem; +}; diff --git a/libraries/AP_HAL_ESP32/SdCard.cpp b/libraries/AP_HAL_ESP32/SdCard.cpp new file mode 100644 index 0000000000..f652e865fd --- /dev/null +++ b/libraries/AP_HAL_ESP32/SdCard.cpp @@ -0,0 +1,294 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +//#define HAL_ESP32_SDCARD 1 + + +#include +#include + +#include "SdCard.h" + +#include "esp_vfs_fat.h" +#include "esp_ota_ops.h" + +#include "driver/sdmmc_host.h" +#include "driver/sdspi_host.h" +#include "sdmmc_cmd.h" + +#include +#include +#include +#include "SPIDevice.h" + +#ifdef HAL_ESP32_SDCARD + +#if CONFIG_IDF_TARGET_ESP32S2 ||CONFIG_IDF_TARGET_ESP32C3 +#define SPI_DMA_CHAN host.slot +#else +#define SPI_DMA_CHAN 1 +#endif + +sdmmc_card_t* card = nullptr; + +const size_t buffer_size = 8*1024; +const char* fw_name = "/SDCARD/APM/ardupilot.bin"; + +static bool sdcard_running; +static HAL_Semaphore sem; + +void update_fw() +{ + FILE *f = fopen(fw_name, "r"); + void *buffer = calloc(1, buffer_size); + esp_ota_handle_t update_handle = 0 ; + const esp_partition_t *update_partition = esp_ota_get_next_update_partition(NULL); + size_t nread = 0; + if (f == nullptr || buffer == nullptr || update_partition == nullptr) { + goto done; + } + printf("updating firmware...\n"); + if (esp_ota_begin(update_partition, OTA_SIZE_UNKNOWN, &update_handle) != ESP_OK) { + goto done; + } + do { + nread = fread(buffer, 1, buffer_size, f); + if (nread > 0) { + if (esp_ota_write(update_handle, buffer, nread) != ESP_OK) { + goto done; + } + } + } while (nread > 0); +done: + if (update_handle != 0) { + if (esp_ota_end(update_handle) == ESP_OK && + esp_ota_set_boot_partition(update_partition) == ESP_OK) { + printf("firmware updated\n"); + } + } + if (f != nullptr) { + fclose(f); + } + if (buffer != nullptr) { + free(buffer); + } + unlink(fw_name); +} + +#ifdef HAL_ESP32_SDMMC + +void mount_sdcard_mmc() +{ + printf("Mounting sd \n"); + WITH_SEMAPHORE(sem); + + /* + https://docs.espressif.com/projects/esp-idf/en/v4.1/api-reference/peripherals/sdmmc_host.html + + // we take the MMC parts from this example: + https://github.com/espressif/esp-idf/blob/release/v4.1/examples/storage/sd_card/main/sd_card_example_main.c + + hardcoded SDMMC gpio options.... + + Slot 0 (SDMMC_HOST_SLOT_0) is an 8-bit slot. It uses HS1_* signals in the PIN MUX.- dont use slot0, is used for SPI-flash chip. + + Slot 1 (SDMMC_HOST_SLOT_1) is a 4-bit slot. It uses HS2_* signals in the PIN MUX. + + this is the full list, but u can get away without some (2 or 3) of these in some cases: + Signal Slot 1 + CMD GPIO15 + CLK GPIO14 + D0 GPIO2 + D1 GPIO4 + D2 GPIO12 + D3 GPIO13 + + */ + + // the dedicated SDMMC host peripheral on the esp32 is about twice as fast as SD card SPI interface in '1-line' mode and somewht faster again in '4-line' mode. we've using 1-line mode in this driver, so we need less gpio's + + static const char *TAG = "SD..."; + ESP_LOGI(TAG, "Initializing SD card as SDMMC"); + + sdmmc_host_t host = SDMMC_HOST_DEFAULT(); + + // This initializes the slot without card detect (CD) and write protect (WP) signals. + // Modify slot_config.gpio_cd and slot_config.gpio_wp if your board has these signals. + sdmmc_slot_config_t slot_config = SDMMC_SLOT_CONFIG_DEFAULT(); + + // To use 1-line SD mode (this driver does), uncomment the following line: + slot_config.width = 1; + + // GPIOs 15, 2, 4, 12, 13 should have external 10k pull-ups. + // Internal pull-ups are not sufficient. However, enabling internal pull-ups + // does make a difference some boards, so we do that here. + gpio_set_pull_mode(GPIO_NUM_15, GPIO_PULLUP_ONLY); // CMD, needed in 4- and 1- line modes + gpio_set_pull_mode(GPIO_NUM_2, GPIO_PULLUP_ONLY); // D0, needed in 4- and 1-line modes + //gpio_set_pull_mode(GPIO_NUM_4, GPIO_PULLUP_ONLY); // D1, needed in 4-line mode only + //gpio_set_pull_mode(GPIO_NUM_12, GPIO_PULLUP_ONLY); // D2, needed in 4-line mode only + // + // Pin 13 / chip-select - is an interesting one, because if its the only thing on this + // spi bus(it is), then NOT connecting the SD to this pin, and instead directly to a pull-up + // also asserts the CS pin 'permanently high' to the SD card, without the micro being involved.. + // which means pin 13 on micro can be re-used elsewhere. If one of these isnt true for u, + // then uncomment this line and connect it electrically to the CS pin on the SDcard. + //gpio_set_pull_mode(GPIO_NUM_13, GPIO_PULLUP_ONLY); // D3, needed in 4- and 1-line modes + + + // Options for mounting the filesystem. + // If format_if_mount_failed is set to true, SD card will be partitioned and + // formatted in case when mounting fails. + esp_vfs_fat_sdmmc_mount_config_t mount_config = { + .format_if_mount_failed = false, + .max_files = 10, + .allocation_unit_size = 16 * 1024 + }; + + //https://docs.espressif.com/projects/esp-idf/en/v4.1/api-reference/peripherals/sdmmc_host.html + + + // Use settings defined above to initialize SD card and mount FAT filesystem. + // Note: esp_vfs_fat_sdmmc_mount is an all-in-one convenience function. + // Please check its source code and implement error recovery when developing + // production applications. + sdmmc_card_t* card; + esp_err_t ret = esp_vfs_fat_sdmmc_mount("/SDCARD", &host, &slot_config, &mount_config, &card); + + + if (ret == ESP_OK) { + mkdir("/SDCARD/APM", 0777); + mkdir("/SDCARD/APM/LOGS", 0777); + printf("sdcard is mounted\n"); + //update_fw(); + sdcard_running = true; + } else { + printf("sdcard is not mounted\n"); + sdcard_running = false; + } +} + +void mount_sdcard() +{ + mount_sdcard_mmc(); +} + +#endif // emd mmc + + +#ifdef HAL_ESP32_SDSPI +ESP32::SPIBusDesc bus_ = HAL_ESP32_SDSPI; + +void mount_sdcard_spi() +{ + + esp_err_t ret; + printf("Mounting sd \n"); + WITH_SEMAPHORE(sem); + + // In SPI mode, pins can be customized... + + // and 'SPI bus sharing with SDSPI has been added in 067f3d2 — please see the new sdspi_host_init_device, sdspi_host_remove_device functions'. https://github.com/espressif/esp-idf/issues/1597 buzz..: thats in idf circa 4.3-dev tho. + + // readme shows esp32 pinouts and pullups needed for different modes and 1 vs 4 line gotchass... + //https://github.com/espressif/esp-idf/blob/master/examples/storage/sd_card/README.md + //https://github.com/espressif/esp-idf/blob/master/examples/storage/sd_card/main/sd_card_example_main.c + + static const char *TAG = "SD..."; + ESP_LOGI(TAG, "Initializing SD card as SDSPI"); + esp_vfs_fat_sdmmc_mount_config_t mount_config = { + .format_if_mount_failed = false, + .max_files = 10, + .allocation_unit_size = 16 * 1024 + }; + + sdmmc_host_t host = SDSPI_HOST_DEFAULT(); + //TODO change to sdspi_host_init_device for spi sharing + spi_bus_config_t bus_cfg = { + .mosi_io_num = bus_.mosi, + .miso_io_num = bus_.miso, + .sclk_io_num = bus_.sclk, + .quadwp_io_num = -1, + .quadhd_io_num = -1, + .max_transfer_sz = 4000, + }; + ret = spi_bus_initialize((spi_host_device_t)host.slot, &bus_cfg, SPI_DMA_CHAN); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to initialize bus."); + return; + } + + sdspi_device_config_t slot_config = SDSPI_DEVICE_CONFIG_DEFAULT(); + slot_config.gpio_cs = bus_.cs; + slot_config.host_id = (spi_host_device_t)host.slot; + + //host.flags = SDMMC_HOST_FLAG_1BIT | SDMMC_HOST_FLAG_DDR; + host.max_freq_khz = SDMMC_FREQ_PROBING; + ret = esp_vfs_fat_sdspi_mount("/SDCARD", &host, &slot_config, &mount_config, &card); + + if (ret == ESP_OK) { + // Card has been initialized, print its properties + + mkdir("/SDCARD/APM", 0777); + mkdir("/SDCARD/APM/LOGS", 0777); + printf("sdcard is mounted\n"); + //update_fw(); + sdcard_running = true; + } else { + printf("sdcard is not mounted\n"); + sdcard_running = false; + } +} +void mount_sdcard() +{ + mount_sdcard_spi(); +} +#endif // end spi + + +bool sdcard_retry(void) +{ + if (!sdcard_running) { + mount_sdcard(); + } + return sdcard_running; +} + + +void unmount_sdcard() +{ + if (card != nullptr) { + esp_vfs_fat_sdmmc_unmount(); + } + sdcard_running = false; +} + + +#else +// empty impl's +void mount_sdcard() +{ + printf("No sdcard setup.\n"); +} +void unmount_sdcard() +{ +} +bool sdcard_retry(void) +{ + return true; +} +#endif + + + + diff --git a/libraries/AP_HAL_ESP32/SdCard.h b/libraries/AP_HAL_ESP32/SdCard.h new file mode 100644 index 0000000000..41f8b1d0b3 --- /dev/null +++ b/libraries/AP_HAL_ESP32/SdCard.h @@ -0,0 +1,20 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +void mount_sdcard(); +void unmount_sdcard(); +bool sdcard_retry(); diff --git a/libraries/AP_HAL_ESP32/Semaphores.cpp b/libraries/AP_HAL_ESP32/Semaphores.cpp new file mode 100644 index 0000000000..a397a53b18 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Semaphores.cpp @@ -0,0 +1,75 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include + +#include "Semaphores.h" + +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#include "freertos/task.h" + +extern const AP_HAL::HAL& hal; + +using namespace ESP32; + +Semaphore::Semaphore() +{ + handle = xSemaphoreCreateRecursiveMutex(); +} + +bool Semaphore::give() +{ + return xSemaphoreGiveRecursive((QueueHandle_t)handle); +} + +bool Semaphore::take(uint32_t timeout_ms) +{ + if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { + take_blocking(); + return true; + } + if (take_nonblocking()) { + return true; + } + uint64_t start = AP_HAL::micros64(); + do { + hal.scheduler->delay_microseconds(200); + if (take_nonblocking()) { + return true; + } + } while ((AP_HAL::micros64() - start) < timeout_ms*1000); + return false; +} + +void Semaphore::take_blocking() +{ + xSemaphoreTakeRecursive((QueueHandle_t)handle, portMAX_DELAY); +} + +bool Semaphore::take_nonblocking() +{ + bool ok = xSemaphoreTakeRecursive((QueueHandle_t)handle, 0) == pdTRUE; + if (ok) { + give(); + } + + return ok; +} + +bool Semaphore::check_owner() +{ + return xSemaphoreGetMutexHolder((QueueHandle_t)handle) == xTaskGetCurrentTaskHandle(); +} diff --git a/libraries/AP_HAL_ESP32/Semaphores.h b/libraries/AP_HAL_ESP32/Semaphores.h new file mode 100644 index 0000000000..844fd1b271 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Semaphores.h @@ -0,0 +1,36 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include +#include "HAL_ESP32_Namespace.h" + +class ESP32::Semaphore : public AP_HAL::Semaphore +{ +public: + Semaphore(); + bool give() override; + bool take(uint32_t timeout_ms) override; + bool take_nonblocking() override; + void take_blocking() override; + + bool check_owner(); +protected: + void* handle; +}; diff --git a/libraries/AP_HAL_ESP32/SoftSigReaderInt.cpp b/libraries/AP_HAL_ESP32/SoftSigReaderInt.cpp new file mode 100644 index 0000000000..2a2a81dd3e --- /dev/null +++ b/libraries/AP_HAL_ESP32/SoftSigReaderInt.cpp @@ -0,0 +1,177 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by David "Buzz" Bussenschutt and others + * + */ + +#include "SoftSigReaderInt.h" + +#include +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "driver/gpio.h" + +// future possible TODO - can we use the RMT peripheral on the esp32 to do this ? looks plausible. +// https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/peripherals/rmt.html +// with an example here for both transmit and recieve of IR signals: +// https://github.com/espressif/esp-idf/tree/2f8b6cfc7/examples/peripherals/rmt_nec_tx_rx + + +//for now, we use GPIO interrupts,refer here: +// https://github.com/espressif/esp-idf/blob/master/examples/peripherals/gpio/main/gpio_example_main.c + + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + +using namespace ESP32; +extern const AP_HAL::HAL& hal; + +#if HAL_USE_EICU == TRUE + +#define GPIO_INPUT_IO_0 (gpio_num_t)4 +#define GPIO_INPUT_PIN_SEL ((1ULL<sigbuf.push(pulse); + pulse.w0 = 0; + pulse.w1 = 0; + } + } + + // reset on too-big-a-big gap between pulse edges + if ( AP_HAL::micros() - last_transitioned_time > 1000000 ) { // thats 1 second with no data at all. + //we have probably missed some pulses + //try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse) + //pulse.w0 = 0; + //pulse.w1 = 0; + //_instance->sigbuf.push(pulse); + } +} + +// singleton instance +SoftSigReaderInt *SoftSigReaderInt::_instance; + +SoftSigReaderInt::SoftSigReaderInt() +{ + _instance = this; + printf("SoftSigReaderInt-constructed\n"); +} + +SoftSigReaderInt::~SoftSigReaderInt() +{ + + //remove isr handler for gpio number. + gpio_isr_handler_remove(GPIO_INPUT_IO_0); + +} + +void SoftSigReaderInt::init() +{ + + printf("SoftSigReaderInt::init\n"); + + // lets start with GPIO4: input, pulled up, interrupt from rising edge and falling edge + + gpio_config_t io_conf; + + //interrupt of rising edge + io_conf.intr_type = GPIO_INTR_ANYEDGE; // GPIO_PIN_INTR_POSEDGE; + //bit mask of the pins, use GPIO4/5 here + io_conf.pin_bit_mask = GPIO_INPUT_PIN_SEL; + //set as input mode + io_conf.mode = GPIO_MODE_INPUT; + //disable pull-down mode + io_conf.pull_down_en = (gpio_pulldown_t)0; + //enable pull-up mode + io_conf.pull_up_en = (gpio_pullup_t)1; + // apply settings to this gpio + gpio_config(&io_conf); + + //change gpio intrrupt type for one pin + //gpio_set_intr_type(GPIO_INPUT_IO_0, GPIO_INTR_ANYEDGE); + + //create a queue to handle gpio event from isr + gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t)); + + //install gpio isr service + gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT); + + //hook isr handler for specific gpio pin + //gpio_isr_handler_add(GPIO_INPUT_IO_0, gpio_isr_handler, (void*) GPIO_INPUT_IO_0); + gpio_isr_handler_add(GPIO_INPUT_IO_0, _irq_handler, (void*) GPIO_INPUT_IO_0); + + +} + +bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1) +{ + //printf("SoftSigReaderInt::read\n"); + + if (sigbuf.available() >= 2) { + pulse_t pulse; + if (sigbuf.pop(pulse)) { + widths0 = uint16_t(pulse.w0 - last_value); + widths1 = uint16_t(pulse.w1 - pulse.w0); + last_value = pulse.w1; + return true; + } + } + return false; +} + +#endif // HAL_USE_EICU + +#endif //CONFIG_HAL_BOARD == HAL_BOARD_ESP32 diff --git a/libraries/AP_HAL_ESP32/SoftSigReaderInt.h b/libraries/AP_HAL_ESP32/SoftSigReaderInt.h new file mode 100644 index 0000000000..adae28eda6 --- /dev/null +++ b/libraries/AP_HAL_ESP32/SoftSigReaderInt.h @@ -0,0 +1,71 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by David "Buzz" Bussenschutt and others + * + */ +#pragma once + +#include +#include "HAL_ESP32_Namespace.h" + +#include +#include + +#include "AP_HAL_ESP32.h" + + +#if HAL_USE_EICU == TRUE + +#define INPUT_CAPTURE_FREQUENCY 1000000 //capture unit in microseconds +#ifndef SOFTSIG_MAX_SIGNAL_TRANSITIONS +#define SOFTSIG_MAX_SIGNAL_TRANSITIONS 128 +#endif + +namespace ESP32 +{ + +class SoftSigReaderInt +{ +public: + SoftSigReaderInt(); + ~SoftSigReaderInt(); + /* Do not allow copies */ + SoftSigReaderInt(const SoftSigReaderInt &other) = delete; + SoftSigReaderInt &operator=(const SoftSigReaderInt&) = delete; + + // get singleton + static SoftSigReaderInt *get_instance(void) + { + return _instance; + } + + void init(); + bool read(uint32_t &widths0, uint32_t &widths1); +private: + // singleton + static SoftSigReaderInt *_instance; + + static void _irq_handler(void * arg); + + typedef struct PACKED { + uint16_t w0; + uint16_t w1; + } pulse_t; + ObjectBuffer sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS}; + uint16_t last_value; +}; +} +#endif // HAL_USE_EICU + diff --git a/libraries/AP_HAL_ESP32/SoftSigReaderRMT.cpp b/libraries/AP_HAL_ESP32/SoftSigReaderRMT.cpp new file mode 100644 index 0000000000..8035f015fe --- /dev/null +++ b/libraries/AP_HAL_ESP32/SoftSigReaderRMT.cpp @@ -0,0 +1,158 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by David "Buzz" Bussenschutt and others + * + */ + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + +#include "SoftSigReaderRMT.h" + +using namespace ESP32; + +#define RMT_CLK_DIV 10 /*!< RMT counter clock divider */ +#define RMT_TICK_US (80000000/RMT_CLK_DIV/1000000) /*!< RMT counter value for 10 us.(Source clock is APB clock) */ +#define PPM_IMEOUT_US 3500 /*!< RMT receiver timeout value(us) */ + + +// the RMT peripheral on the esp32 to do this ? looks plausible. +// https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/peripherals/rmt.html +// with an example here for both transmit and recieve of IR signals: +// https://github.com/espressif/esp-idf/tree/2f8b6cfc7/examples/peripherals/rmt_nec_tx_rx + +extern const AP_HAL::HAL& hal; + + +void SoftSigReaderRMT::init() +{ + + printf("SoftSigReaderRMT::init\n"); + + printf("%s\n",__PRETTY_FUNCTION__); + + // in case the peripheral was left in a bad state, such as reporting full buffers, this can help clear it, and can be called repeatedly if need be. + //periph_module_reset(PERIPH_RMT_MODULE); + + + rmt_config_t config; + config.rmt_mode = RMT_MODE_RX; + config.channel = RMT_CHANNEL_0; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_ICARUS + config.gpio_num = (gpio_num_t)36; +#endif +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_DIY + config.gpio_num = (gpio_num_t)4; +#endif +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_BUZZ + config.gpio_num = (gpio_num_t)4; +#endif + config.clk_div = RMT_CLK_DIV; + config.mem_block_num = 1; + config.rx_config.filter_en = true; + config.rx_config.filter_ticks_thresh = 100; + config.rx_config.idle_threshold = PPM_IMEOUT_US * (RMT_TICK_US); + + rmt_config(&config); + rmt_driver_install(config.channel, 1000, 0); + rmt_get_ringbuf_handle(config.channel, &rb); + + // we could start it here, but then we get RMT RX BUFFER FULL message will we start calling read() + //rmt_rx_start(config.channel, true); +} + + +/* +this is what the inside of a rmt_item32_t looks like: +// noting that its 15bits+1bit and another 15bits+ 1bit, so values over 32767 in "duration" bits don't work. +// and the entire size is 32bits +typedef struct rmt_item32_s { + union { + struct { + uint32_t duration0 :15; + uint32_t level0 :1; + uint32_t duration1 :15; + uint32_t level1 :1; + }; + uint32_t val; + }; +} rmt_item32_t; +*/ + +bool SoftSigReaderRMT::read(uint32_t &widths0, uint32_t &widths1) +{ + + //printf("%s\n",__PRETTY_FUNCTION__); + + // delayed start till the threads are initialised and we are ready to read() from it.... + if (! started ) { + rmt_rx_start(RMT_CHANNEL_0, true); + started = true; + } + + size_t rx_size = 0; + + static uint32_t channeldata0[16] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 }; // don't hardcode this + static uint32_t channeldata1[16] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 }; // don't hardcode this + static int channelpointer = -1; + + int channels; + + // always give priority to handling the RMT queue first + rmt_item32_t* item = (rmt_item32_t*) xRingbufferReceive(rb, &rx_size, 0); + + if (item) { + + channels = (rx_size / 4) - 1; + //printf("PPM RX %d (%d) channels: ", channels, rx_size); + for (int i = 0; i < channels; i++) { + //printf("%04d ", ((item+i)->duration1 + (item+i)->duration0) / RMT_TICK_US); + channeldata0[i] = ((item+i)->duration0)/RMT_TICK_US; + channeldata1[i] = ((item+i)->duration1)/RMT_TICK_US; + if ( channelpointer < 0 ) { + channelpointer = 0; + } + } + //printf("\n"); + + vRingbufferReturnItem(rb, (void*) item); + item = nullptr; + } + + // each time we are externally called as read() we'll give some return data to the caller. + if ( channelpointer >= 0 ) { + widths0 = uint16_t(channeldata0[channelpointer]); + widths1 = uint16_t(channeldata1[channelpointer]); + + //printf(" hi low ch -> %d %d %d\n",width_high,width_low,channelpointer); + channelpointer++; + + // in here, after the 8th channel, we're going to re-insert the "wide" pulse that is the idle pulse for ppmsum: + if ( channelpointer == 9 ) { + widths0 = 3000; // must together add up over 2700 + widths1 = 1000; + } + if ( channelpointer > 9 ) { + channelpointer = 0; + return false; + } + return true; + } + + return false; + +} + + +#endif //CONFIG_HAL_BOARD == HAL_BOARD_ESP32 diff --git a/libraries/AP_HAL_ESP32/SoftSigReaderRMT.h b/libraries/AP_HAL_ESP32/SoftSigReaderRMT.h new file mode 100644 index 0000000000..b7900f1691 --- /dev/null +++ b/libraries/AP_HAL_ESP32/SoftSigReaderRMT.h @@ -0,0 +1,47 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by David "Buzz" Bussenschutt and others + * + */ +#pragma once + +#include +#include "AP_HAL_ESP32.h" +#include "driver/rmt.h" + +namespace ESP32 +{ + +class SoftSigReaderRMT +{ +public: + // get singleton + static SoftSigReaderRMT *get_instance(void) + { + return _instance; + } + + void init(); + bool read(uint32_t &widths0, uint32_t &widths1); +private: + RingbufHandle_t rb = NULL; + static SoftSigReaderRMT *_instance; + + uint16_t last_value; + bool started = false; + +}; +} + diff --git a/libraries/AP_HAL_ESP32/Storage.cpp b/libraries/AP_HAL_ESP32/Storage.cpp new file mode 100644 index 0000000000..58d0181a72 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Storage.cpp @@ -0,0 +1,217 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#include +#include +#include "Storage.h" + +//#define STORAGEDEBUG 1 + +using namespace ESP32; + +extern const AP_HAL::HAL& hal; + +void Storage::_storage_open(void) +{ + if (_initialised) { + return; + } +#ifdef STORAGEDEBUG + printf("%s:%d _storage_open \n", __PRETTY_FUNCTION__, __LINE__); +#endif + _dirty_mask.clearall(); + p = esp_partition_find_first((esp_partition_type_t)0x45, ESP_PARTITION_SUBTYPE_ANY, nullptr); + // load from storage backend + _flash_load(); + _initialised = true; +} + +/* + mark some lines as dirty. Note that there is no attempt to avoid + the race condition between this code and the _timer_tick() code + below, which both update _dirty_mask. If we lose the race then the + result is that a line is written more than once, but it won't result + in a line not being written. +*/ +void Storage::_mark_dirty(uint16_t loc, uint16_t length) +{ +#ifdef STORAGEDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + uint16_t end = loc + length; + for (uint16_t line=loc>>STORAGE_LINE_SHIFT; + line <= end>>STORAGE_LINE_SHIFT; + line++) { + _dirty_mask.set(line); + } +} + +void Storage::read_block(void *dst, uint16_t loc, size_t n) +{ + if (loc >= sizeof(_buffer)-(n-1)) { +#ifdef STORAGEDEBUG + printf("%s:%d read_block failed \n", __PRETTY_FUNCTION__, __LINE__); +#endif + return; + } + _storage_open(); + memcpy(dst, &_buffer[loc], n); +} + +void Storage::write_block(uint16_t loc, const void *src, size_t n) +{ + if (loc >= sizeof(_buffer)-(n-1)) { +#ifdef STORAGEDEBUG + printf("%s:%d write_block failed \n", __PRETTY_FUNCTION__, __LINE__); +#endif + return; + } + if (memcmp(src, &_buffer[loc], n) != 0) { + _storage_open(); + memcpy(&_buffer[loc], src, n); + _mark_dirty(loc, n); + } +} + +void Storage::_timer_tick(void) +{ + if (!_initialised) { + return; + } + if (_dirty_mask.empty()) { + _last_empty_ms = AP_HAL::millis(); + return; + } + + // write out the first dirty line. We don't write more + // than one to keep the latency of this call to a minimum + uint16_t i; + for (i=0; i 5000) { + _last_re_init_ms = now; + bool ok = _flash.re_initialise(); + hal.console->printf("Storage: failed at %u:%u for %u - re-init %u\n", + (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok); +#ifdef STORAGEDEBUG + printf("Storage: failed at %u:%u for %u - re-init %u\n", + (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok); +#endif + } + } + return ret; +} + +/* + callback to read data from flash + */ +bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) +{ + size_t address = sector * STORAGE_SECTOR_SIZE + offset; +#ifdef STORAGEDEBUG + printf("%s:%d -> sec:%u off:%d len:%d addr:%d\n", __PRETTY_FUNCTION__, __LINE__,sector,offset,length,address); +#endif + esp_partition_read(p, address, data, length); + return true; +} + +/* + callback to erase flash sector + */ +bool Storage::_flash_erase_sector(uint8_t sector) +{ +#ifdef STORAGEDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + size_t address = sector * STORAGE_SECTOR_SIZE; + return esp_partition_erase_range(p, address, STORAGE_SECTOR_SIZE) == ESP_OK; +} + +/* + callback to check if erase is allowed + */ +bool Storage::_flash_erase_ok(void) +{ +#ifdef STORAGEDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + // only allow erase while disarmed + return !hal.util->get_soft_armed(); +} + +/* + consider storage healthy if we have nothing to write sometime in the + last 2 seconds + */ +bool Storage::healthy(void) +{ +#ifdef STORAGEDEBUG + printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); +#endif + return _initialised && AP_HAL::millis() - _last_empty_ms < 2000; +} diff --git a/libraries/AP_HAL_ESP32/Storage.h b/libraries/AP_HAL_ESP32/Storage.h new file mode 100644 index 0000000000..81a4aa573e --- /dev/null +++ b/libraries/AP_HAL_ESP32/Storage.h @@ -0,0 +1,68 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +#include +#include "HAL_ESP32_Namespace.h" +#include +#include + +#include "esp_partition.h" + +#define STORAGE_SIZE HAL_STORAGE_SIZE +#define STORAGE_SECTOR_SIZE (64*1024) + +#define STORAGE_LINE_SHIFT 3 + +#define STORAGE_LINE_SIZE (1< _dirty_mask; + + bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length); + bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length); + bool _flash_erase_sector(uint8_t sector); + bool _flash_erase_ok(void); + bool _flash_failed; + uint32_t _last_re_init_ms; + uint32_t _last_empty_ms; + + AP_FlashStorage _flash{_buffer, + STORAGE_SECTOR_SIZE, + FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t), + FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t), + FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t), + FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_ok, bool)}; + + void _flash_load(void); + void _flash_write(uint16_t line); +}; diff --git a/libraries/AP_HAL_ESP32/UARTDriver.cpp b/libraries/AP_HAL_ESP32/UARTDriver.cpp new file mode 100644 index 0000000000..31794ee2e3 --- /dev/null +++ b/libraries/AP_HAL_ESP32/UARTDriver.cpp @@ -0,0 +1,216 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include +#include + +#include "esp_log.h" + +extern const AP_HAL::HAL& hal; + +namespace ESP32 +{ + +UARTDesc uart_desc[] = {HAL_ESP32_UART_DEVICES}; + +void UARTDriver::vprintf(const char *fmt, va_list ap) +{ + + uart_port_t p = uart_desc[uart_num].port; + if (p == 0) { + esp_log_writev(ESP_LOG_INFO, "", fmt, ap); + } else { + AP_HAL::UARTDriver::vprintf(fmt, ap); + } +} + +void UARTDriver::begin(uint32_t b) +{ + begin(b, 0, 0); +} + + +void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + if (uart_num < ARRAY_SIZE(uart_desc)) { + uart_port_t p = uart_desc[uart_num].port; + if (!_initialized) { + + uart_config_t config = { + .baud_rate = (int)b, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + }; + uart_param_config(p, &config); + uart_set_pin(p, + uart_desc[uart_num].tx, + 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); + _readbuf.set_size(RX_BUF_SIZE); + _writebuf.set_size(TX_BUF_SIZE); + + _initialized = true; + } else { + flush(); + uart_set_baudrate(p, b); + + } + } +} + +void UARTDriver::end() +{ + if (_initialized) { + uart_driver_delete(uart_desc[uart_num].port); + _readbuf.set_size(0); + _writebuf.set_size(0); + } + _initialized = false; +} + +void UARTDriver::flush() +{ + uart_port_t p = uart_desc[uart_num].port; + uart_flush(p); +} + +bool UARTDriver::is_initialized() +{ + return _initialized; +} + +void UARTDriver::set_blocking_writes(bool blocking) +{ + //blocking writes do not used anywhere +} + +bool UARTDriver::tx_pending() +{ + return (_writebuf.available() > 0); +} + + +uint32_t UARTDriver::available() +{ + if (!_initialized) { + return 0; + } + return _readbuf.available(); +} + +uint32_t UARTDriver::txspace() +{ + if (!_initialized) { + return 0; + } + int result = _writebuf.space(); + result -= TX_BUF_SIZE / 4; + return MAX(result, 0); + +} + +ssize_t IRAM_ATTR UARTDriver::read(uint8_t *buffer, uint16_t count) +{ + if (!_initialized) { + return -1; + } + + const uint32_t ret = _readbuf.read(buffer, count); + if (ret == 0) { + return 0; + } + + return ret; +} + +int16_t IRAM_ATTR UARTDriver::read() +{ + if (!_initialized) { + return -1; + } + uint8_t byte; + if (!_readbuf.read_byte(&byte)) { + return -1; + } + return byte; +} + +void IRAM_ATTR UARTDriver::_timer_tick(void) +{ + if (!_initialized) { + return; + } + read_data(); + write_data(); +} + +void IRAM_ATTR UARTDriver::read_data() +{ + uart_port_t p = uart_desc[uart_num].port; + int count = 0; + do { + count = uart_read_bytes(p, _buffer, sizeof(_buffer), 0); + if (count > 0) { + _readbuf.write(_buffer, count); + } + } while (count > 0); +} + +void IRAM_ATTR UARTDriver::write_data() +{ + uart_port_t p = uart_desc[uart_num].port; + int count = 0; + _write_mutex.take_blocking(); + do { + count = _writebuf.peekbytes(_buffer, sizeof(_buffer)); + if (count > 0) { + count = uart_tx_chars(p, (const char*) _buffer, count); + _writebuf.advance(count); + } + } while (count > 0); + _write_mutex.give(); +} + +size_t IRAM_ATTR UARTDriver::write(uint8_t c) +{ + return write(&c,1); +} + +size_t IRAM_ATTR UARTDriver::write(const uint8_t *buffer, size_t size) +{ + if (!_initialized) { + return 0; + } + + _write_mutex.take_blocking(); + + + size_t ret = _writebuf.write(buffer, size); + _write_mutex.give(); + return ret; +} + +bool UARTDriver::discard_input() +{ + //uart_port_t p = uart_desc[uart_num].port; + //return uart_flush_input(p) == ESP_OK; + return false; +} + +} diff --git a/libraries/AP_HAL_ESP32/UARTDriver.h b/libraries/AP_HAL_ESP32/UARTDriver.h new file mode 100644 index 0000000000..4eedc36f74 --- /dev/null +++ b/libraries/AP_HAL_ESP32/UARTDriver.h @@ -0,0 +1,96 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include + +#include "driver/gpio.h" +#include "driver/uart.h" + +namespace ESP32 +{ + +struct UARTDesc { + uart_port_t port; + gpio_num_t rx; + gpio_num_t tx; +}; + +class UARTDriver : public AP_HAL::UARTDriver +{ +public: + + UARTDriver(uint8_t serial_num) + : AP_HAL::UARTDriver() + { + _initialized = false; + uart_num = serial_num; + } + + virtual ~UARTDriver() = default; + + void vprintf(const char *fmt, va_list ap) override; + + void begin(uint32_t b) override; + void begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + void end() override; + void flush() override; + bool is_initialized() override; + void set_blocking_writes(bool blocking) override; + bool tx_pending() override; + + uint32_t available() override; + //uint32_t available_locked(uint32_t key) override; + + uint32_t txspace() override; + + ssize_t read(uint8_t *buffer, uint16_t count) override; + int16_t read() override; + //ssize_t read(uint8_t *buffer, uint16_t count) override; + //int16_t read_locked(uint32_t key) override; + + void _timer_tick(void) override; + + size_t write(uint8_t c) override; + size_t write(const uint8_t *buffer, size_t size) override; + + bool discard_input() override; // discard all bytes available for reading + uint32_t bw_in_kilobytes_per_second() const override + { + return 10; + } + + //bool lock_port(uint32_t write_key, uint32_t read_key) override; + + //size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) override; +private: + bool _initialized; + const size_t TX_BUF_SIZE = 1024; + const size_t RX_BUF_SIZE = 1024; + uint8_t _buffer[32]; + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; + Semaphore _write_mutex; + void read_data(); + void write_data(); + + uint8_t uart_num; +}; + +} diff --git a/libraries/AP_HAL_ESP32/Util.cpp b/libraries/AP_HAL_ESP32/Util.cpp new file mode 100644 index 0000000000..1fac298b79 --- /dev/null +++ b/libraries/AP_HAL_ESP32/Util.cpp @@ -0,0 +1,296 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit and David "Buzz" Bussenschutt + */ +#include +#include + +#include "Util.h" + +#include "RCOutput.h" + +#include +#include "SdCard.h" + +#include +#include +#include + +#include +#include +#include "esp_log.h" +#include "esp_system.h" +#include "esp_heap_caps.h" +#include "esp_system.h" + + +extern const AP_HAL::HAL& hal; + +using namespace ESP32; + + +/** + how much free memory do we have in bytes. +*/ +uint32_t Util::available_memory(void) +{ + return heap_caps_get_largest_free_block(MALLOC_CAP_DEFAULT); + +} + +/* + Special Allocation Routines +*/ + +void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) +{ + + // https://docs.espressif.com/projects/esp-idf/en/v4.0.2/api-reference/system/mem_alloc.html + // esp32 has DRAM, IRAM and D/IRAM that can be used as either + + /* + DRAM (Data RAM) is memory used to hold data. This is the most common kind of memory accessed as heap. + + IRAM (Instruction RAM) usually holds executable data only. If accessed as generic memory, all accesses must be 32-bit aligned. + + D/IRAM is RAM which can be used as either Instruction or Data RAM. + */ + + //The ESP-IDF malloc() implementation internally calls heap_caps_malloc(size, MALLOC_CAP_8BIT) in order to allocate DRAM that is byte-addressable. + + //For most purposes, the standard libc malloc() and free() functions can be used for heap allocation without any special consideration. + // return malloc(size); + + if (mem_type == AP_HAL::Util::MEM_DMA_SAFE) { + return heap_caps_calloc(1, size, MALLOC_CAP_DMA); + //} else if (mem_type == AP_HAL::Util::MEM_FAST) { + // return heap_caps_calloc(1, size, MALLOC_CAP_32BIT); //WARNING 32bit memory cannot use unless 32bit access + } else { + return heap_caps_calloc(1, size, MALLOC_CAP_8BIT); + } +} + +void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) +{ + if (ptr != NULL) { + heap_caps_free(ptr); + } +} + + +#ifdef ENABLE_HEAP + +void *Util::allocate_heap_memory(size_t size) +{ + void *buf = calloc(1, size); + if (buf == nullptr) { + return nullptr; + } + + multi_heap_handle_t *heap = (multi_heap_handle_t *)calloc(1, sizeof(multi_heap_handle_t)); + if (heap != nullptr) { + auto hp = multi_heap_register(buf, size); + memcpy(heap, &hp, sizeof(multi_heap_handle_t)); + } + + return heap; +} + +void *Util::heap_realloc(void *heap, void *ptr, size_t new_size) +{ + if (heap == nullptr) { + return nullptr; + } + + return multi_heap_realloc(*(multi_heap_handle_t *)heap, ptr, new_size); +} + +/* + realloc implementation thanks to wolfssl, used by AP_Scripting + */ +void *Util::std_realloc(void *addr, size_t size) +{ + if (size == 0) { + free(addr); + return nullptr; + } + if (addr == nullptr) { + return calloc(1, size); + } + void *new_mem = calloc(1, size); + if (new_mem != nullptr) { + //memcpy(new_mem, addr, chHeapGetSize(addr) > size ? size : chHeapGetSize(addr)); + memcpy(new_mem, addr, size ); + free(addr); + } + return new_mem; +} + +#endif // ENABLE_HEAP + + +/* + get safety switch state + */ +Util::safety_state Util::safety_switch_state(void) +{ + +#if HAL_USE_PWM == TRUE + return ((RCOutput *)hal.rcout)->_safety_switch_state(); +#else + return SAFETY_NONE; +#endif +} + +#ifdef HAL_PWM_ALARM +struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM; + +bool Util::toneAlarm_init() +{ + _toneAlarm_pwm_group.pwm_cfg.period = 1000; + pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg); + + return true; +} + +void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) +{ + if (is_zero(frequency) || is_zero(volume)) { + pwmDisableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan); + } else { + pwmChangePeriod(_toneAlarm_pwm_group.pwm_drv, + roundf(_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)); + + pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2); + } +} +#endif // HAL_PWM_ALARM + +/* + set HW RTC in UTC microseconds +*/ +void Util::set_hw_rtc(uint64_t time_utc_usec) +{ + //stm32_set_utc_usec(time_utc_usec); +} + +/* + get system clock in UTC microseconds +*/ +uint64_t Util::get_hw_rtc() const +{ + return esp_timer_get_time(); +} + +#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) + +#if defined(HAL_NO_GCS) || defined(HAL_BOOTLOADER_BUILD) +#define Debug(fmt, args ...) do { hal.console->printf(fmt, ## args); } while (0) +#else +#include +#define Debug(fmt, args ...) do { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } while (0) +#endif + +Util::FlashBootloader Util::flash_bootloader() +{ + // ....esp32 too + return FlashBootloader::FAIL; +} +#endif // !HAL_NO_FLASH_SUPPORT && !HAL_NO_ROMFS_SUPPORT + +/* + display system identifer - board type and serial number + */ + + +bool Util::get_system_id(char buf[40]) +{ + //uint8_t serialid[12]; + char board_name[14] = "esp32-buzz "; + + uint8_t base_mac_addr[6] = {0}; + esp_err_t ret = esp_efuse_mac_get_custom(base_mac_addr); + if (ret != ESP_OK) { + ret = esp_efuse_mac_get_default(base_mac_addr); + } + + char board_mac[20] = " "; + snprintf(board_mac,20, "%x %x %x %x %x %x", + base_mac_addr[0], base_mac_addr[1], base_mac_addr[2], base_mac_addr[3], base_mac_addr[4], base_mac_addr[5]); + + // null terminate both + board_name[13] = 0; + board_mac[19] = 0; + + // tack strings togehter + snprintf(buf, 40, "%s %s", board_name, board_mac); + // and null terminate that too.. + buf[39] = 0; + return true; +} + +bool Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len) +{ + len = MIN(12, len); + + + uint8_t base_mac_addr[6] = {0}; + esp_err_t ret = esp_efuse_mac_get_custom(base_mac_addr); + if (ret != ESP_OK) { + ret = esp_efuse_mac_get_default(base_mac_addr); + } + + memcpy(buf, (const void *)base_mac_addr, len); + + return true; +} + +// return true if the reason for the reboot was a watchdog reset +bool Util::was_watchdog_reset() const +{ + return false; + esp_reset_reason_t reason = esp_reset_reason(); + + return reason == ESP_RST_PANIC + || reason == ESP_RST_PANIC + || reason == ESP_RST_TASK_WDT + || reason == ESP_RST_WDT; +} + +#if CH_DBG_ENABLE_STACK_CHECK == TRUE +/* + display stack usage as text buffer for @SYS/threads.txt + */ +size_t Util::thread_info(char *buf, size_t bufsize) +{ + thread_t *tp; + size_t total = 0; + + // a header to allow for machine parsers to determine format + int n = snprintf(buf, bufsize, "ThreadsV1\n"); + if (n <= 0) { + return 0; + } + + // char buffer[1024]; + // vTaskGetRunTimeStats(buffer); + // snprintf(buf, bufsize,"\n\n%s\n", buffer); + + // total = .. + + return total; +} +#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE + diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h new file mode 100644 index 0000000000..93ae6fe69d --- /dev/null +++ b/libraries/AP_HAL_ESP32/Util.h @@ -0,0 +1,101 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit and David "Buzz" Bussenschutt + */ +#pragma once + +#include +#include "HAL_ESP32_Namespace.h" +#include "AP_HAL_ESP32.h" + +class ESP32::Util : public AP_HAL::Util +{ +public: + static Util *from(AP_HAL::Util *util) + { + return static_cast(util); + } + + bool run_debug_shell(AP_HAL::BetterStream *stream) override + { + return false; + } + uint32_t available_memory() override; + + // Special Allocation Routines + void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; + void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; + +#ifdef ENABLE_HEAP + // heap functions, note that a heap once alloc'd cannot be dealloc'd + virtual void *allocate_heap_memory(size_t size) override; + virtual void *heap_realloc(void *heap, void *ptr, size_t new_size) override; + virtual void *std_realloc(void *ptr, size_t new_size) override; +#endif // ENABLE_HEAP + + /* + return state of safety switch, if applicable + */ + enum safety_state safety_switch_state(void) override; + + // get system ID as a string + bool get_system_id(char buf[40]) override; + bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; + +#ifdef HAL_PWM_ALARM + bool toneAlarm_init() override; + void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; +#endif + + // return true if the reason for the reboot was a watchdog reset + bool was_watchdog_reset() const override; + +#if CH_DBG_ENABLE_STACK_CHECK == TRUE + // request information on running threads + size_t thread_info(char *buf, size_t bufsize) override; +#endif + +private: +#ifdef HAL_PWM_ALARM + struct ToneAlarmPwmGroup { + pwmchannel_t chan; + PWMConfig pwm_cfg; + PWMDriver* pwm_drv; + }; + + static ToneAlarmPwmGroup _toneAlarm_pwm_group; +#endif + + /* + set HW RTC in UTC microseconds + */ + void set_hw_rtc(uint64_t time_utc_usec) override; + + /* + get system clock in UTC microseconds + */ + uint64_t get_hw_rtc() const override; +#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) + FlashBootloader flash_bootloader() override; +#endif + +#ifdef ENABLE_HEAP + // static memory_heap_t scripting_heap; +#endif // ENABLE_HEAP + + // stm32F4 and F7 have 20 total RTC backup registers. We use the first one for boot type + // flags, so 19 available for persistent data + static_assert(sizeof(persistent_data) <= 19*4, "watchdog persistent data too large"); +}; diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.cpp b/libraries/AP_HAL_ESP32/WiFiDriver.cpp new file mode 100644 index 0000000000..6e5a77f948 --- /dev/null +++ b/libraries/AP_HAL_ESP32/WiFiDriver.cpp @@ -0,0 +1,299 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include +#include +#include + +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "esp_system.h" +#include "esp_wifi.h" +#include "esp_event_loop.h" +#include "nvs_flash.h" + +#include "lwip/err.h" +#include "lwip/sockets.h" +#include "lwip/sys.h" +#include "lwip/netdb.h" + +using namespace ESP32; + +extern const AP_HAL::HAL& hal; + +WiFiDriver::WiFiDriver() +{ + _state = NOT_INITIALIZED; + accept_socket = -1; + + for (unsigned short i = 0; i < WIFI_MAX_CONNECTION; ++i) { + socket_list[i] = -1; + } +} + +void WiFiDriver::begin(uint32_t b) +{ + begin(b, 0, 0); +} + +void WiFiDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + if (_state == NOT_INITIALIZED) { + initialize_wifi(); + xTaskCreate(_wifi_thread, "APM_WIFI", Scheduler::WIFI_SS, this, Scheduler::WIFI_PRIO, &_wifi_task_handle); + _readbuf.set_size(RX_BUF_SIZE); + _writebuf.set_size(TX_BUF_SIZE); + _state = INITIALIZED; + } +} + +void WiFiDriver::end() +{ + //TODO +} + +void WiFiDriver::flush() +{ +} + +bool WiFiDriver::is_initialized() +{ + return _state != NOT_INITIALIZED; +} + +void WiFiDriver::set_blocking_writes(bool blocking) +{ + //blocking writes do not used anywhere +} + +bool WiFiDriver::tx_pending() +{ + return (_writebuf.available() > 0); +} + +uint32_t WiFiDriver::available() +{ + if (_state != CONNECTED) { + return 0; + } + return _readbuf.available(); +} + +uint32_t WiFiDriver::txspace() +{ + if (_state != CONNECTED) { + return 0; + } + int result = _writebuf.space(); + result -= TX_BUF_SIZE / 4; + return MAX(result, 0); +} + +int16_t WiFiDriver::read() +{ + if (_state != CONNECTED) { + return -1; + } + uint8_t byte; + if (!_readbuf.read_byte(&byte)) { + return -1; + } + return byte; +} + +bool WiFiDriver::start_listen() +{ + accept_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (accept_socket < 0) { + accept_socket = -1; + return false; + } + int opt; + setsockopt(accept_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + struct sockaddr_in destAddr; + destAddr.sin_addr.s_addr = htonl(INADDR_ANY); + destAddr.sin_family = AF_INET; + destAddr.sin_port = htons(5760); + int err = bind(accept_socket, (struct sockaddr *)&destAddr, sizeof(destAddr)); + if (err != 0) { + close(accept_socket); + accept_socket = 0; + return false; + } + err = listen(accept_socket, 5); + if (err != 0) { + close(accept_socket); + accept_socket = -1; + return false; + } + return true; + +} + +bool WiFiDriver::try_accept() +{ + struct sockaddr_in sourceAddr; + uint addrLen = sizeof(sourceAddr); + short i = available_socket(); + if (i != WIFI_MAX_CONNECTION) { + socket_list[i] = accept(accept_socket, (struct sockaddr *)&sourceAddr, &addrLen); + if (socket_list[i] >= 0) { + fcntl(socket_list[i], F_SETFL, O_NONBLOCK); + return true; + } + } + return false; +} + +bool WiFiDriver::read_data() +{ + for (unsigned short i = 0; i < WIFI_MAX_CONNECTION && socket_list[i] > -1; ++i) { + int count = 0; + do { + count = recv(socket_list[i], (void *)_buffer, sizeof(_buffer), 0); + if (count > 0) { + _readbuf.write(_buffer, count); + if (count == sizeof(_buffer)) { + _more_data = true; + } + } else if (count < 0 && errno != EAGAIN) { + shutdown(socket_list[i], 0); + close(socket_list[i]); + socket_list[i] = -1; + _state = INITIALIZED; + return false; + } + } while (count > 0); + } + return true; +} + +bool WiFiDriver::write_data() +{ + for (unsigned short i = 0; i < WIFI_MAX_CONNECTION && socket_list[i] > -1; ++i) { + int count = 0; + _write_mutex.take_blocking(); + do { + count = _writebuf.peekbytes(_buffer, sizeof(_buffer)); + if (count > 0) { + count = send(socket_list[i], (void*) _buffer, count, 0); + if (count > 0) { + _writebuf.advance(count); + if (count == sizeof(_buffer)) { + _more_data = true; + } + } else if (count < 0 && errno != EAGAIN) { + shutdown(socket_list[i], 0); + close(socket_list[i]); + socket_list[i] = -1; + _state = INITIALIZED; + _write_mutex.give(); + return false; + } + } + } while (count > 0); + } + _write_mutex.give(); + return true; +} + +void WiFiDriver::initialize_wifi() +{ + tcpip_adapter_init(); + nvs_flash_init(); + esp_event_loop_init(nullptr, nullptr); + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + esp_wifi_init(&cfg); + esp_wifi_set_storage(WIFI_STORAGE_FLASH); + wifi_config_t wifi_config; + memset(&wifi_config, 0, sizeof(wifi_config)); +#ifdef WIFI_SSID + strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); +#else + strcpy((char *)wifi_config.ap.ssid, "ardupilot"); +#endif +#ifdef WIFI_PWD + strcpy((char *)wifi_config.ap.password, WIFI_PWD); +#else + strcpy((char *)wifi_config.ap.password, "ardupilot1"); +#endif + wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK; + wifi_config.ap.max_connection = WIFI_MAX_CONNECTION; + esp_wifi_set_mode(WIFI_MODE_AP); + esp_wifi_set_config(WIFI_IF_AP, &wifi_config); + esp_wifi_start(); +} + +size_t WiFiDriver::write(uint8_t c) +{ + return write(&c,1); +} + +size_t WiFiDriver::write(const uint8_t *buffer, size_t size) +{ + if (_state != CONNECTED) { + return 0; + } + if (!_write_mutex.take_nonblocking()) { + return 0; + } + size_t ret = _writebuf.write(buffer, size); + _write_mutex.give(); + return ret; +} + +void WiFiDriver::_wifi_thread(void *arg) +{ + WiFiDriver *self = (WiFiDriver *) arg; + if (!self->start_listen()) { + vTaskDelete(nullptr); + } + while (true) { + if (self->try_accept()) { + self->_state = CONNECTED; + while (true) { + self->_more_data = false; + if (!self->read_data()) { + self->_state = INITIALIZED; + break; + } + if (!self->write_data()) { + self->_state = INITIALIZED; + break; + } + if (!self->_more_data) { + hal.scheduler->delay_microseconds(1000); + } + } + } + } +} + +bool WiFiDriver::discard_input() +{ + return false; +} + +unsigned short WiFiDriver::available_socket() +{ + for (unsigned short i = 0; i < WIFI_MAX_CONNECTION; ++i) + if (socket_list[i] == -1) { + return i; + } + + return WIFI_MAX_CONNECTION; +} diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.h b/libraries/AP_HAL_ESP32/WiFiDriver.h new file mode 100644 index 0000000000..19e492d8bc --- /dev/null +++ b/libraries/AP_HAL_ESP32/WiFiDriver.h @@ -0,0 +1,79 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include + +#ifndef WIFI_MAX_CONNECTION +#define WIFI_MAX_CONNECTION 5 +#endif + +class ESP32::WiFiDriver : public AP_HAL::UARTDriver +{ +public: + WiFiDriver(); + + void begin(uint32_t b) override; + void begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + void end() override; + void flush() override; + bool is_initialized() override; + void set_blocking_writes(bool blocking) override; + bool tx_pending() override; + + uint32_t available() override; + uint32_t txspace() override; + int16_t read() override; + + size_t write(uint8_t c) override; + size_t write(const uint8_t *buffer, size_t size) override; + + uint32_t bw_in_kilobytes_per_second() const override + { + return 1000; + } + + + bool discard_input() override; + + bool _more_data; +private: + enum ConnectionState { + NOT_INITIALIZED, + INITIALIZED, + CONNECTED + }; + const size_t TX_BUF_SIZE = 1024; + const size_t RX_BUF_SIZE = 1024; + uint8_t _buffer[32]; + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; + Semaphore _write_mutex; + ConnectionState _state; + short accept_socket; + short socket_list[WIFI_MAX_CONNECTION]; + void *_wifi_task_handle; + void initialize_wifi(); + bool read_data(); + bool write_data(); + bool start_listen(); + bool try_accept(); + static void _wifi_thread(void* arg); + unsigned short available_socket(); +}; diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp new file mode 100644 index 0000000000..9d6c66833e --- /dev/null +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp @@ -0,0 +1,250 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include +#include +#include + +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "esp_system.h" +#include "esp_wifi.h" +#include "esp_event_loop.h" +#include "nvs_flash.h" +#include "esp_event.h" +#include "esp_log.h" + +#include "lwip/err.h" +#include "lwip/sockets.h" +#include "lwip/sys.h" +#include "lwip/netdb.h" + +using namespace ESP32; + +extern const AP_HAL::HAL& hal; + +#define UDP_PORT 14550 + +WiFiUdpDriver::WiFiUdpDriver() +{ + _state = NOT_INITIALIZED; + accept_socket = -1; +} + +void WiFiUdpDriver::begin(uint32_t b) +{ + begin(b, 0, 0); +} + +void WiFiUdpDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + if (_state == NOT_INITIALIZED) { + initialize_wifi(); + if (!start_listen()) { + return; + } + + xTaskCreate(_wifi_thread, "APM_WIFI", Scheduler::WIFI_SS, this, Scheduler::WIFI_PRIO, &_wifi_task_handle); + _readbuf.set_size(RX_BUF_SIZE); + _writebuf.set_size(TX_BUF_SIZE); + _state = INITIALIZED; + } +} + +void WiFiUdpDriver::end() +{ + //TODO +} + +void WiFiUdpDriver::flush() +{ +} + +bool WiFiUdpDriver::is_initialized() +{ + return true; +} + +void WiFiUdpDriver::set_blocking_writes(bool blocking) +{ + //blocking writes do not used anywhere +} + +bool WiFiUdpDriver::tx_pending() +{ + return (_writebuf.available() > 0); +} + +uint32_t WiFiUdpDriver::available() +{ + return _readbuf.available(); +} + +uint32_t WiFiUdpDriver::txspace() +{ + int result = _writebuf.space(); + result -= TX_BUF_SIZE / 4; + return MAX(result, 0); +} + +int16_t WiFiUdpDriver::read() +{ + if (!_read_mutex.take_nonblocking()) { + return 0; + } + + uint8_t byte; + if (!_readbuf.read_byte(&byte)) { + return -1; + } + + _read_mutex.give(); + return byte; +} + +bool WiFiUdpDriver::start_listen() +{ + accept_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + if (accept_socket < 0) { + accept_socket = -1; + return false; + } + int opt; + setsockopt(accept_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + struct sockaddr_in destAddr; + destAddr.sin_addr.s_addr = htonl(INADDR_ANY); + destAddr.sin_family = AF_INET; + destAddr.sin_port = htons(UDP_PORT); + int err = bind(accept_socket, (struct sockaddr *)&destAddr, sizeof(destAddr)); + if (err != 0) { + close(accept_socket); + accept_socket = 0; + return false; + } + //memset(&client_addr, 0, sizeof(client_addr)); + fcntl(accept_socket, F_SETFL, O_NONBLOCK); + + return true; + +} + +bool WiFiUdpDriver::read_all() +{ + _read_mutex.take_blocking(); + struct sockaddr_in client_addr; + socklen_t socklen = sizeof(client_addr); + int count = recvfrom(accept_socket, _buffer, sizeof(_buffer) - 1, 0, (struct sockaddr *)&client_addr, &socklen); + if (count > 0) { + _readbuf.write(_buffer, count); + _read_mutex.give(); + } else { + return false; + } + _read_mutex.give(); + return true; +} + +bool WiFiUdpDriver::write_data() +{ + + _write_mutex.take_blocking(); + struct sockaddr_in dest_addr; + dest_addr.sin_addr.s_addr = inet_addr("192.168.4.255"); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(UDP_PORT); + int count = _writebuf.peekbytes(_buffer, sizeof(_buffer)); + if (count > 0) { + count = sendto(accept_socket, _buffer, count, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr)); + if (count > 0) { + _writebuf.advance(count); + } else { + _write_mutex.give(); + return false; + } + } + _write_mutex.give(); + return true; +} + +void WiFiUdpDriver::initialize_wifi() +{ + esp_event_loop_init(nullptr, nullptr); + + tcpip_adapter_init(); + nvs_flash_init(); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + esp_wifi_init(&cfg); + esp_wifi_set_storage(WIFI_STORAGE_FLASH); + wifi_config_t wifi_config; + memset(&wifi_config, 0, sizeof(wifi_config)); +#ifdef WIFI_SSID + strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); +#else + strcpy((char *)wifi_config.ap.ssid, "ardupilot"); +#endif +#ifdef WIFI_PWD + strcpy((char *)wifi_config.ap.password, WIFI_PWD); +#else + strcpy((char *)wifi_config.ap.password, "ardupilot1"); +#endif + wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK; + wifi_config.ap.max_connection = 4; + esp_wifi_set_mode(WIFI_MODE_AP); + esp_wifi_set_config(WIFI_IF_AP, &wifi_config); + esp_wifi_start(); +} + +size_t WiFiUdpDriver::write(uint8_t c) +{ + return write(&c,1); +} + +size_t WiFiUdpDriver::write(const uint8_t *buffer, size_t size) +{ + if (!_write_mutex.take_nonblocking()) { + return 0; + } + size_t ret = _writebuf.write(buffer, size); + _write_mutex.give(); + return ret; +} + +void WiFiUdpDriver::_wifi_thread(void *arg) +{ + WiFiUdpDriver *self = (WiFiUdpDriver *) arg; + while (true) { + struct timeval tv = { + .tv_sec = 0, + .tv_usec = 1000, + }; + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(self->accept_socket, &rfds); + + int s = select(self->accept_socket + 1, &rfds, NULL, NULL, &tv); + if (s > 0 && FD_ISSET(self->accept_socket, &rfds)) { + self->read_all(); + } + self->write_data(); + } +} + +bool WiFiUdpDriver::discard_input() +{ + return false; +} diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.h b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h new file mode 100644 index 0000000000..ba94f4f849 --- /dev/null +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h @@ -0,0 +1,77 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include +#include "lwip/sockets.h" +#include "esp_event.h" + + +class ESP32::WiFiUdpDriver : public AP_HAL::UARTDriver +{ +public: + WiFiUdpDriver(); + + void begin(uint32_t b) override; + void begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + void end() override; + void flush() override; + bool is_initialized() override; + void set_blocking_writes(bool blocking) override; + bool tx_pending() override; + + uint32_t available() override; + uint32_t txspace() override; + int16_t read() override; + + size_t write(uint8_t c) override; + size_t write(const uint8_t *buffer, size_t size) override; + + uint32_t bw_in_kilobytes_per_second() const override + { + return 1000; + } + + + bool discard_input() override; +private: + enum ConnectionState { + NOT_INITIALIZED, + INITIALIZED, + CONNECTED + }; + const size_t TX_BUF_SIZE = 1024; + const size_t RX_BUF_SIZE = 1024; + uint8_t _buffer[32]; + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; + Semaphore _write_mutex; + Semaphore _read_mutex; + ConnectionState _state; + + int accept_socket; + + void *_wifi_task_handle; + void initialize_wifi(); + bool read_all(); + bool write_data(); + bool start_listen(); + bool try_accept(); + static void _wifi_thread(void* arg); +}; diff --git a/libraries/AP_HAL_ESP32/boards/defaults.parm b/libraries/AP_HAL_ESP32/boards/defaults.parm new file mode 100644 index 0000000000..ed3f89d82c --- /dev/null +++ b/libraries/AP_HAL_ESP32/boards/defaults.parm @@ -0,0 +1,3 @@ +#defaults for all ESP32 boards unless overwridden elsewhere +LOG_DISARMED 1 +SERIAL0_PROTOCOL 0 diff --git a/libraries/AP_HAL_ESP32/boards/esp32buzz.h b/libraries/AP_HAL_ESP32/boards/esp32buzz.h new file mode 100644 index 0000000000..8eb73e827d --- /dev/null +++ b/libraries/AP_HAL_ESP32/boards/esp32buzz.h @@ -0,0 +1,183 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +// make sensor selection clearer +#define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) +#define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) +#define PROBE_IMU_SPI2(driver, devname1, devname2, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname1),hal.spi->get_device(devname2),##args)) + +#define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) +#define PROBE_BARO_SPI(driver, devname, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(hal.spi->get_device(devname)),##args)) + +#define PROBE_MAG_I2C(driver, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(GET_I2C_DEVICE(bus, addr),##args)) +#define PROBE_MAG_SPI(driver, devname, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(hal.spi->get_device(devname),##args)) +#define PROBE_MAG_IMU(driver, imudev, imu_instance, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(imu_instance,##args)) +#define PROBE_MAG_IMU_I2C(driver, imudev, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(GET_I2C_DEVICE(bus,addr),##args)) +//------------------------------------ + + +//#define CONFIG_HAL_BOARD 12 +//#define HAL_BOARD_ESP32 12 + +//INS choices: +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +//#define HAL_INS_MPU9250_NAME "MPU9250" + +// or this: +//#define HAL_INS_DEFAULT HAL_INS_ICM20XXX_I2C +//#define HAL_INS_ICM20XXX_I2C_BUS 0 +//#define HAL_INS_ICM20XXX_I2C_ADDR (0x68) + +// BARO choices: +#define HAL_BARO_DEFAULT HAL_BARO_BMP280_SPI +#define HAL_BARO_BMP280_NAME "BMP280" +// or one of these: +//#define HAL_BARO_DEFAULT HAL_BARO_MS5837_I2C +// or: GPIO 34 +//#define HAL_BARO_ANALOG_PIN (6) + +// MAG/COMPASS choices: +//#define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 +// or others: +//#define HAL_COMPASS_ICM20948_I2C_ADDR (0x68) +//#define HAL_COMPASS_AK09916_I2C_BUS 0 +//#define HAL_COMPASS_AK09916_I2C_ADDR (0x0C) +//#define HAL_COMPASS_MAX_SENSORS 3 + +// IMU probing: +//#define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensensev2, 0, 0x68, ROTATION_YAW_270) +// MAG/COMPASS probing: +//#define HAL_MAG_PROBE_LIST ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948_I2C(0, ROTATION_NONE)); +// BARO probing: +//#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(BMP280, 0, 0x77) + +// allow boot without a baro +#define HAL_BARO_ALLOW_INIT_NO_BARO 1 + + +// ADC is available on lots of pints on the esp32, but adc2 cant co-exist with wifi we choose to allow ADC on : +//#define HAL_DISABLE_ADC_DRIVER 1 +#define TRUE 1 +#define HAL_USE_ADC TRUE + +// the pin number, the gain/multiplier associated with it, the ardupilot name for the pin in parameter/s. +// +// two different pin numbering schemes, both are ok, but only one at a time: +#define HAL_ESP32_ADC_PINS_OPTION1 {\ + {ADC1_GPIO35_CHANNEL, 11, 1},\ + {ADC1_GPIO34_CHANNEL, 11, 2},\ + {ADC1_GPIO39_CHANNEL, 11, 3},\ + {ADC1_GPIO36_CHANNEL, 11, 4}\ +} +#define HAL_ESP32_ADC_PINS_OPTION2 {\ + {ADC1_GPIO35_CHANNEL, 11, 35},\ + {ADC1_GPIO34_CHANNEL, 11, 34},\ + {ADC1_GPIO39_CHANNEL, 11, 39},\ + {ADC1_GPIO36_CHANNEL, 11, 36}\ +} +// pick one: +//#define HAL_ESP32_ADC_PINS HAL_ESP32_ADC_PINS_OPTION1 +#define HAL_ESP32_ADC_PINS HAL_ESP32_ADC_PINS_OPTION2 + + + +#define HAL_PROBE_EXTERNAL_I2C_COMPASSES 1 + + +#define HAL_INS_MPU9250_NAME "mpu9250" + +// uncommenting one or more of these will give more console debug in certain areas.. +//#define INSEDEBUG 1 +//#define STORAGEDEBUG 1 +//#define SCHEDDEBUG 1 +//#define FSDEBUG 1 +//#define BUSDEBUG 1 + +#define HAL_INS_PROBE_LIST PROBE_IMU_SPI( Invensense, HAL_INS_MPU9250_NAME, ROTATION_NONE) +//#define HAL_INS_PROBE_LIST PROBE_IMU_SPI( Invensense, HAL_INS_MPU9250_NAME, ROTATION_ROLL_180) + + +#define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(BMP280, "bmp280") + +// 2 use udp, 1 use tcp... for udp,client needs to connect as UDPCL in missionplanner etc to 192.168.4.1 port 14550 +#define HAL_ESP32_WIFI 1 + +// tip: if u are ok getting mavlink-over-tcp or mavlink-over-udp and want to disable mavlink-over-serial-usb +//then set ardupilot parameter SERIAL0_PROTOCOL = 0 and reboot. +// u also will/may want.. +//LOG_BACKEND_TYPE 1 +//LOG_DISARMED 1 +//SERIAL0_PROTOCOL 0 + + +// see boards.py +#ifndef ENABLE_HEAP +#define ENABLE_HEAP 1 +#endif + +#define WIFI_SSID "ardupilot123" +#define WIFI_PWD "ardupilot123" + +//RCOUT which pins are used? + +#define HAL_ESP32_RCOUT { GPIO_NUM_25,GPIO_NUM_27, GPIO_NUM_33, GPIO_NUM_32, GPIO_NUM_22, GPIO_NUM_21 } + +// SPI BUS setup, including gpio, dma, etc +// note... we use 'vspi' for the bmp280 and mpu9250 +#define HAL_ESP32_SPI_BUSES \ + {.host=VSPI_HOST, .dma_ch=1, .mosi=GPIO_NUM_23, .miso=GPIO_NUM_19, .sclk=GPIO_NUM_18} +// tip: VSPI_HOST is an alternative name for esp's SPI3 +//#define HAL_ESP32_SPI_BUSES {} + +// SPI per-device setup, including speeds, etc. +#define HAL_ESP32_SPI_DEVICES \ + {.name= "bmp280", .bus=0, .device=0, .cs=GPIO_NUM_26, .mode = 3, .lspeed=1*MHZ, .hspeed=1*MHZ}, \ + {.name="mpu9250", .bus=0, .device=1, .cs=GPIO_NUM_5, .mode = 0, .lspeed=2*MHZ, .hspeed=8*MHZ} +//#define HAL_ESP32_SPI_DEVICES {} + +//I2C bus list +#define HAL_ESP32_I2C_BUSES \ + {.port=I2C_NUM_0, .sda=GPIO_NUM_13, .scl=GPIO_NUM_12, .speed=400*KHZ, .internal=true} +//#define HAL_ESP32_I2C_BUSES {} // using this embty block appears to cause crashes? + + +// rcin on what pin? +#define HAL_ESP32_RCIN GPIO_NUM_4 + + +//HARDWARE UARTS +#define HAL_ESP32_UART_DEVICES \ + {.port=UART_NUM_0, .rx=GPIO_NUM_3, .tx=GPIO_NUM_1 },{.port=UART_NUM_1, .rx=GPIO_NUM_16, .tx=GPIO_NUM_17 } + +#define HAVE_FILESYSTEM_SUPPORT 1 + +// 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, +#define HAL_ESP32_SDMMC 1 +// and spi is more flexible pinouts.... dont forget vspi/hspi should be selected to NOT conflict with SPI_BUSES above +//#define HAL_ESP32_SDSPI {.host=VSPI_HOST, .dma_ch=2, .mosi=GPIO_NUM_2, .miso=GPIO_NUM_15, .sclk=GPIO_NUM_14, .cs=GPIO_NUM_21} + +#define HAL_ESP32_SDCARD 1 +#define LOGGER_MAVLINK_SUPPORT 1 +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" +#define HAL_BOARD_STORAGE_DIRECTORY "/SDCARD/APM/STORAGE" +#define HAL_OS_POSIX_IO 1 + +// this becomes the default value for the ardupilot param LOG_BACKEND_TYPE, which most ppl want to be 1, for log-to-flash +// setting to 2 means log-over-mavlink to a companion computer etc. +#define HAL_LOGGING_BACKENDS_DEFAULT 1 + diff --git a/libraries/AP_HAL_ESP32/boards/esp32diy.h b/libraries/AP_HAL_ESP32/boards/esp32diy.h new file mode 100644 index 0000000000..34b6272db2 --- /dev/null +++ b/libraries/AP_HAL_ESP32/boards/esp32diy.h @@ -0,0 +1,113 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +// make sensor selection clearer +#define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) +#define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) +#define PROBE_IMU_SPI2(driver, devname1, devname2, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname1),hal.spi->get_device(devname2),##args)) + +#define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) +#define PROBE_BARO_SPI(driver, devname, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(hal.spi->get_device(devname)),##args)) + +#define PROBE_MAG_I2C(driver, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(GET_I2C_DEVICE(bus, addr),##args)) +#define PROBE_MAG_SPI(driver, devname, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(hal.spi->get_device(devname),##args)) +#define PROBE_MAG_IMU(driver, imudev, imu_instance, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(imu_instance,##args)) +#define PROBE_MAG_IMU_I2C(driver, imudev, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(GET_I2C_DEVICE(bus,addr),##args)) +//------------------------------------ + +#define HAL_INS_DEFAULT HAL_INS_ICM20XXX_I2C +#define HAL_INS_ICM20XXX_I2C_BUS 0 +#define HAL_INS_ICM20XXX_I2C_ADDR (0x68) + +#define HAL_LOGGING_STACK_SIZE 4096 + +//#define HAL_BARO_DEFAULT HAL_BARO_MS5837_I2C +//GPIO 34 +//#define HAL_BARO_ANALOG_PIN (6) + +#define HAL_COMPASS_ICM20948_I2C_ADDR (0x68) +#define HAL_COMPASS_AK09916_I2C_BUS 0 +#define HAL_COMPASS_AK09916_I2C_ADDR (0x0C) +#define HAL_COMPASS_MAX_SENSORS 3 + +#define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensensev2, 0, 0x68, ROTATION_ROLL_180) + +#define HAL_MAG_PROBE_LIST ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948_I2C(0, ROTATION_ROLL_180_YAW_270)); + +#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(BMP280, 0, 0x77) + +#define HAL_ESP32_WIFI 1 //To define tcp wifi + +//TODO RCOUT config +#define HAL_ESP32_RCOUT {GPIO_NUM_15, GPIO_NUM_2, GPIO_NUM_0, GPIO_NUM_4} + +#define HAL_ESP32_SPI_BUSES {} + +#define HAL_ESP32_SPI_DEVICES {} + +#define HAL_ESP32_I2C_BUSES \ + {.port=I2C_NUM_0, .sda=GPIO_NUM_5, .scl=GPIO_NUM_18, .speed=400*KHZ, .internal=true, .soft=true},\ + {.port=I2C_NUM_1, .sda=GPIO_NUM_22, .scl=GPIO_NUM_23, .speed=400*KHZ, .internal=true, .soft=false} + +// GPIO36 +#define HAL_BATT_VOLT_PIN (0) +#define HAL_BATT_VOLT_SCALE (18.1) +//GPIO 32 +#define HAL_BATT_CURR_PIN (4) +#define HAL_BATT_CURR_SCALE (36) + +// ADC is available on lots of pints on the esp32, but adc2 cant co-exist with wifi we choose to allow ADC on : +//#define HAL_DISABLE_ADC_DRIVER 1 +#define TRUE 1 +#define HAL_USE_ADC TRUE + +#ifndef ENABLE_HEAP +#define ENABLE_HEAP 0 +#endif + +// the pin number, the gain/multiplier associated with it, the ardupilot name for the pin in parameter/s. +#define HAL_ESP32_ADC_PINS {\ + {ADC1_GPIO36_CHANNEL, 11, 1},\ + {ADC1_GPIO32_CHANNEL, 11, 2}\ +} + +#define HAL_ESP32_RCIN GPIO_NUM_17 + +#define HAL_ESP32_UART_DEVICES \ + {.port=UART_NUM_0, .rx=GPIO_NUM_3, .tx=GPIO_NUM_1 },\ + {.port=UART_NUM_1, .rx=GPIO_NUM_39, .tx=GPIO_NUM_33 },\ + {.port=UART_NUM_2, .rx=GPIO_NUM_34, .tx=GPIO_NUM_25 } + +#define HAVE_FILESYSTEM_SUPPORT 1 +#define HAL_ESP32_SDCARD 1 +#define LOGGER_MAVLINK_SUPPORT 1 +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" +#define HAL_BOARD_STORAGE_DIRECTORY "/SDCARD/APM/STORAGE" +#define HAL_OS_POSIX_IO 1 + +#define HAL_LOGGING_BACKENDS_DEFAULT 2 + + +#define HAL_ESP32_SDSPI \ + {.host=VSPI_HOST, .dma_ch=1, .mosi=GPIO_NUM_19, .miso=GPIO_NUM_35, .sclk=GPIO_NUM_12, .cs=GPIO_NUM_21} + + +#define HAL_LOGGING_BACKENDS_DEFAULT 2 + + + + diff --git a/libraries/AP_HAL_ESP32/boards/esp32icarus.h b/libraries/AP_HAL_ESP32/boards/esp32icarus.h new file mode 100644 index 0000000000..eae9d9c455 --- /dev/null +++ b/libraries/AP_HAL_ESP32/boards/esp32icarus.h @@ -0,0 +1,53 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#define HAL_INS_DEFAULT HAL_INS_MPU60XX_SPI +#define HAL_INS_MPU60x0_NAME "MPU6000" + +#define HAL_BARO_DEFAULT HAL_BARO_BMP280_I2C +#define HAL_BARO_BMP280_BUS 0 +#define HAL_BARO_BMP280_I2C_ADDR (0x76) + +#define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE +#define ALLOW_ARM_NO_COMPASS + +#define HAL_ESP32_SDCARD 1 +#define HAL_OS_POSIX_IO 1 +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" + +#define HAL_ESP32_WIFI 1 + +#define HAL_ESP32_NO_MAVLINK_0 1 + +#define HAL_ESP32_RCIN GPIO_NUM_36 + +#define HAL_ESP32_RCOUT {GPIO_NUM_9, GPIO_NUM_10, GPIO_NUM_27, GPIO_NUM_13, GPIO_NUM_22, GPIO_NUM_21} + +#define HAL_ESP32_SPI_BUSES \ + {.host=VSPI_HOST, .dma_ch=1, .mosi=GPIO_NUM_23, .miso=GPIO_NUM_19, .sclk=GPIO_NUM_18} + +#define HAL_ESP32_SPI_DEVICES \ + {.name="MPU6000", .bus=0, .device=1, .cs=GPIO_NUM_5, .mode=0, .lspeed=1*MHZ, .hspeed=6*MHZ} + +#define HAL_ESP32_I2C_BUSES \ + {.port=I2C_NUM_0, .sda=GPIO_NUM_26, .scl=GPIO_NUM_25, .speed=400*KHZ, .internal=false} + +#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_32},\ + {.port=UART_NUM_2, .rx=GPIO_NUM_35, .tx=GPIO_NUM_33} + diff --git a/libraries/AP_HAL_ESP32/hwdef/esp32buzz/hwdef.dat b/libraries/AP_HAL_ESP32/hwdef/esp32buzz/hwdef.dat new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ESP32/hwdef/esp32diy/hwdef.dat b/libraries/AP_HAL_ESP32/hwdef/esp32diy/hwdef.dat new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ESP32/i2c_sw.c b/libraries/AP_HAL_ESP32/i2c_sw.c new file mode 100644 index 0000000000..ba443791b1 --- /dev/null +++ b/libraries/AP_HAL_ESP32/i2c_sw.c @@ -0,0 +1,725 @@ +/* + * Copyright (C) 2019 Gunar Schorcht + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @ingroup cpu_esp_common + * @ingroup drivers_periph_i2c + * @{ + * + * @file + * @brief Low-level I2C driver software implementation using for ESP SoCs + * + * @author Gunar Schorcht + * + * @} + */ + +/* + PLEASE NOTE: + + The implementation bases on the bit-banging I2C master implementation as + described in [wikipedia](https://en.wikipedia.org/wiki/I%C2%B2C#Example_of_bit-banging_the_I%C2%B2C_master_protocol). +*/ + +#include "esp_err.h" +#define DEBUG printf + +#include +#include +#include +#include + +#include "esp_attr.h" +#include "esp32/rom/ets_sys.h" + +#include "soc/gpio_reg.h" +#include "soc/gpio_struct.h" + + +// IMPORTS FROM esp-idf hw implem +#include +#include +#include "esp_types.h" +#include "esp_attr.h" +#include "esp_intr_alloc.h" +#include "esp_log.h" +#include "malloc.h" +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#include "freertos/xtensa_api.h" +#include "freertos/task.h" +#include "freertos/ringbuf.h" +#include "soc/dport_reg.h" +#include "esp_pm.h" +#include "soc/soc_memory_layout.h" +//#include "hal/i2c_hal.h" +#include "soc/i2c_periph.h" +#include "driver/i2c.h" +#include "driver/periph_ctrl.h" +#include "lwip/netdb.h" +#include "i2c_sw.h" + +/* max clock stretching counter */ +#define I2C_CLOCK_STRETCH 200 + +/* gpio access macros */ +#define GPIO_SET(l,h,b) if (b < 32) GPIO.l = BIT(b); else GPIO.h.val = BIT(b-32) +#define GPIO_GET(l,h,b) ((b < 32) ? GPIO.l & BIT(b) : GPIO.h.val & BIT(b-32)) + +/* to ensure that I2C is always optimized with -O2 to use the defined delays */ +#pragma GCC optimize ("O2") + +static const uint32_t _i2c_delays[][3] = +{ + /* values specify one half-period and are only valid for -O2 option */ + /* value = [period - 0.25 us (240 MHz) / 0.5us(160MHz) / 1.0us(80MHz)] */ + /* * cycles per second / 2 */ + /* 1 us = 16 cycles (80 MHz) / 32 cycles (160 MHz) / 48 cycles (240) */ + /* values for 80, 160, 240 MHz */ + [I2C_SPEED_LOW] = {790, 1590, 2390}, /* 10 kbps (period 100 us) */ + [I2C_SPEED_NORMAL] = { 70, 150, 230}, /* 100 kbps (period 10 us) */ + [I2C_SPEED_FAST] = { 11, 31, 51}, /* 400 kbps (period 2.5 us) */ + [I2C_SPEED_FAST_PLUS] = { 0, 7, 15}, /* 1 Mbps (period 1 us) */ + [I2C_SPEED_HIGH] = { 0, 0, 0} /* 3.4 Mbps (period 0.3 us) not working */ +}; + +/* forward declaration of internal functions */ + +static inline void _i2c_delay(_i2c_bus_t* bus); +static inline bool _i2c_scl_read(_i2c_bus_t* bus); +static inline bool _i2c_sda_read(_i2c_bus_t* bus); +static inline void _i2c_scl_high(_i2c_bus_t* bus); +static inline void _i2c_scl_low(_i2c_bus_t* bus); +static inline void _i2c_sda_high(_i2c_bus_t* bus); +static inline void _i2c_sda_low(_i2c_bus_t* bus); +static int _i2c_start_cond(_i2c_bus_t* bus); +static int _i2c_stop_cond(_i2c_bus_t* bus); +static int _i2c_write_bit(_i2c_bus_t* bus, bool bit); +static int _i2c_read_bit(_i2c_bus_t* bus, bool* bit); +static int _i2c_write_byte(_i2c_bus_t* bus, uint8_t byte); +static int _i2c_read_byte(_i2c_bus_t* bus, uint8_t* byte, bool ack); +static int _i2c_arbitration_lost(_i2c_bus_t* bus, const char* func); +static void _i2c_abort(_i2c_bus_t* bus, const char* func); +static void _i2c_clear(_i2c_bus_t* bus); + +/* implementation of i2c interface */ + +void i2c_init(_i2c_bus_t* bus) +{ + if (bus->speed == I2C_SPEED_HIGH) { + DEBUG("i2c I2C_SPEED_HIGH is not supported\n"); + return; + } + + bus->scl_bit = BIT(bus->scl); /* store bit mask for faster access */ + bus->sda_bit = BIT(bus->sda); /* store bit mask for faster access */ + bus->started = false; /* for handling of repeated start condition */ + + switch (ets_get_cpu_frequency()) { + case 80: bus->delay = _i2c_delays[bus->speed][0]; break; + 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()); + return; + } + + DEBUG("%s: scl=%d sda=%d speed=%d\n", __func__, + bus->scl, bus->sda, bus->speed); + + /* reset the GPIO usage if the pins were used for I2C before */ + gpio_reset_pin(bus->scl); + gpio_reset_pin(bus->sda); + + /* Configure and initialize SDA and SCL pin. */ + /* + * ESP32 pins are used in input/output mode with open-drain output driver. + * Signal levels are then realized as following: + * + * - HIGH: Output value 1 lets the pin floating and is pulled-up to high. + * - LOW : Output value 0 actively drives the pin to low. + */ + gpio_config_t gpio_conf = { + .pin_bit_mask = bus->scl_bit | bus->sda_bit, + .mode = GPIO_MODE_INPUT_OUTPUT_OD, + .pull_up_en = GPIO_PULLUP_ENABLE, + .pull_down_en = GPIO_PULLDOWN_DISABLE, + .intr_type = GPIO_INTR_DISABLE + }; + + esp_err_t err = gpio_config(&gpio_conf); + assert(!err); + + /* set SDA and SCL to be floating and pulled-up to high */ + _i2c_sda_high(bus); + _i2c_scl_high(bus); + + /* clear the bus if necessary (SDA is driven permanently low) */ + _i2c_clear(bus); +} + +int IRAM_ATTR i2c_read_bytes(_i2c_bus_t* bus, uint16_t addr, void *data, size_t len, uint8_t flags) +{ + int res = 0; + + /* send START condition and address if I2C_NOSTART is not set */ + if (!(flags & I2C_NOSTART)) { + + /* START condition */ + if ((res = _i2c_start_cond(bus)) != 0) { + return res; + } + + /* send 10 bit or 7 bit address */ + if (flags & I2C_ADDR10) { + /* prepare 10 bit address bytes */ + uint8_t addr1 = 0xf0 | (addr & 0x0300) >> 7 | I2C_READ; + uint8_t addr2 = addr & 0xff; + /* send address bytes with read flag */ + if ((res = _i2c_write_byte(bus, addr1)) != 0 || + (res = _i2c_write_byte(bus, addr2)) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return -ENXIO; + } + } + else { + /* send address byte with read flag */ + if ((res = _i2c_write_byte(bus, (addr << 1 | I2C_READ))) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return -ENXIO; + } + } + } + + /* receive bytes if send address was successful */ + for (unsigned int i = 0; i < len; i++) { + if ((res = _i2c_read_byte(bus, &(((uint8_t*)data)[i]), i < len-1)) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return res; + } + } + + /* send STOP condition if I2C_NOSTOP flag is not set */ + if (!(flags & I2C_NOSTOP)) { + res = _i2c_stop_cond(bus); + } + + return res; +} + +int IRAM_ATTR i2c_write_bytes(_i2c_bus_t* bus, uint16_t addr, const void *data, size_t len, uint8_t flags) +{ + int res = 0; + + /* if I2C_NOSTART is not set, send START condition and ADDR */ + if (!(flags & I2C_NOSTART)) { + + /* START condition */ + if ((res = _i2c_start_cond(bus)) != 0) { + return res; + } + + /* send 10 bit or 7 bit address */ + if (flags & I2C_ADDR10) { + /* prepare 10 bit address bytes */ + uint8_t addr1 = 0xf0 | (addr & 0x0300) >> 7; + uint8_t addr2 = addr & 0xff; + /* send address bytes without read flag */ + if ((res = _i2c_write_byte(bus, addr1)) != 0 || + (res = _i2c_write_byte(bus, addr2)) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return -ENXIO; + } + } + else { + /* send address byte without read flag */ + if ((res = _i2c_write_byte(bus, addr << 1)) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return -ENXIO; + } + } + } + + /* send bytes if send address was successful */ + for (unsigned int i = 0; i < len; i++) { + if ((res = _i2c_write_byte(bus, ((uint8_t*)data)[i])) != 0) { + /* abort transfer */ + _i2c_abort(bus, __func__); + return res; + } + } + + /* send STOP condition if I2C_NOSTOP flag is not set */ + if (!(flags & I2C_NOSTOP)) { + res = _i2c_stop_cond(bus); + } + + return res; +} + +/* --- internal functions --- */ + +static inline void _i2c_delay(_i2c_bus_t* bus) +{ + /* produces a delay */ + uint32_t cycles = bus->delay; + if (cycles) { + __asm__ volatile ("1: _addi.n %0, %0, -1 \n" + " bnez %0, 1b \n" : "=r" (cycles) : "0" (cycles)); + } +} + +/* + * Please note: SDA and SDL pins are used in GPIO_OD_PU mode + * (open-drain with pull-ups). + * + * Setting a pin which is in open-drain mode leaves the pin floating and + * the signal is pulled up to high. The signal can then be actively driven + * to low by a slave. A read operation returns the current signal at the pin. + * + * Clearing a pin which is in open-drain mode actively drives the signal to + * low. + */ + +static inline bool _i2c_scl_read(_i2c_bus_t* bus) +{ + // return gpio_get_level(bus->scl); + /* read SCL status (pin is in open-drain mode and set) */ + return GPIO_GET(in, in1, bus->scl); +} + +static inline bool _i2c_sda_read(_i2c_bus_t* bus) +{ + // return gpio_get_level(bus->sda); + /* read SDA status (pin is in open-drain mode and set) */ + return GPIO_GET(in, in1, bus->sda); +} + +static inline void _i2c_scl_high(_i2c_bus_t* bus) +{ + // gpio_set_level(bus->scl, 1); + // return; + /* set SCL signal high (pin is in open-drain mode and pulled-up) */ + GPIO_SET(out_w1ts, out1_w1ts, bus->scl); +} + +static inline void _i2c_scl_low(_i2c_bus_t* bus) +{ + // gpio_set_level(bus->scl, 0); + // return; + /* set SCL signal low (actively driven to low) */ + GPIO_SET(out_w1tc, out1_w1tc, bus->scl); +} + +static inline void _i2c_sda_high(_i2c_bus_t* bus) +{ + // gpio_set_level(bus->sda, 1); + // return; + /* set SDA signal high (pin is in open-drain mode and pulled-up) */ + GPIO_SET(out_w1ts, out1_w1ts, bus->sda); +} + +static inline void _i2c_sda_low(_i2c_bus_t* bus) +{ + // gpio_set_level(bus->sda, 0); + // return; + /* set SDA signal low (actively driven to low) */ + GPIO_SET(out_w1tc, out1_w1tc, bus->sda); +} + +static void _i2c_clear(_i2c_bus_t* bus) +{ + //DEBUG("%s: dev=%u\n", __func__, bus->dev); + + /** + * Sometimes a slave blocks and drives the SDA line permanently low. + * Send some clock pulses in that case (10 at maximum) + */ + + /* + * If SDA is low while SCL is high for 10 half cycles, it is not an + * arbitration lost but a bus lock. + */ + int count = 10; + while (!_i2c_sda_read(bus) && _i2c_scl_read(bus) && count) { + count--; + _i2c_delay(bus); + } + + if (count) { + /* was not a bus lock */ + return; + } + + /* send 10 clock pulses in case of bus lock */ + count = 10; + while (!_i2c_sda_read(bus) && count--) { + _i2c_scl_low(bus); + _i2c_delay(bus); + _i2c_scl_high(bus); + _i2c_delay(bus); + } +} + +static void _i2c_abort(_i2c_bus_t* bus, const char* func) +{ + //DEBUG("%s: dev=%u\n", func, bus->dev); + + /* reset SCL and SDA to passive HIGH (floating and pulled-up) */ + _i2c_sda_high(bus); + _i2c_scl_high(bus); + + /* reset repeated start indicator */ + bus->started = false; + + /* clear the bus if necessary (SDA is driven permanently low) */ + _i2c_clear(bus); +} + +static IRAM_ATTR int _i2c_arbitration_lost(_i2c_bus_t* bus, const char* func) +{ + //DEBUG("%s: arbitration lost dev=%u\n", func, bus->dev); + + /* reset SCL and SDA to passive HIGH (floating and pulled-up) */ + _i2c_sda_high(bus); + _i2c_scl_high(bus); + + /* reset repeated start indicator */ + bus->started = false; + + /* clear the bus if necessary (SDA is driven permanently low) */ + _i2c_clear(bus); + + return -EAGAIN; +} + +static IRAM_ATTR int _i2c_start_cond(_i2c_bus_t* bus) +{ + /* + * send start condition + * on entry: SDA and SCL are set to be floating and pulled-up to high + * on exit : SDA and SCL are actively driven to low + */ + + int res = 0; + + if (bus->started) { + /* prepare the repeated start condition */ + + /* SDA = passive HIGH (floating and pulled-up) */ + _i2c_sda_high(bus); + + /* t_VD;DAT not necessary */ + /* _i2c_delay(bus); */ + + /* SCL = passive HIGH (floating and pulled-up) */ + _i2c_scl_high(bus); + + /* clock stretching, wait as long as clock is driven to low by the slave */ + uint32_t stretch = I2C_CLOCK_STRETCH; + while (stretch && !_i2c_scl_read(bus)) { + stretch--; + } + if (stretch == 0) { + //DEBUG("%s: clock stretching timeout dev=%u\n", __func__, bus->dev); + res = -ETIMEDOUT; + } + + /* wait t_SU;STA - set-up time for a repeated START condition */ + /* min. in us: 4.7 (SM), 0.6 (FM), 0.26 (FPM), 0.16 (HSM); no max. */ + _i2c_delay(bus); + } + + /* if SDA is low, arbitration is lost and someone else is driving the bus */ + if (!_i2c_sda_read(bus)) { + return _i2c_arbitration_lost(bus, __func__); + } + + /* begin the START condition: SDA = active LOW */ + _i2c_sda_low(bus); + + /* wait t_HD;STA - hold time (repeated) START condition, */ + /* max none */ + /* min 4.0 us (SM), 0.6 us (FM), 0.26 us (FPM), 0.16 us (HSM) */ + _i2c_delay(bus); + + /* complete the START condition: SCL = active LOW */ + _i2c_scl_low(bus); + + /* needed for repeated start condition */ + bus->started = true; + + return res; +} + +static IRAM_ATTR int _i2c_stop_cond(_i2c_bus_t* bus) +{ + /* + * send stop condition + * on entry: SCL is active low and SDA can be changed + * on exit : SCL and SDA are set to be floating and pulled-up to high + */ + + int res = 0; + + /* begin the STOP condition: SDA = active LOW */ + _i2c_sda_low(bus); + + /* wait t_LOW - LOW period of SCL clock */ + /* min. in us: 4.7 (SM), 1.3 (FM), 0.5 (FPM), 0.16 (HSM); no max. */ + _i2c_delay(bus); + + /* SCL = passive HIGH (floating and pulled up) while SDA = active LOW */ + _i2c_scl_high(bus); + + /* clock stretching, wait as long as clock is driven to low by the slave */ + uint32_t stretch = I2C_CLOCK_STRETCH; + while (stretch && !_i2c_scl_read(bus)) { + stretch--; + } + if (stretch == 0) { + //DEBUG("%s: clock stretching timeout dev=%u\n", __func__, bus->dev); + res = -ETIMEDOUT; + } + + /* wait t_SU;STO - hold time STOP condition, */ + /* min. in us: 4.0 (SM), 0.6 (FM), 0.26 (FPM), 0.16 (HSM); no max. */ + _i2c_delay(bus); + + /* complete the STOP condition: SDA = passive HIGH (floating and pulled up) */ + _i2c_sda_high(bus); + + /* reset repeated start indicator */ + bus->started = false; + + /* wait t_BUF - bus free time between a STOP and a START condition */ + /* min. in us: 4.7 (SM), 1.3 (FM), 0.5 (FPM), 0.16 (HSM); no max. */ + _i2c_delay(bus); + /* one additional delay */ + _i2c_delay(bus); + + /* if SDA is low, arbitration is lost and someone else is driving the bus */ + if (_i2c_sda_read(bus) == 0) { + return _i2c_arbitration_lost(bus, __func__); + } + + return res; +} + +static IRAM_ATTR int _i2c_write_bit(_i2c_bus_t* bus, bool bit) +{ + /* + * send one bit + * on entry: SCL is active low, SDA can be changed + * on exit : SCL is active low, SDA can be changed + */ + + int res = 0; + + /* SDA = bit */ + if (bit) { + _i2c_sda_high(bus); + } + else { + _i2c_sda_low(bus); + } + + /* wait t_VD;DAT - data valid time (time until data are valid) */ + /* max. in us: 3.45 (SM), 0.9 (FM), 0.45 (FPM); no min */ + _i2c_delay(bus); + + /* SCL = passive HIGH (floating and pulled-up), SDA value is available */ + _i2c_scl_high(bus); + + /* wait t_HIGH - time for the slave to read SDA */ + /* min. in us: 4 (SM), 0.6 (FM), 0.26 (FPM), 0.09 (HSM); no max. */ + _i2c_delay(bus); + + /* clock stretching, wait as long as clock is driven low by the slave */ + uint32_t stretch = I2C_CLOCK_STRETCH; + while (stretch && !_i2c_scl_read(bus)) { + stretch--; + } + if (stretch == 0) { + //DEBUG("%s: clock stretching timeout dev=%u\n", __func__, bus->dev); + res = -ETIMEDOUT; + } + + /* if SCL is high, now data is valid */ + /* if SDA is high, check that nobody else is driving SDA low */ + if (bit && !_i2c_sda_read(bus)) { + return _i2c_arbitration_lost(bus, __func__); + } + + /* SCL = active LOW to allow next SDA change */ + _i2c_scl_low(bus); + + return res; +} + +static IRAM_ATTR int _i2c_read_bit(_i2c_bus_t* bus, bool* bit) +{ + /* read one bit + * on entry: SCL is active low, SDA can be changed + * on exit : SCL is active low, SDA can be changed + */ + + int res = 0; + + /* SDA = passive HIGH (floating and pulled-up) to let the slave drive data */ + _i2c_sda_high(bus); + + /* wait t_VD;DAT - data valid time (time until data are valid) */ + /* max. in us: 3.45 (SM), 0.9 (FM), 0.45 (FPM); no min */ + _i2c_delay(bus); + + /* SCL = passive HIGH (floating and pulled-up), SDA value is available */ + _i2c_scl_high(bus); + + /* clock stretching, wait as long as clock is driven to low by the slave */ + uint32_t stretch = I2C_CLOCK_STRETCH; + while (stretch && !_i2c_scl_read(bus)) { + stretch--; + } + if (stretch == 0) { + //DEBUG("%s: clock stretching timeout dev=%u\n", __func__, bus->dev); + res = -ETIMEDOUT; + } + + /* wait t_HIGH - time for the slave to read SDA */ + /* min. in us: 4 (SM), 0.6 (FM), 0.26 (FPM), 0.09 (HSM); no max. */ + _i2c_delay(bus); + + /* SCL is high, read out bit */ + *bit = _i2c_sda_read(bus); + + /* SCL = active LOW to allow next SDA change */ + _i2c_scl_low(bus); + + return res; +} + +static IRAM_ATTR int _i2c_write_byte(_i2c_bus_t* bus, uint8_t byte) +{ + /* send one byte and returns 0 in case of ACK from slave */ + + /* send the byte from MSB to LSB */ + for (unsigned i = 0; i < 8; i++) { + int res = _i2c_write_bit(bus, (byte & 0x80) != 0); + if (res != 0) { + return res; + } + byte = byte << 1; + } + + /* read acknowledge bit (low) from slave */ + bool bit; + int res = _i2c_read_bit(bus, &bit); + if (res != 0) { + return res; + } + + return !bit ? 0 : -EIO; +} + + +static IRAM_ATTR int _i2c_read_byte(_i2c_bus_t* bus, uint8_t *byte, bool ack) +{ + bool bit; + + /* read the byte */ + for (unsigned i = 0; i < 8; i++) { + int res = _i2c_read_bit(bus, &bit); + if (res != 0) { + return res; + } + *byte = (*byte << 1) | (bit ? 1 : 0); + } + + /* write acknowledgement flag */ + _i2c_write_bit(bus, !ack); + + return 0; +} + +int i2c_read_regs(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + void *data, size_t len, uint8_t flags) +{ + uint16_t reg_end = reg; + + if (flags & (I2C_NOSTOP | I2C_NOSTART)) { + return -EOPNOTSUPP; + } + + /* Handle endianness of register if 16 bit */ + if (flags & I2C_REG16) { + reg_end = htons(reg); /* Make sure register is in big-endian on I2C bus */ + } + + /* First set ADDR and register with no stop */ + int ret = i2c_write_bytes(bus, addr, ®_end, (flags & I2C_REG16) ? 2 : 1, + flags | I2C_NOSTOP); + if (ret < 0) { + return ret; + } + /* Then get the data from device */ + return i2c_read_bytes(bus, addr, data, len, flags); +} + +int i2c_read_reg(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + void *data, uint8_t flags) +{ + return i2c_read_regs(bus, addr, reg, data, 1, flags); +} + + +int i2c_read_byte(_i2c_bus_t* bus, uint16_t addr, void *data, uint8_t flags) +{ + return i2c_read_bytes(bus, addr, data, 1, flags); +} + +int i2c_write_byte(_i2c_bus_t* bus, uint16_t addr, uint8_t data, uint8_t flags) +{ + return i2c_write_bytes(bus, addr, &data, 1, flags); +} + +int i2c_write_regs(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + const void *data, size_t len, uint8_t flags) +{ + uint16_t reg_end = reg; + + if (flags & (I2C_NOSTOP | I2C_NOSTART)) { + return -EOPNOTSUPP; + } + + /* Handle endianness of register if 16 bit */ + if (flags & I2C_REG16) { + reg_end = htons(reg); /* Make sure register is in big-endian on I2C bus */ + } + + /* First set ADDR and register with no stop */ + int ret = i2c_write_bytes(bus, addr, ®_end, (flags & I2C_REG16) ? 2 : 1, + flags | I2C_NOSTOP); + if (ret < 0) { + return ret; + } + /* Then write data to the device */ + return i2c_write_bytes(bus, addr, data, len, flags | I2C_NOSTART); +} + +int i2c_write_reg(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + uint8_t data, uint8_t flags) +{ + return i2c_write_regs(bus, addr, reg, &data, 1, flags); +} diff --git a/libraries/AP_HAL_ESP32/i2c_sw.h b/libraries/AP_HAL_ESP32/i2c_sw.h new file mode 100644 index 0000000000..d1544480da --- /dev/null +++ b/libraries/AP_HAL_ESP32/i2c_sw.h @@ -0,0 +1,430 @@ +/* + * Copyright (C) 2014-2017 Freie Universität Berlin + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @defgroup drivers_periph_i2c I2C + * @ingroup drivers_periph + * @brief Low-level I2C peripheral driver + * + * This interface provides a simple abstraction to use the MCUs I2C peripherals. + * It provides support for 7-bit and 10-bit addressing and can be used for + * different kind of register addressing schemes. + * + * + * @section sec_i2c_usage Usage + * + * Example for reading a 8-bit register on a device, using a 10-bit device + * address and 8-bit register addresses and using a RESTART condition (CAUTION: + * this example does not check any return values...): + * + * @code{c} + * // initialize the bus (this is normally done during boot time) + * i2c_init(dev); + * ... + * // before accessing the bus, we need to acquire it + * i2c_acquire(dev); + * // next we write the register address, but create no STOP condition when done + * i2c_write_byte(dev, device_addr, reg_addr, (I2C_NOSTOP | I2C_ADDR10)); + * // and now we read the register value + * i2c_read_byte(dev, device_addr, ®_value, I2C_ADDR10); + * // finally we have to release the bus + * i2c_release(dev); + * @endcode + * + * Example for writing a 16-bit register with 16-bit register addressing and + * 7-bit device addressing: + * + * @code{c} + * // initialize the bus + * i2c_init(dev); + * ... + * // first, acquire the shared bus again + * i2c_acquire(dev); + * // write the 16-bit register address to the device and prevent STOP condition + * i2c_write_byte(dev, device_addr, reg_addr, I2C_NOSTOP); + * // and write the data after a REPEATED START + * i2c_write_bytes(dev, device_addr, reg_data, 2, 0); + * // and finally free the bus again + * i2c_release(dev); + * @endcode + * + * + * @section sec_i2c_pull Pull Resistors + * + * The I2C signal lines SDA/SCL need external pull-up resistors which connect + * the lines to the positive voltage supply Vcc. The I2C driver implementation + * should enable the pin's internal pull-up resistors. There are however some + * use cases for which the internal pull resistors are not strong enough and the + * I2C bus will show faulty behavior. This can for example happen when + * connecting a logic analyzer which will raise the capacitance of the bus. In + * this case you should make sure you connect external pull-up resistors to both + * I2C bus lines. + * + * The minimum and maximum resistances are computed by: + * \f{eqnarray*}{ + * R_{min} &=& \frac{V_{DD} - V_{OL(max)}} {I_{OL}}\\ + * R_{max} &=& \frac{t_r} {(0.8473 \cdot C_b)} + * \f}
+ * where:
+ * \f$ V_{DD} =\f$ Supply voltage, + * \f$ V_{OL(max)} =\f$ Low level voltage, + * \f$ I_{OL} =\f$ Low level output current, + * \f$ t_r =\f$ Signal rise time, + * \f$ C_b =\f$ Bus capacitance
+ *
The pull-up resistors depend on the bus speed. + * Some typical values are:
+ * Normal mode: 10kΩ
+ * Fast mode: 2kΩ
+ * Fast plus mode: 2kΩ + * + * For more details refer to section 7.1 in:
+ * http://www.nxp.com/documents/user_manual/UM10204.pdf + * + * + * @section sec_i2c_pm (Low-) power implications + * + * The I2C interface realizes a transaction-based access scheme to the bus. From + * a power management perspective, we can leverage this by only powering on the + * I2C peripheral while it is actually used, that is inside an i2c_acquire() - + * i2c_release() block. + * + * After initialization, the I2C peripheral **should** be powered off (e.g. + * through peripheral clock gating). It should only be powered on once a + * transaction on the I2C bus starts, namely in the i2c_acquire() function. Once + * the transaction is finished, the corresponding I2C peripheral **should** be + * powered off again in the i2c_release() function. + * + * If the implementation puts the active thread to sleep while a transfer is in + * progress (e.g. when using DMA), the implementation might need to block + * certain power states. + * + * @{ + * @file + * @brief Low-level I2C peripheral driver interface definition + * + * @author Hauke Petersen + * @author Thomas Eichinger + */ + +#ifndef PERIPH_I2C_H +#define PERIPH_I2C_H + +#include +#include +#include + +#include "driver/gpio.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/** + * @brief Default I2C device access macro + * @{ + */ +#ifndef I2C_DEV +#define I2C_DEV(x) (x) +#endif +/** @} */ + +/** + * @brief Default I2C undefined value + * @{ + */ +#ifndef I2C_UNDEF +#define I2C_UNDEF (UINT_MAX) +#endif +/** @} */ + +/** + * @brief Default i2c_t type definition + * @{ + */ +#ifndef HAVE_I2C_T +typedef unsigned int i2c_t; +#endif +/** @} */ + +/** + * @brief Read bit needs to be set when reading + */ +#define I2C_READ (0x0001) + +/** + * @brief Special bit pattern indicating a 10 bit address is used + * + * Should only be used internally in CPU driver implementations, this is not + * intended to be used by applications. + * + * @see https://www.i2c-bus.org/addressing/10-bit-addressing/ + */ +#define I2C_10BIT_MAGIC (0xF0u) + +/** + * @brief Default mapping of I2C bus speed values + * @{ + */ +typedef enum { + I2C_SPEED_LOW = 0, /**< low speed mode: ~10 kbit/s */ + I2C_SPEED_NORMAL, /**< normal mode: ~100 kbit/s */ + I2C_SPEED_FAST, /**< fast mode: ~400 kbit/s */ + I2C_SPEED_FAST_PLUS, /**< fast plus mode: ~1000 kbit/s */ + I2C_SPEED_HIGH, /**< high speed mode: ~3400 kbit/s */ +} i2c_speed_t; +/** @} */ + +/** + * @brief I2C transfer flags + * @{ + */ +typedef enum { + I2C_ADDR10 = 0x01, /**< use 10-bit device addressing */ + I2C_REG16 = 0x02, /**< use 16-bit register addressing, big-endian */ + I2C_NOSTOP = 0x04, /**< do not issue a STOP condition after transfer */ + I2C_NOSTART = 0x08, /**< skip START sequence, ignores address field */ +} i2c_flags_t; +/** @} */ + +typedef struct { + i2c_speed_t speed; + + bool started; + + gpio_num_t scl; + gpio_num_t sda; + + uint32_t scl_bit; /* gpio bit mask for faster access */ + uint32_t sda_bit; /* gpio bit mask for faster access */ + + uint32_t delay; +} _i2c_bus_t; + +/** + * @brief Initialize the given I2C bus + * + * The given I2C device will be initialized with the parameters as specified in + * the boards periph_conf.h, using the pins and the speed value given there. + * + * The bus MUST not be acquired before initializing it, as this is handled + * internally by the i2c_init function! + * + * @param[in] dev the device to initialize + */ +void i2c_init(_i2c_bus_t* bus); + +/** + * @brief Convenience function for reading one byte from a given register + * address + * + * @note This function is using a repeated start sequence for reading from + * the specified register address. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] reg register address to read from (8- or 16-bit, + * right-aligned) + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[out] data memory location to store received data + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ + +int i2c_read_reg(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + void *data, uint8_t flags); + +/** + * @brief Convenience function for reading several bytes from a given + * register address + * + * @note This function is using a repeated start sequence for reading from + * the specified register address. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] reg register address to read from (8- or 16-bit, + * right-aligned) + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[out] data memory location to store received data + * @param[in] len the number of bytes to read into @p data + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ +int i2c_read_regs(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + void *data, size_t len, uint8_t flags); + +/** + * @brief Convenience function for reading one byte from a device + * + * @note This function is using a repeated start sequence for reading from + * the specified register address. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[out] data memory location to store received data + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ + +int i2c_read_byte(_i2c_bus_t* bus, uint16_t addr, void *data, uint8_t flags); + +/** + * @brief Convenience function for reading bytes from a device + * + * @note This function is using a repeated start sequence for reading from + * the specified register address. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[out] data memory location to store received data + * @param[in] len the number of bytes to read into @p data + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ + +int i2c_read_bytes(_i2c_bus_t* bus, uint16_t addr, + void *data, size_t len, uint8_t flags); + +/** + * @brief Convenience function for writing a single byte onto the bus + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[in] data byte to write to the device + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ +int i2c_write_byte(_i2c_bus_t* bus, uint16_t addr, uint8_t data, uint8_t flags); + +/** + * @brief Convenience function for writing several bytes onto the bus + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[in] data array holding the bytes to write to the device + * @param[in] len the number of bytes to write + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ +int i2c_write_bytes(_i2c_bus_t* bus, uint16_t addr, const void *data, + size_t len, uint8_t flags); + +/** + * @brief Convenience function for writing one byte to a given + * register address + * + * @note This function is using a continuous sequence for writing to the + * specified register address. It first writes the register then data. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] reg register address to read from (8- or 16-bit, + * right-aligned) + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[in] data byte to write + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ +int i2c_write_reg(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + uint8_t data, uint8_t flags); + +/** + * @brief Convenience function for writing data to a given register address + * + * @note This function is using a continuous sequence for writing to the + * specified register address. It first writes the register then data. + * + * @pre i2c_acquire must be called before accessing the bus + * + * @param[in] dev I2C peripheral device + * @param[in] reg register address to read from (8- or 16-bit, + * right-aligned) + * @param[in] addr 7-bit or 10-bit device address (right-aligned) + * @param[out] data memory location to store received data + * @param[in] len the number of bytes to write + * @param[in] flags optional flags (see @ref i2c_flags_t) + * + * @return 0 When success + * @return -EIO When slave device doesn't ACK the byte + * @return -ENXIO When no devices respond on the address sent on the bus + * @return -ETIMEDOUT When timeout occurs before device's response + * @return -EINVAL When an invalid argument is given + * @return -EOPNOTSUPP When MCU driver doesn't support the flag operation + * @return -EAGAIN When a lost bus arbitration occurs + */ +int i2c_write_regs(_i2c_bus_t* bus, uint16_t addr, uint16_t reg, + const void *data, size_t len, uint8_t flags); + + +#ifdef __cplusplus +} +#endif + +#endif /* PERIPH_I2C_H */ +/** @} */ diff --git a/libraries/AP_HAL_ESP32/profile/LinkerScriptGenerator.kt b/libraries/AP_HAL_ESP32/profile/LinkerScriptGenerator.kt new file mode 100644 index 0000000000..44ebbbd642 --- /dev/null +++ b/libraries/AP_HAL_ESP32/profile/LinkerScriptGenerator.kt @@ -0,0 +1,94 @@ +import java.io.File +import java.io.FileOutputStream +import java.nio.charset.Charset +import java.util.* +import java.util.regex.Pattern +import kotlin.collections.ArrayList + +data class Func( + var symbol: String, + var place: String, + var address: Int, + var size: Int, + var count: Int +) + +val p = Pattern.compile(" (?:\\.iram1|\\.text)+\\.(\\S*)\\s*(0x.{16})\\s*(0x\\S*)\\s*(\\S*)") +val placep = Pattern.compile("[^(]*\\(([^.]*)") + +fun generateLinkerScript(mapFileName: String, profileFileName: String, scriptFileName: String) { + var addressToFunction = TreeMap() + fun parseMap() { + val s = File(mapFileName).readText(Charset.defaultCharset()) + val m = p.matcher(s) + while (m.find()) { + val symbol = m.group(1) + val address = Integer.decode(m.group(2)) + val size = Integer.decode(m.group(3)) + var place = m.group(4) + if (address == 0) { + continue + } + var placem = placep.matcher(place) + if (placem.find()) { + place = placem.group(1) + } + var f = Func(symbol, place, address, size, 0) + addressToFunction[f.address] = f + } + } + + fun parseProfile() { + Scanner(File(profileFileName)).use { scanner -> + while (scanner.hasNext()) { + val ad = scanner.next() + System.out.println(ad) + val address = Integer.decode(ad) + val count = Integer.decode(scanner.next()) + for(f in addressToFunction.values) { + if(f.address <= address && address < f.address + f.size) { + f.count = count + } + } + } + } + } + + fun writeScript() { + var excl = listOf( "_ZN12NavEKF2_core15readRangeFinderEv", + "_ZN12NavEKF2_core18SelectRngBcnFusionEv","_ZN12NavEKF2_core14readRngBcnDataEv") + + var lst = ArrayList(addressToFunction.values.filter { it.count > 5000 && !excl.contains(it.symbol) }) + lst.sortWith(kotlin.Comparator { o1, o2 -> o2.count - o1.count }) + + var s = 0 + for (a in lst) { + System.out.format( + "0x%016x\t%8d\t%8d\t%s:%s\n", + a.address, + a.size, + a.count, + a.place, + a.symbol + ) + s += a.size + } + FileOutputStream(scriptFileName).use { + for (a in lst) { + it.write(String.format("%s:%s\n", a.place, a.symbol).toByteArray()) + } + } + System.out.format("total: %d\n", s) + } + parseMap() + parseProfile() + writeScript() +} + +fun main(args: Array) { + generateLinkerScript( + "ardusub.map", + "PROF000.TXT", + "functions.list" + ) +} diff --git a/libraries/AP_HAL_ESP32/profile/PROF000.TXT b/libraries/AP_HAL_ESP32/profile/PROF000.TXT new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ESP32/profile/README.md b/libraries/AP_HAL_ESP32/profile/README.md new file mode 100644 index 0000000000..f309415969 --- /dev/null +++ b/libraries/AP_HAL_ESP32/profile/README.md @@ -0,0 +1,39 @@ +# How to profile ardupilot binary on ESP32 and use result to place critical functions to the IRAM +## Prepare source +1. Configure project with `--enable-profile` option: `./waf configure --board=esp32diy --enable-profile` +2. Rebuild binaries +## Run program with profiling enabled +1. Flash and run binary +2. Try to use mode silimiar to real flight. I.E. compass/gps connected, armed state, and so no +3. It will write profile stat to the `/APM/APM/PROF000.TXT` files on the sdcard every one minute +## Use profile info to optimize program +1. Copy `PROF*.TXT` file from sd card +2. Copy file `arducopter.map` from build directory +2. Use them to produce `functions.list` by the script `LinkerScriptGenerator.kt` (modify paths and limits inside) +3. Place file `functions.list` to the `target/copter/main` folder + + +# how to build LinkerScriptGenerator.kt + +$ sudo snap install --classic kotlin + +# build +kotlinc LinkerScriptGenerator.kt -d generator.jar + +# collect up profiling file into this folder... as per above. + +# files that the kotlin tool expects to find in this folder: + "ardusub.map", + "PROF000.TXT", + +# file/s it will create in this folder: + "functions.list" + + +cp ../../../build/esp32buzz/idf-plane/arduplane.map ardusub.map +touch PROF000.TXT + +#run: +java -jar generator.jar + + diff --git a/libraries/AP_HAL_ESP32/profile/ardusub.map b/libraries/AP_HAL_ESP32/profile/ardusub.map new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ESP32/system.cpp b/libraries/AP_HAL_ESP32/system.cpp new file mode 100644 index 0000000000..20f07a0147 --- /dev/null +++ b/libraries/AP_HAL_ESP32/system.cpp @@ -0,0 +1,63 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include +#include +#include "SdCard.h" + +#include +#include "esp_timer.h" + +namespace AP_HAL +{ + +void panic(const char *errormsg, ...) +{ + va_list ap; + + va_start(ap, errormsg); + vprintf(errormsg, ap); + va_end(ap); + + while (1) {} +} + +uint32_t micros() +{ + return micros64(); +} + +uint32_t millis() +{ + return millis64(); +} + +uint64_t micros64() +{ + return esp_timer_get_time(); +} + +uint64_t millis64() +{ + return micros64()/1000; +} + +} // namespace AP_HAL + +const AP_HAL::HAL& AP_HAL::get_HAL() +{ + static const HAL_ESP32 hal; + return hal; +} diff --git a/libraries/AP_HAL_ESP32/targets/.gitignore b/libraries/AP_HAL_ESP32/targets/.gitignore new file mode 100644 index 0000000000..ec58be2346 --- /dev/null +++ b/libraries/AP_HAL_ESP32/targets/.gitignore @@ -0,0 +1,2 @@ +/sdkconfig.old +/board.txt diff --git a/libraries/AP_HAL_ESP32/targets/esp-idf/CMakeLists.txt b/libraries/AP_HAL_ESP32/targets/esp-idf/CMakeLists.txt new file mode 100644 index 0000000000..07f1a4df7e --- /dev/null +++ b/libraries/AP_HAL_ESP32/targets/esp-idf/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_TOOLCHAIN_FILE $ENV{IDF_PATH}/tools/cmake/toolchain-esp32.cmake CACHE STRING "") + +project(ardupilot) + +# Include for ESP-IDF build system functions +include($ENV{IDF_PATH}/tools/cmake/idf.cmake) + +# Create idf::esp32 and idf::freertos static libraries +idf_build_process(esp32 + # try and trim the build; additional components + # will be included as needed based on dependency tree + # + # although esptool_py does not generate static library, + # processing the component is needed for flashing related + # targets and file generation + COMPONENTS esp32 + freertos + tcpip_adapter + fatfs + esp_adc_cal + nvs_flash + esptool_py + app_update + SDKCONFIG ${CMAKE_CURRENT_LIST_DIR}/sdkconfig + BUILD_DIR ${CMAKE_BINARY_DIR}) + +set(elf_file ardupilot.elf) + +add_executable(${elf_file} main.c) + +if(NOT DEFINED ARDUPILOT_CMD) + set(ARDUPILOT_CMD "none") +endif() + +IF(${ARDUPILOT_CMD} STREQUAL "plane") + message("Building for plane") + target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarduplane.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduPlane_libs.a") +ENDIF() + +IF(${ARDUPILOT_CMD} STREQUAL "copter") + message("Building for copter") + target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarducopter.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduCopter_libs.a") +ENDIF() + +IF(${ARDUPILOT_CMD} STREQUAL "rover") + message("Building for rover") + target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardurover.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libRover_libs.a") +ENDIF() + +IF(${ARDUPILOT_CMD} STREQUAL "sub") + message("Building for submarine") + target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardusub.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduSub_libs.a") +ENDIF() + +add_custom_target(showinc ALL + COMMAND echo -e + "$" + > includes.list + VERBATIM + BYPRODUCTS includes.list + COMMAND_EXPAND_LISTS +) + +#Find files name lib to link +#function(SUBLINK target curdir) +# FILE(GLOB children RELATIVE ${curdir} ${curdir}/*) +# FOREACH(child ${children}) +# IF(NOT IS_DIRECTORY ${curdir}/${child}) +# SET(PATH "${curdir}/${child}") +# message("Linking ${PATH}") +# target_link_libraries(${target} "${PATH}") +# ENDIF() +# ENDFOREACH() +#endfunction() + +#IF (DEFINED ARDUPILOT_BIN) +# SUBLINK(${elf_file} ${ARDUPILOT_BIN}) +#ENDIF() +#IF (DEFINED ARDUPILOT_LIB) +# SUBLINK(${elf_file} ${ARDUPILOT_LIB}) +#ENDIF() + +# Link the static libraries to the executable +target_link_libraries(${elf_file} + idf::esp32 + idf::freertos + idf::fatfs + idf::esp_adc_cal + idf::nvs_flash + idf::spi_flash + idf::tcpip_adapter + idf::app_update + ) + +# Attach additional targets to the executable file for flashing, +# linker script generation, partition_table generation, etc. +idf_build_executable(${elf_file}) + +set(CMAKE_EXPORT_COMPILE_COMMANDS 1) + diff --git a/libraries/AP_HAL_ESP32/targets/esp-idf/main.c b/libraries/AP_HAL_ESP32/targets/esp-idf/main.c new file mode 100644 index 0000000000..138dd8f31a --- /dev/null +++ b/libraries/AP_HAL_ESP32/targets/esp-idf/main.c @@ -0,0 +1,14 @@ +/* Hello World Example + This example code is in the Public Domain (or CC0 licensed, at your option.) + Unless required by applicable law or agreed to in writing, this + software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR + CONDITIONS OF ANY KIND, either express or implied. +*/ +#include + +int main(int argc, char *argv[]); + +void app_main() +{ + main(0, NULL); +} diff --git a/libraries/AP_HAL_ESP32/targets/esp-idf/sdkconfig b/libraries/AP_HAL_ESP32/targets/esp-idf/sdkconfig new file mode 100644 index 0000000000..ec81e2d7ea --- /dev/null +++ b/libraries/AP_HAL_ESP32/targets/esp-idf/sdkconfig @@ -0,0 +1,970 @@ +# +# Automatically generated file. DO NOT EDIT. +# Espressif IoT Development Framework (ESP-IDF) Project Configuration +# +CONFIG_IDF_CMAKE=y +CONFIG_IDF_TARGET="esp32" +CONFIG_IDF_TARGET_ESP32=y +CONFIG_IDF_FIRMWARE_CHIP_ID=0x0000 + +# +# SDK tool configuration +# +CONFIG_SDK_TOOLPREFIX="xtensa-esp32-elf-" +# CONFIG_SDK_TOOLCHAIN_SUPPORTS_TIME_WIDE_64_BITS is not set +# end of SDK tool configuration + +# +# Build type +# +CONFIG_APP_BUILD_TYPE_APP_2NDBOOT=y +# CONFIG_APP_BUILD_TYPE_ELF_RAM is not set +CONFIG_APP_BUILD_GENERATE_BINARIES=y +CONFIG_APP_BUILD_BOOTLOADER=y +CONFIG_APP_BUILD_USE_FLASH_SECTIONS=y +# end of Build type + +# +# Application manager +# +# CONFIG_APP_COMPILE_TIME_DATE is not set +CONFIG_APP_EXCLUDE_PROJECT_VER_VAR=y +CONFIG_APP_EXCLUDE_PROJECT_NAME_VAR=y +# CONFIG_APP_PROJECT_VER_FROM_CONFIG is not set +CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16 +# end of Application manager + +# +# Bootloader config +# +# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_SIZE is not set +# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_DEBUG is not set +CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_PERF=y +# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_NONE is not set +# CONFIG_BOOTLOADER_LOG_LEVEL_NONE is not set +# CONFIG_BOOTLOADER_LOG_LEVEL_ERROR is not set +# CONFIG_BOOTLOADER_LOG_LEVEL_WARN is not set +CONFIG_BOOTLOADER_LOG_LEVEL_INFO=y +# CONFIG_BOOTLOADER_LOG_LEVEL_DEBUG is not set +# CONFIG_BOOTLOADER_LOG_LEVEL_VERBOSE is not set +CONFIG_BOOTLOADER_LOG_LEVEL=3 +# CONFIG_BOOTLOADER_SPI_CUSTOM_WP_PIN is not set +CONFIG_BOOTLOADER_SPI_WP_PIN=7 +CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y +# CONFIG_BOOTLOADER_FACTORY_RESET is not set +# CONFIG_BOOTLOADER_APP_TEST is not set +CONFIG_BOOTLOADER_WDT_ENABLE=y +# CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE is not set +CONFIG_BOOTLOADER_WDT_TIME_MS=9000 +# CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE is not set +# CONFIG_BOOTLOADER_SKIP_VALIDATE_IN_DEEP_SLEEP is not set +CONFIG_BOOTLOADER_RESERVE_RTC_SIZE=0 +# CONFIG_BOOTLOADER_CUSTOM_RESERVE_RTC is not set +# end of Bootloader config + +# +# Security features +# +# CONFIG_SECURE_SIGNED_APPS_NO_SECURE_BOOT is not set +# CONFIG_SECURE_BOOT is not set +# CONFIG_SECURE_FLASH_ENC_ENABLED is not set +# end of Security features + +# +# Serial flasher config +# +CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200 +CONFIG_ESPTOOLPY_WITH_STUB=y +CONFIG_ESPTOOLPY_FLASHMODE_QIO=y +# CONFIG_ESPTOOLPY_FLASHMODE_QOUT is not set +# CONFIG_ESPTOOLPY_FLASHMODE_DIO is not set +# CONFIG_ESPTOOLPY_FLASHMODE_DOUT is not set +CONFIG_ESPTOOLPY_FLASHMODE="dio" +CONFIG_ESPTOOLPY_FLASHFREQ_80M=y +# CONFIG_ESPTOOLPY_FLASHFREQ_40M is not set +# CONFIG_ESPTOOLPY_FLASHFREQ_26M is not set +# CONFIG_ESPTOOLPY_FLASHFREQ_20M is not set +CONFIG_ESPTOOLPY_FLASHFREQ="80m" +# CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set +# CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set +CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +# CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set +# CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set +CONFIG_ESPTOOLPY_FLASHSIZE="4MB" +CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y +CONFIG_ESPTOOLPY_BEFORE_RESET=y +# CONFIG_ESPTOOLPY_BEFORE_NORESET is not set +CONFIG_ESPTOOLPY_BEFORE="default_reset" +CONFIG_ESPTOOLPY_AFTER_RESET=y +# CONFIG_ESPTOOLPY_AFTER_NORESET is not set +CONFIG_ESPTOOLPY_AFTER="hard_reset" +# CONFIG_ESPTOOLPY_MONITOR_BAUD_9600B is not set +# CONFIG_ESPTOOLPY_MONITOR_BAUD_57600B is not set +CONFIG_ESPTOOLPY_MONITOR_BAUD_115200B=y +# CONFIG_ESPTOOLPY_MONITOR_BAUD_230400B is not set +# CONFIG_ESPTOOLPY_MONITOR_BAUD_921600B is not set +# CONFIG_ESPTOOLPY_MONITOR_BAUD_2MB is not set +# CONFIG_ESPTOOLPY_MONITOR_BAUD_OTHER is not set +CONFIG_ESPTOOLPY_MONITOR_BAUD_OTHER_VAL=115200 +CONFIG_ESPTOOLPY_MONITOR_BAUD=115200 +# end of Serial flasher config + +# +# Partition Table +# +# CONFIG_PARTITION_TABLE_SINGLE_APP is not set +# CONFIG_PARTITION_TABLE_TWO_OTA is not set +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="../partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="../partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y +# end of Partition Table + +# +# Compiler options +# +# CONFIG_COMPILER_OPTIMIZATION_DEFAULT is not set +# CONFIG_COMPILER_OPTIMIZATION_SIZE is not set +CONFIG_COMPILER_OPTIMIZATION_PERF=y +# CONFIG_COMPILER_OPTIMIZATION_NONE is not set +CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE=y +# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_SILENT is not set +# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_DISABLE is not set +# CONFIG_COMPILER_CXX_EXCEPTIONS is not set +# CONFIG_COMPILER_CXX_RTTI is not set +CONFIG_COMPILER_STACK_CHECK_MODE_NONE=y +# CONFIG_COMPILER_STACK_CHECK_MODE_NORM is not set +# CONFIG_COMPILER_STACK_CHECK_MODE_STRONG is not set +# CONFIG_COMPILER_STACK_CHECK_MODE_ALL is not set +# CONFIG_COMPILER_WARN_WRITE_STRINGS is not set +# CONFIG_COMPILER_DISABLE_GCC8_WARNINGS is not set +# end of Compiler options + +# +# Component config +# + +# +# Application Level Tracing +# +# CONFIG_APPTRACE_DEST_TRAX is not set +CONFIG_APPTRACE_DEST_NONE=y +CONFIG_APPTRACE_LOCK_ENABLE=y +# end of Application Level Tracing + +# +# Driver configurations +# + +# +# ADC configuration +# +# CONFIG_ADC_FORCE_XPD_FSM is not set +CONFIG_ADC_DISABLE_DAC=y +# end of ADC configuration + +# +# SPI configuration +# +CONFIG_SPI_MASTER_IN_IRAM=y +CONFIG_SPI_MASTER_ISR_IN_IRAM=y +# CONFIG_SPI_SLAVE_IN_IRAM is not set +CONFIG_SPI_SLAVE_ISR_IN_IRAM=y +# end of SPI configuration + +# +# TWAI configuration +# +# CONFIG_TWAI_ISR_IN_IRAM is not set +# end of TWAI configuration + +# +# UART configuration +# +# CONFIG_UART_ISR_IN_IRAM is not set +# end of UART configuration + +# +# RTCIO configuration +# +# CONFIG_RTCIO_SUPPORT_RTC_GPIO_DESC is not set +# end of RTCIO configuration +# end of Driver configurations + +# +# eFuse Bit Manager +# +# CONFIG_EFUSE_CUSTOM_TABLE is not set +# CONFIG_EFUSE_VIRTUAL is not set +# CONFIG_EFUSE_CODE_SCHEME_COMPAT_NONE is not set +CONFIG_EFUSE_CODE_SCHEME_COMPAT_3_4=y +# CONFIG_EFUSE_CODE_SCHEME_COMPAT_REPEAT is not set +CONFIG_EFUSE_MAX_BLK_LEN=192 +# end of eFuse Bit Manager + +# +# ESP32-specific +# +CONFIG_ESP32_REV_MIN_0=y +# CONFIG_ESP32_REV_MIN_1 is not set +# CONFIG_ESP32_REV_MIN_2 is not set +# CONFIG_ESP32_REV_MIN_3 is not set +CONFIG_ESP32_REV_MIN=0 +CONFIG_ESP32_DPORT_WORKAROUND=y +# CONFIG_ESP32_DEFAULT_CPU_FREQ_80 is not set +# CONFIG_ESP32_DEFAULT_CPU_FREQ_160 is not set +CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y +CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240 +# CONFIG_ESP32_SPIRAM_SUPPORT is not set +# CONFIG_ESP32_TRAX is not set +CONFIG_ESP32_TRACEMEM_RESERVE_DRAM=0x0 +CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_TWO=y +# CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_FOUR is not set +CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES=2 +# CONFIG_ESP32_ULP_COPROC_ENABLED is not set +CONFIG_ESP32_ULP_COPROC_RESERVE_MEM=0 +CONFIG_ESP32_DEBUG_OCDAWARE=y +CONFIG_ESP32_BROWNOUT_DET=y +CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_0=y +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_1 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_2 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_3 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_4 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_5 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_6 is not set +# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_7 is not set +CONFIG_ESP32_BROWNOUT_DET_LVL=0 +CONFIG_ESP32_REDUCE_PHY_TX_POWER=y +CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y +# CONFIG_ESP32_TIME_SYSCALL_USE_RTC is not set +# CONFIG_ESP32_TIME_SYSCALL_USE_FRC1 is not set +# CONFIG_ESP32_TIME_SYSCALL_USE_NONE is not set +CONFIG_ESP32_RTC_CLK_SRC_INT_RC=y +# CONFIG_ESP32_RTC_CLK_SRC_EXT_CRYS is not set +# CONFIG_ESP32_RTC_CLK_SRC_EXT_OSC is not set +# CONFIG_ESP32_RTC_CLK_SRC_INT_8MD256 is not set +CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024 +CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000 +CONFIG_ESP32_XTAL_FREQ_40=y +# CONFIG_ESP32_XTAL_FREQ_26 is not set +# CONFIG_ESP32_XTAL_FREQ_AUTO is not set +CONFIG_ESP32_XTAL_FREQ=40 +# CONFIG_ESP32_DISABLE_BASIC_ROM_CONSOLE is not set +# CONFIG_ESP32_NO_BLOBS is not set +# CONFIG_ESP32_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set +# CONFIG_ESP32_COMPATIBLE_PRE_V3_1_BOOTLOADERS is not set +# CONFIG_ESP32_USE_FIXED_STATIC_RAM_SIZE is not set +CONFIG_ESP32_DPORT_DIS_INTERRUPT_LVL=5 +# end of ESP32-specific + +# +# Power Management +# +CONFIG_PM_ENABLE=y +# CONFIG_PM_DFS_INIT_AUTO is not set +# CONFIG_PM_PROFILING is not set +# CONFIG_PM_TRACE is not set +# end of Power Management + +# +# ADC-Calibration +# +CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y +CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y +CONFIG_ADC_CAL_LUT_ENABLE=y +# end of ADC-Calibration + +# +# Common ESP-related +# +CONFIG_ESP_ERR_TO_NAME_LOOKUP=y +CONFIG_ESP_SYSTEM_EVENT_QUEUE_SIZE=32 +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=2304 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=3584 +CONFIG_ESP_IPC_TASK_STACK_SIZE=1024 +CONFIG_ESP_IPC_USES_CALLERS_PRIORITY=y +CONFIG_ESP_MINIMAL_SHARED_STACK_SIZE=2048 +CONFIG_ESP_CONSOLE_UART_DEFAULT=y +# CONFIG_ESP_CONSOLE_UART_CUSTOM is not set +# CONFIG_ESP_CONSOLE_UART_NONE is not set +CONFIG_ESP_CONSOLE_UART_NUM=0 +CONFIG_ESP_CONSOLE_UART_TX_GPIO=1 +CONFIG_ESP_CONSOLE_UART_RX_GPIO=3 +CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200 +CONFIG_ESP_INT_WDT=y +CONFIG_ESP_INT_WDT_TIMEOUT_MS=1000 +CONFIG_ESP_INT_WDT_CHECK_CPU1=y +CONFIG_ESP_TASK_WDT=y +# CONFIG_ESP_TASK_WDT_PANIC is not set +CONFIG_ESP_TASK_WDT_TIMEOUT_S=5 +CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0=y +CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU1=y +# CONFIG_ESP_PANIC_HANDLER_IRAM is not set +CONFIG_ESP_MAC_ADDR_UNIVERSE_WIFI_STA=y +CONFIG_ESP_MAC_ADDR_UNIVERSE_BT=y +CONFIG_ESP_MAC_ADDR_UNIVERSE_BT_OFFSET=1 +# end of Common ESP-related + +# +# Ethernet +# +CONFIG_ETH_ENABLED=y +CONFIG_ETH_USE_ESP32_EMAC=y +CONFIG_ETH_PHY_INTERFACE_RMII=y +# CONFIG_ETH_PHY_INTERFACE_MII is not set +CONFIG_ETH_RMII_CLK_INPUT=y +# CONFIG_ETH_RMII_CLK_OUTPUT is not set +CONFIG_ETH_RMII_CLK_IN_GPIO=0 +CONFIG_ETH_DMA_BUFFER_SIZE=512 +CONFIG_ETH_DMA_RX_BUFFER_NUM=10 +CONFIG_ETH_DMA_TX_BUFFER_NUM=10 +CONFIG_ETH_USE_SPI_ETHERNET=y +CONFIG_ETH_SPI_ETHERNET_DM9051=y +# CONFIG_ETH_USE_OPENETH is not set +# end of Ethernet + +# +# Event Loop Library +# +# CONFIG_ESP_EVENT_LOOP_PROFILING is not set +CONFIG_ESP_EVENT_POST_FROM_ISR=y +CONFIG_ESP_EVENT_POST_FROM_IRAM_ISR=y +# end of Event Loop Library + +# +# ESP NETIF Adapter +# +CONFIG_ESP_NETIF_IP_LOST_TIMER_INTERVAL=120 +CONFIG_ESP_NETIF_TCPIP_LWIP=y +# CONFIG_ESP_NETIF_LOOPBACK is not set +CONFIG_ESP_NETIF_TCPIP_ADAPTER_COMPATIBLE_LAYER=y +# end of ESP NETIF Adapter + +# +# ESP System Settings +# +# CONFIG_ESP_SYSTEM_PANIC_PRINT_HALT is not set +CONFIG_ESP_SYSTEM_PANIC_PRINT_REBOOT=y +# CONFIG_ESP_SYSTEM_PANIC_SILENT_REBOOT is not set +# CONFIG_ESP_SYSTEM_PANIC_GDBSTUB is not set +# end of ESP System Settings + +# +# High resolution timer (esp_timer) +# +# CONFIG_ESP_TIMER_PROFILING is not set +CONFIG_ESP_TIMER_TASK_STACK_SIZE=3584 +# CONFIG_ESP_TIMER_IMPL_FRC2 is not set +CONFIG_ESP_TIMER_IMPL_TG0_LAC=y +# end of High resolution timer (esp_timer) + +# +# Wi-Fi +# +CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=2 +CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=16 +# CONFIG_ESP32_WIFI_STATIC_TX_BUFFER is not set +CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y +CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1 +CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=16 +# CONFIG_ESP32_WIFI_CSI_ENABLED is not set +# CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED is not set +# CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED is not set +# CONFIG_ESP32_WIFI_NVS_ENABLED is not set +CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y +# CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_1 is not set +CONFIG_ESP32_WIFI_SOFTAP_BEACON_MAX_LEN=752 +CONFIG_ESP32_WIFI_MGMT_SBUF_NUM=32 +# CONFIG_ESP32_WIFI_DEBUG_LOG_ENABLE is not set +# CONFIG_ESP32_WIFI_IRAM_OPT is not set +CONFIG_ESP32_WIFI_RX_IRAM_OPT=y +CONFIG_ESP32_WIFI_ENABLE_WPA3_SAE=y +# end of Wi-Fi + +# +# PHY +# +CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y +# CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set +CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 +CONFIG_ESP32_PHY_MAX_TX_POWER=20 +# end of PHY + +# +# Core dump +# +# CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH is not set +CONFIG_ESP32_ENABLE_COREDUMP_TO_UART=y +# CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE is not set +# CONFIG_ESP32_COREDUMP_DATA_FORMAT_BIN is not set +CONFIG_ESP32_COREDUMP_DATA_FORMAT_ELF=y +CONFIG_ESP32_COREDUMP_CHECKSUM_CRC32=y +# CONFIG_ESP32_COREDUMP_CHECKSUM_SHA256 is not set +CONFIG_ESP32_ENABLE_COREDUMP=y +CONFIG_ESP32_CORE_DUMP_MAX_TASKS_NUM=64 +CONFIG_ESP32_CORE_DUMP_UART_DELAY=0 +CONFIG_ESP32_CORE_DUMP_STACK_SIZE=0 +CONFIG_ESP32_CORE_DUMP_DECODE_INFO=y +# CONFIG_ESP32_CORE_DUMP_DECODE_DISABLE is not set +CONFIG_ESP32_CORE_DUMP_DECODE="info" +# end of Core dump + +# +# FAT Filesystem support +# +# CONFIG_FATFS_CODEPAGE_DYNAMIC is not set +CONFIG_FATFS_CODEPAGE_437=y +# CONFIG_FATFS_CODEPAGE_720 is not set +# CONFIG_FATFS_CODEPAGE_737 is not set +# CONFIG_FATFS_CODEPAGE_771 is not set +# CONFIG_FATFS_CODEPAGE_775 is not set +# CONFIG_FATFS_CODEPAGE_850 is not set +# CONFIG_FATFS_CODEPAGE_852 is not set +# CONFIG_FATFS_CODEPAGE_855 is not set +# CONFIG_FATFS_CODEPAGE_857 is not set +# CONFIG_FATFS_CODEPAGE_860 is not set +# CONFIG_FATFS_CODEPAGE_861 is not set +# CONFIG_FATFS_CODEPAGE_862 is not set +# CONFIG_FATFS_CODEPAGE_863 is not set +# CONFIG_FATFS_CODEPAGE_864 is not set +# CONFIG_FATFS_CODEPAGE_865 is not set +# CONFIG_FATFS_CODEPAGE_866 is not set +# CONFIG_FATFS_CODEPAGE_869 is not set +# CONFIG_FATFS_CODEPAGE_932 is not set +# CONFIG_FATFS_CODEPAGE_936 is not set +# CONFIG_FATFS_CODEPAGE_949 is not set +# CONFIG_FATFS_CODEPAGE_950 is not set +CONFIG_FATFS_CODEPAGE=437 +CONFIG_FATFS_LFN_NONE=y +# CONFIG_FATFS_LFN_HEAP is not set +# CONFIG_FATFS_LFN_STACK is not set +CONFIG_FATFS_FS_LOCK=0 +CONFIG_FATFS_TIMEOUT_MS=10000 +CONFIG_FATFS_PER_FILE_CACHE=y +# end of FAT Filesystem support + +# +# FreeRTOS +# +# CONFIG_FREERTOS_UNICORE is not set +CONFIG_FREERTOS_NO_AFFINITY=0x7FFFFFFF +# CONFIG_FREERTOS_CORETIMER_0 is not set +CONFIG_FREERTOS_CORETIMER_1=y +CONFIG_FREERTOS_HZ=500 +CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y +# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set +# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set +CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y +# CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK is not set +CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y +CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=10 +# CONFIG_FREERTOS_ASSERT_FAIL_ABORT is not set +CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE=y +# CONFIG_FREERTOS_ASSERT_DISABLE is not set +CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1536 +CONFIG_FREERTOS_ISR_STACKSIZE=2100 +# CONFIG_FREERTOS_LEGACY_HOOKS is not set +CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16 +CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION=y +# CONFIG_FREERTOS_ENABLE_STATIC_TASK_CLEAN_UP is not set +CONFIG_FREERTOS_TIMER_TASK_PRIORITY=1 +CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=2048 +CONFIG_FREERTOS_TIMER_QUEUE_LENGTH=10 +CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0 +CONFIG_FREERTOS_USE_TRACE_FACILITY=y +CONFIG_FREERTOS_USE_STATS_FORMATTING_FUNCTIONS=y +CONFIG_FREERTOS_VTASKLIST_INCLUDE_COREID=y +CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=y +CONFIG_FREERTOS_RUN_TIME_STATS_USING_ESP_TIMER=y +# CONFIG_FREERTOS_RUN_TIME_STATS_USING_CPU_CLK is not set +# CONFIG_FREERTOS_USE_TICKLESS_IDLE is not set +CONFIG_FREERTOS_CHECK_MUTEX_GIVEN_BY_OWNER=y +# CONFIG_FREERTOS_CHECK_PORT_CRITICAL_COMPLIANCE is not set +CONFIG_FREERTOS_DEBUG_OCDAWARE=y +# CONFIG_FREERTOS_FPU_IN_ISR is not set +# end of FreeRTOS + +# +# Heap memory debugging +# +CONFIG_HEAP_POISONING_DISABLED=y +# CONFIG_HEAP_POISONING_LIGHT is not set +# CONFIG_HEAP_POISONING_COMPREHENSIVE is not set +CONFIG_HEAP_TRACING_OFF=y +# CONFIG_HEAP_TRACING_STANDALONE is not set +# CONFIG_HEAP_TRACING_TOHOST is not set +# CONFIG_HEAP_ABORT_WHEN_ALLOCATION_FAILS is not set +# end of Heap memory debugging + +# +# Log output +# +# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set +# CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set +# CONFIG_LOG_DEFAULT_LEVEL_WARN is not set +CONFIG_LOG_DEFAULT_LEVEL_INFO=y +# CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set +# CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set +CONFIG_LOG_DEFAULT_LEVEL=3 +CONFIG_LOG_COLORS=y +CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y +# CONFIG_LOG_TIMESTAMP_SOURCE_SYSTEM is not set +# end of Log output + +# +# LWIP +# +CONFIG_LWIP_LOCAL_HOSTNAME="espressif" +CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y +# CONFIG_LWIP_L2_TO_L3_COPY is not set +# CONFIG_LWIP_IRAM_OPTIMIZATION is not set +CONFIG_LWIP_TIMERS_ONDEMAND=y +CONFIG_LWIP_MAX_SOCKETS=5 +# CONFIG_LWIP_USE_ONLY_LWIP_SELECT is not set +# CONFIG_LWIP_SO_LINGER is not set +CONFIG_LWIP_SO_REUSE=y +CONFIG_LWIP_SO_REUSE_RXTOALL=y +# CONFIG_LWIP_SO_RCVBUF is not set +# CONFIG_LWIP_NETBUF_RECVINFO is not set +CONFIG_LWIP_IP4_FRAG=y +CONFIG_LWIP_IP6_FRAG=y +# CONFIG_LWIP_IP4_REASSEMBLY is not set +# CONFIG_LWIP_IP6_REASSEMBLY is not set +# CONFIG_LWIP_IP_FORWARD is not set +# CONFIG_LWIP_STATS is not set +# CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set +CONFIG_LWIP_ESP_GRATUITOUS_ARP=y +CONFIG_LWIP_GARP_TMR_INTERVAL=60 +CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32 +CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y +# CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set + +# +# DHCP server +# +CONFIG_LWIP_DHCPS_LEASE_UNIT=60 +CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8 +# end of DHCP server + +# CONFIG_LWIP_AUTOIP is not set +# CONFIG_LWIP_IPV6_AUTOCONFIG is not set +CONFIG_LWIP_NETIF_LOOPBACK=y +CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8 + +# +# TCP +# +CONFIG_LWIP_TCP_ISN_HOOK=y +CONFIG_LWIP_MAX_ACTIVE_TCP=16 +CONFIG_LWIP_MAX_LISTENING_TCP=16 +CONFIG_LWIP_TCP_HIGH_SPEED_RETRANSMISSION=y +CONFIG_LWIP_TCP_MAXRTX=12 +CONFIG_LWIP_TCP_SYNMAXRTX=6 +CONFIG_LWIP_TCP_MSS=1436 +CONFIG_LWIP_TCP_TMR_INTERVAL=250 +CONFIG_LWIP_TCP_MSL=60000 +CONFIG_LWIP_TCP_SND_BUF_DEFAULT=5744 +CONFIG_LWIP_TCP_WND_DEFAULT=5744 +CONFIG_LWIP_TCP_RECVMBOX_SIZE=6 +CONFIG_LWIP_TCP_QUEUE_OOSEQ=y +# CONFIG_LWIP_TCP_SACK_OUT is not set +# CONFIG_LWIP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set +CONFIG_LWIP_TCP_OVERSIZE_MSS=y +# CONFIG_LWIP_TCP_OVERSIZE_QUARTER_MSS is not set +# CONFIG_LWIP_TCP_OVERSIZE_DISABLE is not set +CONFIG_LWIP_TCP_RTO_TIME=3000 +# end of TCP + +# +# UDP +# +CONFIG_LWIP_MAX_UDP_PCBS=16 +CONFIG_LWIP_UDP_RECVMBOX_SIZE=6 +# end of UDP + +CONFIG_LWIP_TCPIP_TASK_STACK_SIZE=3072 +CONFIG_LWIP_TCPIP_TASK_AFFINITY_NO_AFFINITY=y +# CONFIG_LWIP_TCPIP_TASK_AFFINITY_CPU0 is not set +# CONFIG_LWIP_TCPIP_TASK_AFFINITY_CPU1 is not set +CONFIG_LWIP_TCPIP_TASK_AFFINITY=0x7FFFFFFF +# CONFIG_LWIP_PPP_SUPPORT is not set +CONFIG_LWIP_IPV6_MEMP_NUM_ND6_QUEUE=3 +CONFIG_LWIP_IPV6_ND6_NUM_NEIGHBORS=5 + +# +# ICMP +# +# CONFIG_LWIP_MULTICAST_PING is not set +# CONFIG_LWIP_BROADCAST_PING is not set +# end of ICMP + +# +# LWIP RAW API +# +CONFIG_LWIP_MAX_RAW_PCBS=16 +# end of LWIP RAW API + +# +# SNTP +# +CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1 +CONFIG_LWIP_SNTP_UPDATE_DELAY=3600000 +# end of SNTP + +CONFIG_LWIP_ESP_LWIP_ASSERT=y +# end of LWIP + +# +# mbedTLS +# +CONFIG_MBEDTLS_INTERNAL_MEM_ALLOC=y +# CONFIG_MBEDTLS_DEFAULT_MEM_ALLOC is not set +# CONFIG_MBEDTLS_CUSTOM_MEM_ALLOC is not set +CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384 +# CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN is not set +# CONFIG_MBEDTLS_DYNAMIC_BUFFER is not set +# CONFIG_MBEDTLS_DEBUG is not set + +# +# Certificate Bundle +# +CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y +CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL=y +# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_CMN is not set +# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_NONE is not set +# CONFIG_MBEDTLS_CUSTOM_CERTIFICATE_BUNDLE is not set +# end of Certificate Bundle + +# CONFIG_MBEDTLS_ECP_RESTARTABLE is not set +# CONFIG_MBEDTLS_CMAC_C is not set +CONFIG_MBEDTLS_HARDWARE_AES=y +# CONFIG_MBEDTLS_HARDWARE_MPI is not set +# CONFIG_MBEDTLS_HARDWARE_SHA is not set +# CONFIG_MBEDTLS_ATCA_HW_ECDSA_SIGN is not set +# CONFIG_MBEDTLS_ATCA_HW_ECDSA_VERIFY is not set +CONFIG_MBEDTLS_HAVE_TIME=y +# CONFIG_MBEDTLS_HAVE_TIME_DATE is not set +CONFIG_MBEDTLS_ECDSA_DETERMINISTIC=y +CONFIG_MBEDTLS_SHA512_C=y +CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y +# CONFIG_MBEDTLS_TLS_SERVER_ONLY is not set +# CONFIG_MBEDTLS_TLS_CLIENT_ONLY is not set +# CONFIG_MBEDTLS_TLS_DISABLED is not set +CONFIG_MBEDTLS_TLS_SERVER=y +CONFIG_MBEDTLS_TLS_CLIENT=y +CONFIG_MBEDTLS_TLS_ENABLED=y + +# +# TLS Key Exchange Methods +# +CONFIG_MBEDTLS_PSK_MODES=y +CONFIG_MBEDTLS_KEY_EXCHANGE_PSK=y +CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_PSK=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_PSK=y +CONFIG_MBEDTLS_KEY_EXCHANGE_RSA_PSK=y +CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y +# end of TLS Key Exchange Methods + +CONFIG_MBEDTLS_SSL_RENEGOTIATION=y +# CONFIG_MBEDTLS_SSL_PROTO_SSL3 is not set +CONFIG_MBEDTLS_SSL_PROTO_TLS1=y +CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y +CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y +CONFIG_MBEDTLS_SSL_PROTO_DTLS=y +CONFIG_MBEDTLS_SSL_ALPN=y +CONFIG_MBEDTLS_CLIENT_SSL_SESSION_TICKETS=y +CONFIG_MBEDTLS_SERVER_SSL_SESSION_TICKETS=y + +# +# Symmetric Ciphers +# +CONFIG_MBEDTLS_AES_C=y +# CONFIG_MBEDTLS_CAMELLIA_C is not set +# CONFIG_MBEDTLS_DES_C is not set +CONFIG_MBEDTLS_RC4_DISABLED=y +# CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT is not set +# CONFIG_MBEDTLS_RC4_ENABLED is not set +# CONFIG_MBEDTLS_BLOWFISH_C is not set +# CONFIG_MBEDTLS_XTEA_C is not set +CONFIG_MBEDTLS_CCM_C=y +CONFIG_MBEDTLS_GCM_C=y +# end of Symmetric Ciphers + +# CONFIG_MBEDTLS_RIPEMD160_C is not set + +# +# Certificates +# +CONFIG_MBEDTLS_PEM_PARSE_C=y +CONFIG_MBEDTLS_PEM_WRITE_C=y +CONFIG_MBEDTLS_X509_CRL_PARSE_C=y +CONFIG_MBEDTLS_X509_CSR_PARSE_C=y +# end of Certificates + +CONFIG_MBEDTLS_ECP_C=y +CONFIG_MBEDTLS_ECDH_C=y +CONFIG_MBEDTLS_ECDSA_C=y +# CONFIG_MBEDTLS_ECJPAKE_C is not set +CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y +CONFIG_MBEDTLS_ECP_NIST_OPTIM=y +# CONFIG_MBEDTLS_POLY1305_C is not set +# CONFIG_MBEDTLS_CHACHA20_C is not set +# CONFIG_MBEDTLS_HKDF_C is not set +# CONFIG_MBEDTLS_THREADING_C is not set +# CONFIG_MBEDTLS_SECURITY_RISKS is not set +# end of mbedTLS + +# +# Newlib +# +CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y +# CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF is not set +# CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR is not set +# CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF is not set +# CONFIG_NEWLIB_STDIN_LINE_ENDING_LF is not set +CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y +# CONFIG_NEWLIB_NANO_FORMAT is not set +# end of Newlib + +# +# NVS +# +# end of NVS + +# +# PThreads +# +CONFIG_PTHREAD_TASK_PRIO_DEFAULT=5 +CONFIG_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072 +CONFIG_PTHREAD_STACK_MIN=768 +CONFIG_PTHREAD_DEFAULT_CORE_NO_AFFINITY=y +# CONFIG_PTHREAD_DEFAULT_CORE_0 is not set +# CONFIG_PTHREAD_DEFAULT_CORE_1 is not set +CONFIG_PTHREAD_TASK_CORE_DEFAULT=-1 +CONFIG_PTHREAD_TASK_NAME_DEFAULT="pthread" +# end of PThreads + +# +# SPI Flash driver +# +# CONFIG_SPI_FLASH_VERIFY_WRITE is not set +# CONFIG_SPI_FLASH_ENABLE_COUNTERS is not set +CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y +CONFIG_SPI_FLASH_DANGEROUS_WRITE_ABORTS=y +# CONFIG_SPI_FLASH_DANGEROUS_WRITE_FAILS is not set +# CONFIG_SPI_FLASH_DANGEROUS_WRITE_ALLOWED is not set +# CONFIG_SPI_FLASH_USE_LEGACY_IMPL is not set +# CONFIG_SPI_FLASH_SHARE_SPI1_BUS is not set +# CONFIG_SPI_FLASH_BYPASS_BLOCK_ERASE is not set +CONFIG_SPI_FLASH_YIELD_DURING_ERASE=y +CONFIG_SPI_FLASH_ERASE_YIELD_DURATION_MS=20 +CONFIG_SPI_FLASH_ERASE_YIELD_TICKS=1 +# CONFIG_SPI_FLASH_SIZE_OVERRIDE is not set + +# +# Auto-detect flash chips +# +CONFIG_SPI_FLASH_SUPPORT_ISSI_CHIP=y +CONFIG_SPI_FLASH_SUPPORT_MXIC_CHIP=y +CONFIG_SPI_FLASH_SUPPORT_GD_CHIP=y +# end of Auto-detect flash chips + +CONFIG_SPI_FLASH_ENABLE_ENCRYPTED_READ_WRITE=y +# end of SPI Flash driver + +# +# Virtual file system +# +CONFIG_VFS_SUPPORT_IO=y +CONFIG_VFS_SUPPORT_DIR=y +CONFIG_VFS_SUPPORT_SELECT=y +CONFIG_VFS_SUPPRESS_SELECT_DEBUG_OUTPUT=y +CONFIG_VFS_SUPPORT_TERMIOS=y + +# +# Host File System I/O (Semihosting) +# +CONFIG_VFS_SEMIHOSTFS_MAX_MOUNT_POINTS=1 +CONFIG_VFS_SEMIHOSTFS_HOST_PATH_MAX_LEN=128 +# end of Host File System I/O (Semihosting) +# end of Virtual file system + +# +# Wear Levelling +# +# CONFIG_WL_SECTOR_SIZE_512 is not set +CONFIG_WL_SECTOR_SIZE_4096=y +CONFIG_WL_SECTOR_SIZE=4096 +# end of Wear Levelling + +# +# Supplicant +# +CONFIG_WPA_MBEDTLS_CRYPTO=y +# CONFIG_WPA_DEBUG_PRINT is not set +# CONFIG_WPA_TESTING_OPTIONS is not set +# CONFIG_WPA_WPS_STRICT is not set +# end of Supplicant +# end of Component config + +# +# Compatibility options +# +# CONFIG_LEGACY_INCLUDE_COMMON_HEADERS is not set +# end of Compatibility options + +# Deprecated options for backward compatibility +CONFIG_TOOLPREFIX="xtensa-esp32-elf-" +# CONFIG_LOG_BOOTLOADER_LEVEL_NONE is not set +# CONFIG_LOG_BOOTLOADER_LEVEL_ERROR is not set +# CONFIG_LOG_BOOTLOADER_LEVEL_WARN is not set +CONFIG_LOG_BOOTLOADER_LEVEL_INFO=y +# CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG is not set +# CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE is not set +CONFIG_LOG_BOOTLOADER_LEVEL=3 +# CONFIG_APP_ROLLBACK_ENABLE is not set +# CONFIG_FLASH_ENCRYPTION_ENABLED is not set +CONFIG_FLASHMODE_QIO=y +# CONFIG_FLASHMODE_QOUT is not set +# CONFIG_FLASHMODE_DIO is not set +# CONFIG_FLASHMODE_DOUT is not set +# CONFIG_MONITOR_BAUD_9600B is not set +# CONFIG_MONITOR_BAUD_57600B is not set +CONFIG_MONITOR_BAUD_115200B=y +# CONFIG_MONITOR_BAUD_230400B is not set +# CONFIG_MONITOR_BAUD_921600B is not set +# CONFIG_MONITOR_BAUD_2MB is not set +# CONFIG_MONITOR_BAUD_OTHER is not set +CONFIG_MONITOR_BAUD_OTHER_VAL=115200 +CONFIG_MONITOR_BAUD=115200 +# CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG is not set +# CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE is not set +CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y +# CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set +# CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED is not set +# CONFIG_CXX_EXCEPTIONS is not set +CONFIG_STACK_CHECK_NONE=y +# CONFIG_STACK_CHECK_NORM is not set +# CONFIG_STACK_CHECK_STRONG is not set +# CONFIG_STACK_CHECK_ALL is not set +# CONFIG_WARN_WRITE_STRINGS is not set +# CONFIG_DISABLE_GCC8_WARNINGS is not set +# CONFIG_ESP32_APPTRACE_DEST_TRAX is not set +CONFIG_ESP32_APPTRACE_DEST_NONE=y +CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y +CONFIG_ADC2_DISABLE_DAC=y +# CONFIG_SPIRAM_SUPPORT is not set +CONFIG_TRACEMEM_RESERVE_DRAM=0x0 +CONFIG_TWO_UNIVERSAL_MAC_ADDRESS=y +# CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS is not set +CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=2 +# CONFIG_ULP_COPROC_ENABLED is not set +CONFIG_ULP_COPROC_RESERVE_MEM=0 +CONFIG_BROWNOUT_DET=y +CONFIG_BROWNOUT_DET_LVL_SEL_0=y +# CONFIG_BROWNOUT_DET_LVL_SEL_1 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_2 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_3 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_4 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_5 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_6 is not set +# CONFIG_BROWNOUT_DET_LVL_SEL_7 is not set +CONFIG_BROWNOUT_DET_LVL=0 +CONFIG_REDUCE_PHY_TX_POWER=y +CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y +# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL is not set +# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_OSC is not set +# CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_8MD256 is not set +# CONFIG_DISABLE_BASIC_ROM_CONSOLE is not set +# CONFIG_NO_BLOBS is not set +# CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set +CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32 +CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=2304 +CONFIG_MAIN_TASK_STACK_SIZE=3584 +CONFIG_IPC_TASK_STACK_SIZE=1024 +CONFIG_CONSOLE_UART_DEFAULT=y +# CONFIG_CONSOLE_UART_CUSTOM is not set +# CONFIG_CONSOLE_UART_NONE is not set +CONFIG_CONSOLE_UART_NUM=0 +CONFIG_CONSOLE_UART_TX_GPIO=1 +CONFIG_CONSOLE_UART_RX_GPIO=3 +CONFIG_CONSOLE_UART_BAUDRATE=115200 +CONFIG_INT_WDT=y +CONFIG_INT_WDT_TIMEOUT_MS=1000 +CONFIG_INT_WDT_CHECK_CPU1=y +CONFIG_TASK_WDT=y +# CONFIG_TASK_WDT_PANIC is not set +CONFIG_TASK_WDT_TIMEOUT_S=5 +CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y +CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU1=y +# CONFIG_EVENT_LOOP_PROFILING is not set +CONFIG_POST_EVENTS_FROM_ISR=y +CONFIG_POST_EVENTS_FROM_IRAM_ISR=y +# CONFIG_ESP32S2_PANIC_PRINT_HALT is not set +CONFIG_ESP32S2_PANIC_PRINT_REBOOT=y +# CONFIG_ESP32S2_PANIC_SILENT_REBOOT is not set +# CONFIG_ESP32S2_PANIC_GDBSTUB is not set +CONFIG_TIMER_TASK_STACK_SIZE=3584 +CONFIG_SUPPORT_STATIC_ALLOCATION=y +# CONFIG_ENABLE_STATIC_TASK_CLEAN_UP_HOOK is not set +CONFIG_TIMER_TASK_PRIORITY=1 +CONFIG_TIMER_TASK_STACK_DEPTH=2048 +CONFIG_TIMER_QUEUE_LENGTH=10 +# CONFIG_L2_TO_L3_COPY is not set +# CONFIG_USE_ONLY_LWIP_SELECT is not set +CONFIG_ESP_GRATUITOUS_ARP=y +CONFIG_GARP_TMR_INTERVAL=60 +CONFIG_TCPIP_RECVMBOX_SIZE=32 +CONFIG_TCP_MAXRTX=12 +CONFIG_TCP_SYNMAXRTX=6 +CONFIG_TCP_MSS=1436 +CONFIG_TCP_MSL=60000 +CONFIG_TCP_SND_BUF_DEFAULT=5744 +CONFIG_TCP_WND_DEFAULT=5744 +CONFIG_TCP_RECVMBOX_SIZE=6 +CONFIG_TCP_QUEUE_OOSEQ=y +# CONFIG_ESP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set +CONFIG_TCP_OVERSIZE_MSS=y +# CONFIG_TCP_OVERSIZE_QUARTER_MSS is not set +# CONFIG_TCP_OVERSIZE_DISABLE is not set +CONFIG_UDP_RECVMBOX_SIZE=6 +CONFIG_TCPIP_TASK_STACK_SIZE=3072 +CONFIG_TCPIP_TASK_AFFINITY_NO_AFFINITY=y +# CONFIG_TCPIP_TASK_AFFINITY_CPU0 is not set +# CONFIG_TCPIP_TASK_AFFINITY_CPU1 is not set +CONFIG_TCPIP_TASK_AFFINITY=0x7FFFFFFF +# CONFIG_PPP_SUPPORT is not set +CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5 +CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072 +CONFIG_ESP32_PTHREAD_STACK_MIN=768 +CONFIG_ESP32_DEFAULT_PTHREAD_CORE_NO_AFFINITY=y +# CONFIG_ESP32_DEFAULT_PTHREAD_CORE_0 is not set +# CONFIG_ESP32_DEFAULT_PTHREAD_CORE_1 is not set +CONFIG_ESP32_PTHREAD_TASK_CORE_DEFAULT=-1 +CONFIG_ESP32_PTHREAD_TASK_NAME_DEFAULT="pthread" +CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y +# CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS is not set +# CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED is not set +CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y +CONFIG_SUPPORT_TERMIOS=y +CONFIG_SEMIHOSTFS_MAX_MOUNT_POINTS=1 +CONFIG_SEMIHOSTFS_HOST_PATH_MAX_LEN=128 +# End of deprecated options diff --git a/libraries/AP_HAL_ESP32/targets/partitions.csv b/libraries/AP_HAL_ESP32/targets/partitions.csv new file mode 100644 index 0000000000..d245f35d50 --- /dev/null +++ b/libraries/AP_HAL_ESP32/targets/partitions.csv @@ -0,0 +1,5 @@ +# Name, Type, SubType, Offset, Size +nvs, data, nvs, , 0x6000 +phy_init, data, phy, , 0x1000 +factory, app, factory, , 2M +storage, 0x45, 0x0, , 128K diff --git a/libraries/AP_HAL_ESP32/utils/FontConverter.kt b/libraries/AP_HAL_ESP32/utils/FontConverter.kt new file mode 100644 index 0000000000..facb0c80d4 --- /dev/null +++ b/libraries/AP_HAL_ESP32/utils/FontConverter.kt @@ -0,0 +1,78 @@ +import java.io.File +import java.io.Writer +import java.util.* +import kotlin.collections.ArrayList + +const val char_width = 12 +const val char_height = 18 + + +data class Symbol( + var code: Int, + var levl: ArrayList, + var mask: ArrayList +) + +var symbols = ArrayList() + +fun readSymbols(fileName: String) { + var data = ArrayList() + Scanner(File(fileName)).use { scanner -> + scanner.next() + while (scanner.hasNext()) { + data.add(Integer.parseInt(scanner.next(), 2)) + } + } + for (ch in 0 until 256) { + var levl = ArrayList() + var mask = ArrayList() + for (y in 0 until char_height) { + for (x in 0 until char_width) { + var bitoffset = (y * char_width + x) * 2 + var byteoffset = 64 * ch + bitoffset / 8 + var bitshift = 6 - (bitoffset % 8) + var v = (data[byteoffset] ushr bitshift) and 3 + levl.add(v and 2 != 0) + mask.add(v and 1 == 0) + + } + } + symbols.add(Symbol(ch, levl, mask)) + } +} + +fun printSymbols(fileName: String) { + File(fileName).bufferedWriter().use { out -> + out.write("#include \n") + out.write("#ifdef WITH_INT_OSD\n") + + out.write("const uint32_t AP_OSD_INT::font_data[256 * 18] = {\n") + for (code in 0 until 256) { + val s = symbols[code] + + for (y in 0 until char_height) { + var v: Int = 0 + var gr = "" + for(x in 0 until char_width) { + if(s.levl[y * char_width + x]) { + v = v or (1 shl (31 - x)) + gr += "*" + } else { + gr += "-" + } + } + out.write("0x%08x,\t//\t%s\n".format(v, gr)) + print(gr + "\n") + } + out.write("\n") + print("\n") + } + out.write("};\n#endif\n") + } +} + +fun main() { + readSymbols("bold.mcm") + printSymbols("Symbols.cpp") +} + diff --git a/libraries/AP_HAL_ESP32/utils/profile/LinkerScriptGenerator.kt b/libraries/AP_HAL_ESP32/utils/profile/LinkerScriptGenerator.kt new file mode 100644 index 0000000000..e0fc4683c7 --- /dev/null +++ b/libraries/AP_HAL_ESP32/utils/profile/LinkerScriptGenerator.kt @@ -0,0 +1,92 @@ +import java.io.File +import java.io.FileOutputStream +import java.nio.charset.Charset +import java.util.* +import java.util.regex.Pattern +import kotlin.collections.ArrayList + +data class Func( + var symbol: String, + var place: String, + var address: Int, + var size: Int, + var count: Int +) + +val p = Pattern.compile(" (?:\\.iram1|\\.text)+\\.(\\S*)\\s*(0x.{16})\\s*(0x\\S*)\\s*(\\S*)") +val placep = Pattern.compile("[^(]*\\(([^.]*)") + +fun generateLinkerScript(mapFileName: String, profileFileName: String, scriptFileName: String) { + var addressToFunction = TreeMap() + fun parseMap() { + val s = File(mapFileName).readText(Charset.defaultCharset()) + val m = p.matcher(s) + while (m.find()) { + val symbol = m.group(1) + val address = Integer.decode(m.group(2)) + val size = Integer.decode(m.group(3)) + var place = m.group(4) + if (address == 0) { + continue + } + var placem = placep.matcher(place) + if (placem.find()) { + place = placem.group(1) + } + var f = Func(symbol, place, address, size, 0) + addressToFunction[f.address] = f + } + } + + fun parseProfile() { + Scanner(File(profileFileName)).use { scanner -> + while (scanner.hasNext()) { + val address = Integer.decode(scanner.next()) + val count = Integer.decode(scanner.next()) + for(f in addressToFunction.values) { + if(f.address <= address && address < f.address + f.size) { + f.count = count + } + } + } + } + } + + fun writeScript() { + var excl = listOf( "_ZN12NavEKF2_core15readRangeFinderEv", + "_ZN12NavEKF2_core18SelectRngBcnFusionEv","_ZN12NavEKF2_core14readRngBcnDataEv") + + var lst = ArrayList(addressToFunction.values.filter { it.count > 5000 && !excl.contains(it.symbol) }) + lst.sortWith(kotlin.Comparator { o1, o2 -> o2.count - o1.count }) + + var s = 0 + for (a in lst) { + System.out.format( + "0x%016x\t%8d\t%8d\t%s:%s\n", + a.address, + a.size, + a.count, + a.place, + a.symbol + ) + s += a.size + } + FileOutputStream(scriptFileName).use { + for (a in lst) { + it.write(String.format("%s:%s\n", a.place, a.symbol).toByteArray()) + } + } + System.out.format("total: %d\n", s) + } + parseMap() + parseProfile() + writeScript() +} + +fun main(args: Array) { + generateLinkerScript( + "arduplane.map", + "PROF000.TXT", + "functions.list" + ) +} \ No newline at end of file diff --git a/libraries/AP_HAL_ESP32/utils/profile/README.md b/libraries/AP_HAL_ESP32/utils/profile/README.md new file mode 100644 index 0000000000..639916f204 --- /dev/null +++ b/libraries/AP_HAL_ESP32/utils/profile/README.md @@ -0,0 +1,14 @@ +# How to profile ardupilot binary on ESP32 and use result to place critical functions to the IRAM +## Prepare source +1. Configure project with `--enable-profile` option: `./waf configure --board=esp32diy --enable-profile` +2. Rebuild binaries +## Run program with profiling enabled +1. Flash and run binary +2. Try to use mode silimiar to real flight. I.E. compass/gps connected, armed state, and so no +3. It will write profile stat to the `/APM/APM/PROF000.TXT` files on the sdcard every one minute +## Use profile info to optimize program +1. Copy `PROF*.TXT` file from sd card +2. Copy file `arducopter.map` from build directory +2. Use them to produce `functions.list` by the script `LinkerScriptGenerator.kt` (modify paths and limits inside) +3. Place file `functions.list` to the `target/copter/main` folder +