mirror of https://github.com/ArduPilot/ardupilot
310 lines
7.9 KiB
C++
310 lines
7.9 KiB
C++
|
#include <AP_HAL/AP_HAL.h>
|
||
|
|
|
||
|
|
#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 <AP_Math/AP_Math.h>
|
||
|
|
#include <AP_OSD/AP_OSD_INT.h>
|
||
|
|
|
||
|
|
#include <stdio.h>
|
||
|
|
|
||
|
|
#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
|