Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ESP32: RCOutput ported to new mcpwm driver

This commit is contained in:
ARg 2024-10-09 20:02:42 +02:00 committed by Peter Barker
parent d27742983f
commit a6f00a34b1
2 changed files with 101 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
libraries/AP_HAL_ESP32/RCOutput.cpp
View File

@ -12,7 +12,7 @@
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov
* *
*/ */
@ -27,6 +27,13 @@
#include <stdio.h> #include <stdio.h>
#include "esp_log.h"
#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick
#define SERVO_TIMEBASE_PERIOD 20000 // 20000 ticks, 20ms
#define TAG "RCOut"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
using namespace ESP32; using namespace ESP32;
@ -50,63 +57,90 @@ void RCOutput::init()
{ {
_max_channels = MAX_CHANNELS; _max_channels = MAX_CHANNELS;
#ifdef CONFIG_IDF_TARGET_ESP32
//32 and 33 are special as they dont default to gpio, but can be if u disable their rtc setup: // only on plain esp32
// 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_32);
rtc_gpio_deinit(GPIO_NUM_33); rtc_gpio_deinit(GPIO_NUM_33);
#endif
printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n"); printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n");
printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS); printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS);
printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n"); printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n");
static const mcpwm_io_signals_t signals[] = { const int MCPWM_CNT = SOC_MCPWM_OPERATORS_PER_GROUP*SOC_MCPWM_GENERATORS_PER_OPERATOR;
MCPWM0A,
MCPWM0B,
MCPWM1A,
MCPWM1B,
MCPWM2A,
MCPWM2B
};
static const mcpwm_timer_t timers[] = {
MCPWM_TIMER_0,
MCPWM_TIMER_1,
MCPWM_TIMER_2
}; for (int i = 0; i < MAX_CHANNELS; ++i) {
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) { mcpwm_timer_handle_t h_timer;
auto unit = units[i/6]; mcpwm_oper_handle_t h_oper;
auto signal = signals[i % 6];
auto timer = timers[i/2]; ESP_LOGI(TAG, "Initialize CH%02d", i+1);
//Save struct infos //Save struct infos
pwm_out &out = pwm_group_list[i]; pwm_out &out = pwm_group_list[i];
out.group_id = i/MCPWM_CNT;
out.gpio_num = outputs_pins[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; out.chan = i;
//Setup gpio if (0 == i % MCPWM_CNT) {
mcpwm_gpio_init(unit, signal, outputs_pins[i]);
//Setup MCPWM module mcpwm_timer_config_t timer_config = {
mcpwm_config_t pwm_config; .group_id = out.group_id,
pwm_config.frequency = 50; //frequency = 50Hz, i.e. for every servo motor time period should be 20ms .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 0 .resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ,
pwm_config.cmpr_b = 0; //duty cycle of PWMxb = 0 .count_mode = MCPWM_TIMER_COUNT_MODE_UP,
pwm_config.counter_mode = MCPWM_UP_COUNTER; .period_ticks = SERVO_TIMEBASE_PERIOD,
pwm_config.duty_mode = MCPWM_DUTY_MODE_0; };
mcpwm_init(unit, timer, &pwm_config);
mcpwm_start(unit, timer); ESP_LOGI(TAG, "Initialize timer");
ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config, &h_timer));
out.freq = timer_config.resolution_hz/timer_config.period_ticks;
ESP_LOGI(TAG, "Enable and start timer");
ESP_ERROR_CHECK(mcpwm_timer_enable(h_timer));
ESP_ERROR_CHECK(mcpwm_timer_start_stop(h_timer, MCPWM_TIMER_START_NO_STOP));
}
out.h_timer = h_timer;
if (0 == i % SOC_MCPWM_GENERATORS_PER_OPERATOR) {
ESP_LOGI(TAG, "Initialize operator");
mcpwm_operator_config_t operator_config = {
.group_id = out.group_id, // operator must be in the same group to the timer
};
ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config, &h_oper));
}
out.h_oper = h_oper;
ESP_LOGI(TAG, "Connect timer and operator");
ESP_ERROR_CHECK(mcpwm_operator_connect_timer(out.h_oper, out.h_timer));
ESP_LOGI(TAG, "Create comparator and generator from the operator");
mcpwm_comparator_config_t comparator_config = {};
comparator_config.flags.update_cmp_on_tez = true;
ESP_ERROR_CHECK(mcpwm_new_comparator(out.h_oper, &comparator_config, &out.h_cmpr));
mcpwm_generator_config_t generator_config = {
.gen_gpio_num = out.gpio_num,
};
ESP_ERROR_CHECK(mcpwm_new_generator(out.h_oper, &generator_config, &out.h_gen));
ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(out.h_cmpr, 1500));
out.value = 1500;
ESP_LOGI(TAG, "Set generator action on timer and compare event");
// go high on counter empty
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(out.h_gen,
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
// go low on compare threshold
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(out.h_gen,
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, out.h_cmpr, MCPWM_GEN_ACTION_LOW)));
} }
_initialized = true; _initialized = true;
@ -123,7 +157,8 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
for (uint8_t i = 0; i < MAX_CHANNELS; i++) { for (uint8_t i = 0; i < MAX_CHANNELS; i++) {
if (chmask & 1 << i) { if (chmask & 1 << i) {
pwm_out &out = pwm_group_list[i]; pwm_out &out = pwm_group_list[i];
mcpwm_set_frequency(out.unit_num, out.timer_num, freq_hz); ESP_ERROR_CHECK(mcpwm_timer_set_period( out.h_timer, SERVO_TIMEBASE_RESOLUTION_HZ/freq_hz));
out.freq = freq_hz;
} }
} }
} }
@ -143,7 +178,7 @@ uint16_t RCOutput::get_freq(uint8_t chan)
} }
pwm_out &out = pwm_group_list[chan]; pwm_out &out = pwm_group_list[chan];
return mcpwm_get_frequency(out.unit_num, out.timer_num); return out.freq;
} }
void RCOutput::enable_ch(uint8_t chan) void RCOutput::enable_ch(uint8_t chan)
@ -153,7 +188,7 @@ void RCOutput::enable_ch(uint8_t chan)
} }
pwm_out &out = pwm_group_list[chan]; pwm_out &out = pwm_group_list[chan];
mcpwm_start(out.unit_num, out.timer_num); ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_START_NO_STOP));
} }
void RCOutput::disable_ch(uint8_t chan) void RCOutput::disable_ch(uint8_t chan)
@ -164,7 +199,7 @@ void RCOutput::disable_ch(uint8_t chan)
write(chan, 0); write(chan, 0);
pwm_out &out = pwm_group_list[chan]; pwm_out &out = pwm_group_list[chan];
mcpwm_stop(out.unit_num, out.timer_num); ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_STOP_EMPTY));
} }
void RCOutput::write(uint8_t chan, uint16_t period_us) void RCOutput::write(uint8_t chan, uint16_t period_us)
@ -189,9 +224,7 @@ uint16_t RCOutput::read(uint8_t chan)
} }
pwm_out &out = pwm_group_list[chan]; pwm_out &out = pwm_group_list[chan];
double freq = mcpwm_get_frequency(out.unit_num, out.timer_num); return out.value;
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) void RCOutput::read(uint16_t *period_us, uint8_t len)
@ -248,7 +281,8 @@ void RCOutput::write_int(uint8_t chan, uint16_t period_us)
} }
pwm_out &out = pwm_group_list[chan]; pwm_out &out = pwm_group_list[chan];
mcpwm_set_duty_in_us(out.unit_num, out.timer_num, out.op, period_us); ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(out.h_cmpr, period_us));
out.value = period_us;
} }
/* /*

24
libraries/AP_HAL_ESP32/RCOutput.h
View File

@ -12,7 +12,7 @@
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov
* *
*/ */
@ -21,10 +21,12 @@
#include <AP_HAL/RCOutput.h> #include <AP_HAL/RCOutput.h>
#include "HAL_ESP32_Namespace.h" #include "HAL_ESP32_Namespace.h"
#include "driver/mcpwm.h"
#define HAL_PARAM_DEFAULTS_PATH nullptr #define HAL_PARAM_DEFAULTS_PATH nullptr
#include <AP_HAL/Util.h> #include <AP_HAL/Util.h>
#include "driver/gpio.h"
#include "driver/mcpwm_prelude.h"
namespace ESP32 namespace ESP32
{ {
@ -96,13 +98,19 @@ public:
private: private:
struct pwm_out { struct pwm_out {
int gpio_num;
mcpwm_unit_t unit_num; uint8_t chan;
mcpwm_timer_t timer_num; uint8_t gpio_num;
mcpwm_io_signals_t io_signal; uint8_t group_id;
mcpwm_operator_t op; int freq;
uint8_t chan; int value;
mcpwm_timer_handle_t h_timer;
mcpwm_oper_handle_t h_oper;
mcpwm_cmpr_handle_t h_cmpr;
mcpwm_gen_handle_t h_gen;
}; };