From 70f445e7f1c4154762d3718458aebb1f7e0bd8aa Mon Sep 17 00:00:00 2001 From: dorovl Date: Sun, 15 Mar 2020 14:37:26 +0100 Subject: [PATCH] Bone : Faulty PWM output after few flight controller restarts due to wrong IEP Timer configuration modified: Makefile modified: RcAioPRU.p modified: RcAioPRUTest.c modified: RcAioPRU_BBBLUE_bin.h modified: RcAioPRU_BBBMINI_bin.h modified: RcAioPRU_POCKET_bin.h new file: start_test --- .../Linux_HAL_Essentials/pru/aiopru/Makefile | 7 +- .../pru/aiopru/RcAioPRU.p | 82 ++++++--- .../pru/aiopru/RcAioPRUTest.c | 158 +++++++++--------- .../pru/aiopru/RcAioPRU_BBBLUE_bin.h | 31 ++-- .../pru/aiopru/RcAioPRU_BBBMINI_bin.h | 39 +++-- .../pru/aiopru/RcAioPRU_POCKET_bin.h | 27 +-- .../pru/aiopru/start_test | 12 ++ 7 files changed, 209 insertions(+), 147 deletions(-) create mode 100755 Tools/Linux_HAL_Essentials/pru/aiopru/start_test diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/Makefile b/Tools/Linux_HAL_Essentials/pru/aiopru/Makefile index 8f56fa6faf..145d444e61 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/Makefile +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/Makefile @@ -3,8 +3,13 @@ RcAioPRU: RcAioPRU.p pasm -V3 -c RcAioPRU.p RcAioPRU_BBBLUE -DBBBLUE pasm -V3 -c RcAioPRU.p RcAioPRU_POCKET -DPOCKET +debug: RcAioPRU.p + pasm -V3 -c RcAioPRU.p RcAioPRU_BBBMINI -DBBBMINI -DDEBUG + pasm -V3 -c RcAioPRU.p RcAioPRU_BBBLUE -DBBBLUE -DDEBUG + pasm -V3 -c RcAioPRU.p RcAioPRU_POCKET -DPOCKET -DDEBUG + test: RcAioPRUTest.c gcc -g -o RcAioPRUTest RcAioPRUTest.c clean: - rm RcAioPRU_bin.h RcAioPRUTest + rm RcAioPRU_BBBMINI_bin.h RcAioPRU_BBBLUE_bin.h RcAioPRU_POCKET_bin.h RcAioPRUTest diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p index 650bb9203a..00ff5f2162 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p @@ -9,11 +9,11 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . - + // RC AllInOnePRU -// +// // 1 channel RCInput with 5ns accuracy -// 12 channel RCOutput with 1us accuracy +// 12 channel RCOutput with 1us accuracy // Timer #define TICK_PER_US 200 @@ -31,19 +31,18 @@ #define RCIN_RINGBUFFERSIZE 300 // PRU Constants Table +// -> 4.4.1.1 Constants Table in TRM #define ECAP C3 #define RAM C24 #define IEP C26 // IEP +// -> 4.5.4 PRU_ICSS_IEP Registers in TRM #define IEP_TMR_GLB_CFG 0x0 -#define IEP_TMR_GLB_STS 0x4 #define IEP_TMR_CNT 0xc -#define IEP_CNT_ENABLE 0x0 -#define IEP_DEFAULT_INC 0x4 - // ECAP +// -> 15.3.4.1 ECAP Registers in TRM #define ECAP_TSCTR 0x0 #define ECAP_CTRPHS 0x4 #define ECAP_CAP1 0x8 @@ -59,6 +58,7 @@ #define ECAP_REVID 0x5c // ECCTL1 +// -> 15.3.4.1.7 ECCTL1 Register in TRM #define ECAP_CAP1POL 0 #define ECAP_CTRRST1 1 #define ECAP_CAP2POL 2 @@ -72,6 +72,7 @@ #define ECAP_FREE_SOFT 14 // ECCTL2 +// -> 15.3.4.1.8 ECCTL2 Register in TRM #define ECAP_CONT_ONESHT 0 #define ECAP_STOP_WRAP 1 #define ECAP_RE_ARM 3 @@ -83,6 +84,8 @@ #define ECAP_APWMPOL 10 // ECEINT, ECFLG +// -> 15.3.4.1.9 ECEINT Register in TRM +// -> 15.3.4.1.10 ECFLG Register in TRM #define ECAP_INT 0 #define ECAP_CEVT1 1 #define ECAP_CEVT2 2 @@ -118,8 +121,9 @@ #define CH_11_T_TIME_RAM_OFFSET (22 * 4) #define CH_12_PULSE_TIME_RAM_OFFSET (23 * 4) #define CH_12_T_TIME_RAM_OFFSET (24 * 4) +#define TIME_OFFSET (25 * 4) +#define MAX_CYCLE_TIME_OFFSET (26 * 4) -#define MAX_CYCLE_TIME_OFFSET (25 * 4) #define RCIN_RING_HEAD_OFFSET 0x1000 #define RCIN_RING_TAIL_OFFSET 0x1002 @@ -191,7 +195,7 @@ .u32 ch_11_next_time .u32 ch_12_next_time .u32 time - .u32 time_max + .u32 time_max .u32 time_cycle .u32 rcin_ram_pointer .u32 rcin_ram_pointer_index @@ -226,9 +230,9 @@ pwm: // Do not set pin if pulse time is 0 qbeq pwmend, register.temp, 0 - // Check if channel is enabled + // Check if channel is enabled qbbc pwmend, CH_X_ENABLE - + // Set pin set RC_CH_X_PIN jmp pwmend @@ -243,7 +247,7 @@ pwm: // Calculate time to next event (T - pulse duration) sub register.temp, register.temp, register.temp1 add CH_X_NEXT_TIME, CH_X_NEXT_TIME, register.temp - + // Clear pin clr RC_CH_X_PIN pwmend: @@ -251,8 +255,21 @@ pwmend: .macro RCIN_ECAP_INIT // Initialize ECAP + // -> 15.3.4.1.7 ECCTL1 Register in TRM + // ECAP_CTRRST1 = 1 : Reset counter after Event 1 time-stamp has been captured (used in difference mode operation) + // ECAP_CAP1POL = 1 : Capture Event 1 triggered on a falling edge (FE) + // ECAP_CAP2POL = 0 : Capture Event 2 triggered on a rising edge (RE) - default + // ECAP_CTRRST2 = 1 : Reset counter after Event 2 time-stamp has been captured (used in difference mode operation) + // ECAP_CAPLDEN = 1 : Enable CAP1-4 register loads at capture event time mov register.temp, (1 << ECAP_CTRRST1) | (1 << ECAP_CAP1POL) | (1 << ECAP_CTRRST2) | (1 << ECAP_CAPLDEN) sbco register.temp, ECAP, ECAP_ECCTL1, 4 + + // -> 15.3.4.1.8 ECCTL2 Register in TRM + // ECAP_CONT_ONESHT = 1 : Operate in continuous mode - default + // ECAP_STOP_WRAP = = 1 : Wrap after Capture Event 2 in continuous mode + // ECAP_TSCTRSTOP = 1 : TSCTR free-running + // ECAP_SYNCO_SEL = 2 : Disable sync out signal + // ECAP_CAP_APWM = 0 : ECAP module operates in capture mode - default mov register.temp, (1 << ECAP_STOP_WRAP) | (1 << ECAP_TSCTRSTOP) | (2 << ECAP_SYNCO_SEL) sbco register.temp, ECAP, ECAP_ECCTL2, 4 .endm @@ -281,7 +298,7 @@ pwmend: // Check end of ringbuffer qblt rcin_ecap_end, register.rcin_ram_pointer_index_max, register.rcin_ram_pointer_index - mov register.rcin_ram_pointer, RCIN_RINGBUFFER_RAM_OFFSET + mov register.rcin_ram_pointer, RCIN_RINGBUFFER_RAM_OFFSET mov register.rcin_ram_pointer_index, 0 rcin_ecap_end: .endm @@ -317,7 +334,7 @@ rcin_ecap_end: // Initialize ringbuffer mov register.rcin_ram_pointer, RCIN_RINGBUFFER_RAM_OFFSET - mov register.rcin_ram_pointer_index_max, RCIN_RINGBUFFERSIZE + mov register.rcin_ram_pointer_index_max, RCIN_RINGBUFFERSIZE mov register.rcin_ram_pointer_head, RCIN_RING_HEAD_OFFSET mov register.rcin_ram_pointer_tail, RCIN_RING_TAIL_OFFSET mov register.temp, 0 @@ -344,7 +361,7 @@ rcin_ecap_end: // Initialize PWM pulse (0us) mov register.temp, PWM_PULSE_DEFAULT - sbco register.temp, RAM, CH_1_PULSE_TIME_RAM_OFFSET, 4 + sbco register.temp, RAM, CH_1_PULSE_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_2_PULSE_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_3_PULSE_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_4_PULSE_TIME_RAM_OFFSET, 4 @@ -359,7 +376,7 @@ rcin_ecap_end: // Initialize PWM frequency (50Hz) mov register.temp, PWM_FREQ_DEFAULT - sbco register.temp, RAM, CH_1_T_TIME_RAM_OFFSET, 4 + sbco register.temp, RAM, CH_1_T_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_2_T_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_3_T_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_4_T_TIME_RAM_OFFSET, 4 @@ -372,18 +389,33 @@ rcin_ecap_end: sbco register.temp, RAM, CH_11_T_TIME_RAM_OFFSET, 4 sbco register.temp, RAM, CH_12_T_TIME_RAM_OFFSET, 4 - // Initialize counter (1 step = 5ns) - mov register.temp, 1 << IEP_DEFAULT_INC + // Initializes the TIME_OFFSET and MAX_CYCLE_TIME_OFFSET + mov register.temp, 0 + sbco register.temp, RAM, TIME_OFFSET, 4 + sbco register.temp, RAM, MAX_CYCLE_TIME_OFFSET, 4 + + // Disables the counter of IEP timer + // -> 4.5.4.1 IEP_TMR_GLB_CFG Register in TRM + // CNT_ENABLE = 0 + lbco register.temp, IEP, IEP_TMR_GLB_CFG, 4 + clr register.temp.t0 sbco register.temp, IEP, IEP_TMR_GLB_CFG, 4 - // Reset counter + // Resets the counter of IEP timer + // Reset Count Register (CNT) by writing 0xFFFFFFFF to clear + // -> 4.4.3.2.2 Basic Programming Model in TRM + // -> 4.5.4.4 IEP_TMR_CNT Register in TRM mov register.temp, 0xffffffff sbco register.temp, IEP, IEP_TMR_CNT, 4 - // Start counter - lbco register.temp, IEP, IEP_TMR_GLB_CFG, 4 - or register.temp, register.temp, 1 << IEP_CNT_ENABLE - sbco register.temp, IEP, IEP_TMR_GLB_CFG, 4 + // Configures the IEP counter and enables it + // -> 4.5.4.1 IEP_TMR_GLB_CFG Register in TRM + // -> Reference: https://github.com/beagleboard/am335x_pru_package/blob/master/pru_sw/example_apps/PRU_industrialEthernetTimer/PRU_industrialEthernetTimer.p + // CMP_INC = 1 + // DEFAULT_INC = 1 + // CNT_ENABLE = 1 + mov register.temp, 0x0111 + sbco register.temp, IEP, IEP_TMR_GLB_CFG, 2 .endm .origin 0 @@ -393,6 +425,10 @@ init: mainloop: lbco register.ch_enable, RAM, CH_ENABLE_RAM_OFFSET, 4 lbco register.time, IEP, IEP_TMR_CNT, 4 +#ifdef DEBUG + //Reports the IEP counter; this used to check that the IEP counter operates normally (for debug) + sbco register.time, RAM, TIME_OFFSET, 4 +#endif RCOUT_PWM RC_CH_1_PIN, register.ch_1_next_time, RC_CH_1_ENABLE, CH_1_PULSE_TIME_RAM_OFFSET, CH_1_T_TIME_RAM_OFFSET RCOUT_PWM RC_CH_2_PIN, register.ch_2_next_time, RC_CH_2_ENABLE, CH_2_PULSE_TIME_RAM_OFFSET, CH_2_T_TIME_RAM_OFFSET RCOUT_PWM RC_CH_3_PIN, register.ch_3_next_time, RC_CH_3_ENABLE, CH_3_PULSE_TIME_RAM_OFFSET, CH_3_T_TIME_RAM_OFFSET diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c index dce3b0b9a6..417d84643e 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c @@ -11,11 +11,16 @@ #include #include +#include +#include #include #include #include -#include "RcAioPRU_bin.h" +//Comment/uncomment the #includes statements depending on your BeagleBone version: +//#include "RcAioPRU_POCKET_bin.h" +//#include "RcAioPRU_BBBMINI_bin.h" +#include "RcAioPRU_BBBLUE_bin.h" #define NUM_RING_ENTRIES 300 #define RCOUT_PRUSS_RAM_BASE 0x4a302000 @@ -23,12 +28,16 @@ #define RCOUT_PRUSS_IRAM_BASE 0x4a338000 #define RCIN_PRUSS_RAM_BASE 0x4a303000 +#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) + +#define PWM_FREQ 50 + struct ring_buffer { volatile uint16_t ring_head; volatile uint16_t ring_tail; struct { - uint32_t s1; - uint32_t s0; + volatile uint32_t s1; + volatile uint32_t s0; } buffer[NUM_RING_ENTRIES]; }; @@ -58,101 +67,92 @@ struct pwm { volatile uint32_t ch11_t_time; volatile uint32_t ch12_hi_time; volatile uint32_t ch12_t_time; + volatile uint32_t time; volatile uint32_t max_cycle_time; }; volatile struct ring_buffer *ring_buffer; volatile struct pwm *pwm; +static const uint32_t TICK_PER_US = 200; +static const uint32_t TICK_PER_S = 200000000; +static const uint32_t TICK_DURATION_NS = 5; + int main (void) { - unsigned int ret, i, a, s0, s1, min_s0, min_s1, max_s0, max_s1; - uint32_t mem_fd; - uint32_t *iram; - uint32_t *ctrl; - - mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC); - + unsigned int ret, s0, s1, min_s0 = 0xffffffff, min_s1 = 0xffffffff, max_s0 = 0, max_s1 = 0; + uint32_t mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC); ring_buffer = (struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCIN_PRUSS_RAM_BASE); pwm = (struct pwm*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_RAM_BASE); - iram = (uint32_t*)mmap(0, 0x2000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_IRAM_BASE); - ctrl = (uint32_t*)mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_CTRL_BASE); - + uint32_t *iram = (uint32_t*)mmap(0, 0x2000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_IRAM_BASE); + uint32_t *ctrl = (uint32_t*)mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_CTRL_BASE); + uint64_t time_ns; close(mem_fd); - // Reset PRU 1 - *ctrl = 0; - usleep(5 * 1000); - - // Load firmware - for(i = 0; i < ARRAY_SIZE(PRUcode); i++) { - *(iram + i) = PRUcode[i]; - } - - usleep(5 * 1000); - // Start PRU 1 - *ctrl = 3; - - i = 1; - a = 0; - min_s0 = 0xffffffff; - min_s1 = 0xffffffff; - max_s0 = 0; - max_s1 = 0; - - pwm->ch1_t_time = 200 * 2200; - pwm->ch2_t_time = 200 * 2300; - pwm->ch3_t_time = 200 * 2400; - pwm->ch4_t_time = 200 * 2500; - pwm->ch5_t_time = 200 * 2600; - pwm->ch6_t_time = 200 * 2700; - pwm->ch7_t_time = 200 * 2800; - pwm->ch8_t_time = 200 * 2900; - pwm->ch9_t_time = 200 * 3000; - pwm->ch10_t_time = 200 * 3100; - pwm->ch11_t_time = 200 * 3200; - pwm->ch12_t_time = 200 * 3300; - pwm->enable=0xffffffff; + // This loop checks that the IEP counter is really started. If not, the PRU is reset, the program is reload and PRU restarted + // To report pwm->time and pwm->max_cycle_time, the PRU program must be compiled with -DDEBUG option, for example: + // pasm -V3 -c RcAioPRU.p RcAioPRU_BBBLUE -DBBBLUE -DDEBUG + // This is made for you by 'make debug' followed by 'make test' + do { + printf("The PRU will be reset\n"); + // Reset PRU 1 + *ctrl = 0; + //You might uncomment this to identify more easily where the program ends in the IRAM + //memset(iram, '\0', sizeof(PRUcode) + 128); + // Load firmware + memcpy(iram, PRUcode, sizeof(PRUcode)); + // Start PRU 1 + *ctrl |= 2; + pwm->ch1_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch2_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch3_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch4_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch5_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch6_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch7_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch8_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch9_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch10_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch11_t_time = TICK_PER_S / PWM_FREQ; + pwm->ch12_t_time = TICK_PER_S / PWM_FREQ; + pwm->enable=0xffffffff; + printf("IEP counter: 0x%08x\n", pwm->time); + } while (pwm->time == 0xffffffff); while(1) { - for(a = 0; a < NUM_RING_ENTRIES; a++) { + for(unsigned int a = 0; a < NUM_RING_ENTRIES; a++) { s0 = ring_buffer->buffer[a].s0; s1 = ring_buffer->buffer[a].s1; - if((s0 > max_s0) && (s0 != 1001)) {max_s0 = s0;} - if((s1 > max_s1) && (s1 != 1001)) {max_s1 = s1;} - if((s0 < min_s0) && (s0 != 1001)) {min_s0 = s0;} - if((s1 < min_s1) && (s1 != 1001)) {min_s1 = s1;} + if(s0 > max_s0) {max_s0 = s0;} + if(s1 > max_s1) {max_s1 = s1;} + if(s0 < min_s0) {min_s0 = s0;} + if(s1 < min_s1) {min_s1 = s1;} } s0 = ring_buffer->buffer[ring_buffer->ring_tail].s0; s1 = ring_buffer->buffer[ring_buffer->ring_tail].s1; - printf("max ct: %u ns head: %u tail: %3u s0: %7u s1: %7u s01: %7u jitter_s0 %u ns jitter_s1 %u ns\n", pwm->max_cycle_time * 5, ring_buffer->ring_head, ring_buffer->ring_tail, s0/200, s1/200, (s0+s1)/200, ((max_s0-min_s0) * 5), ((max_s1-min_s1) * 5)); -// pwm->ch1_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch2_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch1_hi_time = 1000 * 200; -// pwm->ch2_hi_time = 1500 * 200; - pwm->ch3_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch4_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch5_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch6_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch7_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch8_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch9_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch10_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch11_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - pwm->ch12_hi_time = (uint32_t)((rand() % 1001 + 900) * 200); - - usleep(50 * 1000); - - if(i < 100) { - min_s0 = 0xffffffff; - min_s1 = 0xffffffff; - max_s0 = 0; - max_s1 = 0; - i++; - } - else if(((max_s1 - min_s1) * 5) > 1001000) { - i = 500; - } + time_ns = ((double)pwm->time) * ((double)TICK_DURATION_NS); + printf("max ct: %3u cycles time: %11lluns head: %u tail: %3u s0: %7u s1: %7u s01: %7u jitter_s0: %uns jitter_s1: %uns\n", pwm->max_cycle_time, time_ns, ring_buffer->ring_head, ring_buffer->ring_tail, s0 * TICK_DURATION_NS, s1 * TICK_DURATION_NS, (s0+s1) * TICK_DURATION_NS, ((max_s0-min_s0) * TICK_DURATION_NS), ((max_s1-min_s1) * TICK_DURATION_NS)); + // uint32_t value = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + // pwm->ch1_hi_time = value; + // pwm->ch2_hi_time = value; + //pwm->ch1_hi_time = 1500 * TICK_PER_US; + pwm->ch1_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch2_hi_time = 1500 * TICK_PER_US; + pwm->ch3_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch4_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch5_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch6_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch7_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch8_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch9_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch10_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch11_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + pwm->ch12_hi_time = (uint32_t)((rand() % 1001 + 900) * TICK_PER_US); + usleep(1000000); + min_s0 = 0xffffffff; + min_s1 = 0xffffffff; + max_s0 = 0; + max_s1 = 0; } return 0; } diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBLUE_bin.h b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBLUE_bin.h index 919bd84572..c7e8a33e90 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBLUE_bin.h +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBLUE_bin.h @@ -8,7 +8,7 @@ const unsigned int PRUcode[] = { 0x240000fe, 0x2effaf84, 0x240000f9, - 0x81643899, + 0x81683899, 0x241004f4, 0x24012cf6, 0x241000f7, @@ -57,14 +57,17 @@ const unsigned int PRUcode[] = { 0x81503899, 0x81583899, 0x81603899, - 0x240010f9, + 0x240000f9, + 0x81643899, + 0x81683899, + 0x91003a99, + 0x1d00f9f9, 0x81003a99, 0x24ffffd9, 0x24ffff99, 0x810c3a99, - 0x91003a99, - 0x1301f9f9, - 0x81003a99, + 0x240111f9, + 0x81001a99, 0x24010bf9, 0x81282399, 0x240092f9, @@ -82,7 +85,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc900e408, 0x1f08fefe, - 0x21005300, + 0x21005600, 0x9104389a, 0x91083899, 0x04faf9f9, @@ -99,7 +102,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc901e408, 0x1f0afefe, - 0x21006400, + 0x21006700, 0x910c389a, 0x91103899, 0x04faf9f9, @@ -116,7 +119,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc902e408, 0x1f09fefe, - 0x21007500, + 0x21007800, 0x9114389a, 0x91183899, 0x04faf9f9, @@ -133,7 +136,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc903e408, 0x1f0bfefe, - 0x21008600, + 0x21008900, 0x911c389a, 0x91203899, 0x04faf9f9, @@ -150,7 +153,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc904e408, 0x1f06fefe, - 0x21009700, + 0x21009a00, 0x9124389a, 0x91283899, 0x04faf9f9, @@ -167,7 +170,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc905e408, 0x1f07fefe, - 0x2100a800, + 0x2100ab00, 0x912c389a, 0x91303899, 0x04faf9f9, @@ -184,7 +187,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc906e408, 0x1f04fefe, - 0x2100b900, + 0x2100bc00, 0x9134389a, 0x91383899, 0x04faf9f9, @@ -201,7 +204,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc907e408, 0x1f05fefe, - 0x2100ca00, + 0x2100cd00, 0x913c389a, 0x91403899, 0x04faf9f9, @@ -219,5 +222,5 @@ const unsigned int PRUcode[] = { 0x48f5f603, 0x241004f4, 0x240000f5, - 0x21004000 }; + 0x21004300 }; diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBMINI_bin.h b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBMINI_bin.h index 4fe925148d..9b92907d62 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBMINI_bin.h +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_BBBMINI_bin.h @@ -8,7 +8,7 @@ const unsigned int PRUcode[] = { 0x240000fe, 0x2effaf84, 0x240000f9, - 0x81643899, + 0x81683899, 0x241004f4, 0x24012cf6, 0x241000f7, @@ -57,14 +57,17 @@ const unsigned int PRUcode[] = { 0x81503899, 0x81583899, 0x81603899, - 0x240010f9, + 0x240000f9, + 0x81643899, + 0x81683899, + 0x91003a99, + 0x1d00f9f9, 0x81003a99, 0x24ffffd9, 0x24ffff99, 0x810c3a99, - 0x91003a99, - 0x1301f9f9, - 0x81003a99, + 0x240111f9, + 0x81001a99, 0x24010bf9, 0x81282399, 0x240092f9, @@ -82,7 +85,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc900e408, 0x1f0afefe, - 0x21005300, + 0x21005600, 0x9104389a, 0x91083899, 0x04faf9f9, @@ -99,7 +102,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc901e408, 0x1f08fefe, - 0x21006400, + 0x21006700, 0x910c389a, 0x91103899, 0x04faf9f9, @@ -116,7 +119,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc902e408, 0x1f0bfefe, - 0x21007500, + 0x21007800, 0x9114389a, 0x91183899, 0x04faf9f9, @@ -133,7 +136,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc903e408, 0x1f09fefe, - 0x21008600, + 0x21008900, 0x911c389a, 0x91203899, 0x04faf9f9, @@ -150,7 +153,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc904e408, 0x1f07fefe, - 0x21009700, + 0x21009a00, 0x9124389a, 0x91283899, 0x04faf9f9, @@ -167,7 +170,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc905e408, 0x1f06fefe, - 0x2100a800, + 0x2100ab00, 0x912c389a, 0x91303899, 0x04faf9f9, @@ -184,7 +187,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc906e408, 0x1f05fefe, - 0x2100b900, + 0x2100bc00, 0x9134389a, 0x91383899, 0x04faf9f9, @@ -201,7 +204,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc907e408, 0x1f04fefe, - 0x2100ca00, + 0x2100cd00, 0x913c389a, 0x91403899, 0x04faf9f9, @@ -218,7 +221,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc908e408, 0x1f03fefe, - 0x2100db00, + 0x2100de00, 0x9144389a, 0x91483899, 0x04faf9f9, @@ -235,7 +238,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc909e408, 0x1f02fefe, - 0x2100ec00, + 0x2100ef00, 0x914c389a, 0x91503899, 0x04faf9f9, @@ -252,7 +255,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc90ae408, 0x1f01fefe, - 0x2100fd00, + 0x21010000, 0x9154389a, 0x91583899, 0x04faf9f9, @@ -269,7 +272,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc90be408, 0x1f00fefe, - 0x21010e00, + 0x21011100, 0x915c389a, 0x91603899, 0x04faf9f9, @@ -287,5 +290,5 @@ const unsigned int PRUcode[] = { 0x48f5f603, 0x241004f4, 0x240000f5, - 0x21004000 }; + 0x21004300 }; diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_POCKET_bin.h b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_POCKET_bin.h index 101f8af7eb..3b523db4b1 100644 --- a/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_POCKET_bin.h +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_POCKET_bin.h @@ -8,7 +8,7 @@ const unsigned int PRUcode[] = { 0x240000fe, 0x2effaf84, 0x240000f9, - 0x81643899, + 0x81683899, 0x241004f4, 0x24012cf6, 0x241000f7, @@ -57,14 +57,17 @@ const unsigned int PRUcode[] = { 0x81503899, 0x81583899, 0x81603899, - 0x240010f9, + 0x240000f9, + 0x81643899, + 0x81683899, + 0x91003a99, + 0x1d00f9f9, 0x81003a99, 0x24ffffd9, 0x24ffff99, 0x810c3a99, - 0x91003a99, - 0x1301f9f9, - 0x81003a99, + 0x240111f9, + 0x81001a99, 0x24010bf9, 0x81282399, 0x240092f9, @@ -82,7 +85,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc900e408, 0x1f07fefe, - 0x21005300, + 0x21005600, 0x9104389a, 0x91083899, 0x04faf9f9, @@ -99,7 +102,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc901e408, 0x1f04fefe, - 0x21006400, + 0x21006700, 0x910c389a, 0x91103899, 0x04faf9f9, @@ -116,7 +119,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc902e408, 0x1f01fefe, - 0x21007500, + 0x21007800, 0x9114389a, 0x91183899, 0x04faf9f9, @@ -133,7 +136,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc903e408, 0x1f05fefe, - 0x21008600, + 0x21008900, 0x911c389a, 0x91203899, 0x04faf9f9, @@ -150,7 +153,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc904e408, 0x1f02fefe, - 0x21009700, + 0x21009a00, 0x9124389a, 0x91283899, 0x04faf9f9, @@ -167,7 +170,7 @@ const unsigned int PRUcode[] = { 0x5100f909, 0xc905e408, 0x1f06fefe, - 0x2100a800, + 0x2100ab00, 0x912c389a, 0x91303899, 0x04faf9f9, @@ -185,5 +188,5 @@ const unsigned int PRUcode[] = { 0x48f5f603, 0x241004f4, 0x240000f5, - 0x21004000 }; + 0x21004300 }; diff --git a/Tools/Linux_HAL_Essentials/pru/aiopru/start_test b/Tools/Linux_HAL_Essentials/pru/aiopru/start_test new file mode 100755 index 0000000000..437e267d03 --- /dev/null +++ b/Tools/Linux_HAL_Essentials/pru/aiopru/start_test @@ -0,0 +1,12 @@ +#!/bin/bash +#make debug +#make test +if [ ! -L /sys/class/gpio/gpio80 ] || [ ! -e /sys/class/gpio/gpio80 ] ; then + /bin/echo 80 >/sys/class/gpio/export + /bin/sleep 1 + /bin/echo out >/sys/class/gpio/gpio80/direction +fi +/bin/echo 1 >/sys/class/gpio/gpio80/value +#/bin/echo 0 >/sys/class/gpio/gpio80/value +/bin/echo pruecapin_pu >/sys/devices/platform/ocp/ocp:P8_15_pinmux/state +sudo ./RcAioPRUTest