mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
Linux_HAL_Essentials: RC AIO PRU firmware. Does RC input (5ns accuracy) and 12 PWM RC ouputs (better than 1us accuracy) with only one PRU.
This commit is contained in:
mirkix
Andrew Tridgell
parent
d4d42599b0
commit
bd34ffa7db
8
Tools/Linux_HAL_Essentials/pru/aiopru/Makefile
Normal file
8
Tools/Linux_HAL_Essentials/pru/aiopru/Makefile
Normal file
@ -0,0 +1,8 @@
|
||||
RcAioPRU: RcAioPRU.p
|
||||
pasm -V3 -c RcAioPRU.p
|
||||
|
||||
test: RcAioPRUTest.c
|
||||
gcc -g -o RcAioPRUTest RcAioPRUTest.c
|
||||
|
||||
clean:
|
||||
rm RcAioPRU_bin.h RcAioPRUTest
|
||||
385
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p
Normal file
385
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU.p
Normal file
@ -0,0 +1,385 @@
|
||||
// This program 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 program 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 <http://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
// RC AllInOnePRU
|
||||
//
|
||||
// 1 channel RCInput with 5ns accuracy
|
||||
// 12 channel RCOutput with 1us accuracy
|
||||
|
||||
// Timer
|
||||
#define TICK_PER_US 200
|
||||
#define TICK_PER_MS 200000
|
||||
|
||||
// PWM
|
||||
|
||||
// 920 us
|
||||
#define PWM_PULSE_DEFAULT (920 * TICK_PER_US)
|
||||
|
||||
// 50 Hz
|
||||
#define PWM_FREQ_DEFAULT (20 * TICK_PER_MS)
|
||||
|
||||
// Ringbuffer size
|
||||
#define RCIN_RINGBUFFERSIZE 300
|
||||
|
||||
// PRU Constants Table
|
||||
#define ECAP C3
|
||||
#define RAM C24
|
||||
#define IEP C26
|
||||
|
||||
// IEP
|
||||
#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
|
||||
#define ECAP_TSCTR 0x0
|
||||
#define ECAP_CTRPHS 0x4
|
||||
#define ECAP_CAP1 0x8
|
||||
#define ECAP_CAP2 0xc
|
||||
#define ECAP_CAP3 0x10
|
||||
#define ECAP_CAP4 0x14
|
||||
#define ECAP_ECCTL1 0x28
|
||||
#define ECAP_ECCTL2 0x2a
|
||||
#define ECAP_ECEINT 0x2c
|
||||
#define ECAP_ECFLG 0x2e
|
||||
#define ECAP_ECCLR 0x30
|
||||
#define ECAP_ECFRC 0x32
|
||||
#define ECAP_REVID 0x5c
|
||||
|
||||
// ECCTL1
|
||||
#define ECAP_CAP1POL 0
|
||||
#define ECAP_CTRRST1 1
|
||||
#define ECAP_CAP2POL 2
|
||||
#define ECAP_CTRRST2 3
|
||||
#define ECAP_CAP3POL 4
|
||||
#define ECAP_CTRRST3 5
|
||||
#define ECAP_CAP4POL 6
|
||||
#define ECAP_CTRRST4 7
|
||||
#define ECAP_CAPLDEN 8
|
||||
#define ECAP_PRESCALE 9
|
||||
#define ECAP_FREE_SOFT 14
|
||||
|
||||
// ECCTL2
|
||||
#define ECAP_CONT_ONESHT 0
|
||||
#define ECAP_STOP_WRAP 1
|
||||
#define ECAP_RE_ARM 3
|
||||
#define ECAP_TSCTRSTOP 4
|
||||
#define ECAP_SYNCI_EN 5
|
||||
#define ECAP_SYNCO_SEL 6
|
||||
#define ECAP_SWSYNC 8
|
||||
#define ECAP_CAP_APWM 9
|
||||
#define ECAP_APWMPOL 10
|
||||
|
||||
// ECEINT, ECFLG
|
||||
#define ECAP_INT 0
|
||||
#define ECAP_CEVT1 1
|
||||
#define ECAP_CEVT2 2
|
||||
#define ECAP_CEVT3 3
|
||||
#define ECAP_CEVT4 4
|
||||
#define ECAP_CNTOVF 5
|
||||
#define ECAP_PRDEQ 6
|
||||
#define ECAP_CMPEQ 7
|
||||
|
||||
// RAM
|
||||
#define CH_ENABLE_RAM_OFFSET (0 * 4)
|
||||
#define CH_1_PULSE_TIME_RAM_OFFSET (1 * 4)
|
||||
#define CH_1_T_TIME_RAM_OFFSET (2 * 4)
|
||||
#define CH_2_PULSE_TIME_RAM_OFFSET (3 * 4)
|
||||
#define CH_2_T_TIME_RAM_OFFSET (4 * 4)
|
||||
#define CH_3_PULSE_TIME_RAM_OFFSET (5 * 4)
|
||||
#define CH_3_T_TIME_RAM_OFFSET (6 * 4)
|
||||
#define CH_4_PULSE_TIME_RAM_OFFSET (7 * 4)
|
||||
#define CH_4_T_TIME_RAM_OFFSET (8 * 4)
|
||||
#define CH_5_PULSE_TIME_RAM_OFFSET (9 * 4)
|
||||
#define CH_5_T_TIME_RAM_OFFSET (10 * 4)
|
||||
#define CH_6_PULSE_TIME_RAM_OFFSET (11 * 4)
|
||||
#define CH_6_T_TIME_RAM_OFFSET (12 * 4)
|
||||
#define CH_7_PULSE_TIME_RAM_OFFSET (13 * 4)
|
||||
#define CH_7_T_TIME_RAM_OFFSET (14 * 4)
|
||||
#define CH_8_PULSE_TIME_RAM_OFFSET (15 * 4)
|
||||
#define CH_8_T_TIME_RAM_OFFSET (16 * 4)
|
||||
#define CH_9_PULSE_TIME_RAM_OFFSET (17 * 4)
|
||||
#define CH_9_T_TIME_RAM_OFFSET (18 * 4)
|
||||
#define CH_10_PULSE_TIME_RAM_OFFSET (19 * 4)
|
||||
#define CH_10_T_TIME_RAM_OFFSET (20 * 4)
|
||||
#define CH_11_PULSE_TIME_RAM_OFFSET (21 * 4)
|
||||
#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 MAX_CYCLE_TIME_OFFSET (25 * 4)
|
||||
|
||||
#define RCIN_RING_HEAD_OFFSET 0x1000
|
||||
#define RCIN_RING_TAIL_OFFSET 0x1002
|
||||
#define RCIN_RINGBUFFER_RAM_OFFSET 0x1004
|
||||
|
||||
// RCOut pins
|
||||
#define RC_CH_1_PIN r30.t10
|
||||
#define RC_CH_2_PIN r30.t8
|
||||
#define RC_CH_3_PIN r30.t11
|
||||
#define RC_CH_4_PIN r30.t9
|
||||
#define RC_CH_5_PIN r30.t7
|
||||
#define RC_CH_6_PIN r30.t6
|
||||
#define RC_CH_7_PIN r30.t5
|
||||
#define RC_CH_8_PIN r30.t4
|
||||
#define RC_CH_9_PIN r30.t3
|
||||
#define RC_CH_10_PIN r30.t2
|
||||
#define RC_CH_11_PIN r30.t1
|
||||
#define RC_CH_12_PIN r30.t0
|
||||
|
||||
// RCOut enable bits
|
||||
#define RC_CH_1_ENABLE register.ch_enable.t0
|
||||
#define RC_CH_2_ENABLE register.ch_enable.t1
|
||||
#define RC_CH_3_ENABLE register.ch_enable.t2
|
||||
#define RC_CH_4_ENABLE register.ch_enable.t3
|
||||
#define RC_CH_5_ENABLE register.ch_enable.t4
|
||||
#define RC_CH_6_ENABLE register.ch_enable.t5
|
||||
#define RC_CH_7_ENABLE register.ch_enable.t6
|
||||
#define RC_CH_8_ENABLE register.ch_enable.t7
|
||||
#define RC_CH_9_ENABLE register.ch_enable.t8
|
||||
#define RC_CH_10_ENABLE register.ch_enable.t9
|
||||
#define RC_CH_11_ENABLE register.ch_enable.t10
|
||||
#define RC_CH_12_ENABLE register.ch_enable.t11
|
||||
|
||||
// Register struct
|
||||
.struct RegisterStruct
|
||||
.u32 ch_enable
|
||||
.u32 ch_1_next_time
|
||||
.u32 ch_2_next_time
|
||||
.u32 ch_3_next_time
|
||||
.u32 ch_4_next_time
|
||||
.u32 ch_5_next_time
|
||||
.u32 ch_6_next_time
|
||||
.u32 ch_7_next_time
|
||||
.u32 ch_8_next_time
|
||||
.u32 ch_9_next_time
|
||||
.u32 ch_10_next_time
|
||||
.u32 ch_11_next_time
|
||||
.u32 ch_12_next_time
|
||||
.u32 time
|
||||
.u32 time_max
|
||||
.u32 time_cycle
|
||||
.u32 rcin_ram_pointer
|
||||
.u32 rcin_ram_pointer_index
|
||||
.u32 rcin_ram_pointer_index_max
|
||||
.u32 rcin_ram_pointer_head
|
||||
.u32 rcin_ram_pointer_tail
|
||||
.u32 temp
|
||||
.u32 temp1
|
||||
.u32 test
|
||||
.ends
|
||||
.assign RegisterStruct, R4, *, register
|
||||
|
||||
.macro RCOUT_PWM
|
||||
.mparam RC_CH_X_PIN, CH_X_NEXT_TIME, CH_X_ENABLE, CH_X_PULSE_TIME_RAM_OFFSET, CH_X_T_TIME_RAM_OFFSET
|
||||
pwm:
|
||||
// Handle arithmetic and counter overflow, check if there is something to do
|
||||
sub register.temp, CH_X_NEXT_TIME, register.time
|
||||
mov register.temp1, 0xF0000000
|
||||
qbgt pwmend, register.temp, register.temp1
|
||||
|
||||
mov CH_X_NEXT_TIME, register.time
|
||||
|
||||
// Set pin or clear pin?
|
||||
qbbs pwmclear, RC_CH_X_PIN
|
||||
|
||||
// Load pulse duration
|
||||
lbco register.temp, RAM, CH_X_PULSE_TIME_RAM_OFFSET, 4
|
||||
|
||||
// Calculate time to next event
|
||||
add CH_X_NEXT_TIME, CH_X_NEXT_TIME, register.temp
|
||||
|
||||
// Check if channel is enabled
|
||||
qbbc pwmend, CH_X_ENABLE
|
||||
|
||||
// Set pin
|
||||
set RC_CH_X_PIN
|
||||
jmp pwmend
|
||||
|
||||
pwmclear:
|
||||
// Load pulse time
|
||||
lbco register.temp1, RAM, CH_X_PULSE_TIME_RAM_OFFSET, 4
|
||||
|
||||
// Load T time
|
||||
lbco register.temp, RAM, CH_X_T_TIME_RAM_OFFSET, 4
|
||||
|
||||
// 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:
|
||||
.endm
|
||||
|
||||
.macro RCIN_ECAP_INIT
|
||||
// Initialize ECAP
|
||||
mov register.temp, (1 << ECAP_CTRRST1) | (1 << ECAP_CAP1POL) | (1 << ECAP_CTRRST2) | (1 << ECAP_CAPLDEN)
|
||||
sbco register.temp, ECAP, ECAP_ECCTL1, 4
|
||||
mov register.temp, (1 << ECAP_STOP_WRAP) | (1 << ECAP_TSCTRSTOP) | (2 << ECAP_SYNCO_SEL)
|
||||
sbco register.temp, ECAP, ECAP_ECCTL2, 4
|
||||
.endm
|
||||
|
||||
.macro RCIN_ECAP
|
||||
// New value?
|
||||
lbco register.temp, ECAP, ECAP_ECFLG, 4
|
||||
qbbc rcin_ecap_end, register.temp.t2
|
||||
|
||||
// Copy S0 and S1 duration to temp and temp1
|
||||
lbco register.temp, ECAP, ECAP_CAP1, 8
|
||||
|
||||
// Copy S0 and S1 duration to RAM
|
||||
sbbo register.temp, register.rcin_ram_pointer, 0, 8
|
||||
|
||||
// Clear event flags
|
||||
mov register.temp, (1 << ECAP_CEVT1) | (1 << ECAP_CEVT2)
|
||||
sbco register.temp, ECAP, ECAP_ECCLR, 4
|
||||
|
||||
// Set new tail value
|
||||
RCIN_WRITE_TAIL register.rcin_ram_pointer_index
|
||||
|
||||
// Update pointer
|
||||
add register.rcin_ram_pointer_index, register.rcin_ram_pointer_index, 1
|
||||
add register.rcin_ram_pointer, register.rcin_ram_pointer, 8
|
||||
|
||||
// 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_index, 0
|
||||
rcin_ecap_end:
|
||||
.endm
|
||||
|
||||
.macro RCIN_WRITE_HEAD
|
||||
.mparam RCIN_HEAD
|
||||
sbbo RCIN_HEAD, register.rcin_ram_pointer_head, 0, 2
|
||||
.endm
|
||||
|
||||
.macro RCIN_WRITE_TAIL
|
||||
.mparam RCIN_TAIL
|
||||
sbbo RCIN_TAIL, register.rcin_ram_pointer_tail, 0, 2
|
||||
.endm
|
||||
|
||||
.macro MAX_CYCLE_TIME
|
||||
lbco register.temp1, IEP, IEP_TMR_CNT, 4
|
||||
sub register.temp, register.temp1, register.time_cycle
|
||||
mov register.time_cycle, register.temp1
|
||||
max register.time_max, register.time_max, register.temp
|
||||
sbco register.time_max, RAM, MAX_CYCLE_TIME_OFFSET, 4
|
||||
.endm
|
||||
|
||||
.macro INIT
|
||||
// Reset PWM pins
|
||||
mov r30, 0x0
|
||||
|
||||
// Clear register
|
||||
zero ®ister, SIZE(register)
|
||||
|
||||
// Initialize max cycle time
|
||||
mov register.temp, 0
|
||||
sbco register.temp, RAM, MAX_CYCLE_TIME_OFFSET, 4
|
||||
|
||||
// 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_head, RCIN_RING_HEAD_OFFSET
|
||||
mov register.rcin_ram_pointer_tail, RCIN_RING_TAIL_OFFSET
|
||||
mov register.temp, 0
|
||||
RCIN_WRITE_HEAD register.temp
|
||||
RCIN_WRITE_TAIL register.temp
|
||||
|
||||
// Load balancing
|
||||
mov register.ch_1_next_time, 1000
|
||||
mov register.ch_2_next_time, 2000
|
||||
mov register.ch_3_next_time, 3000
|
||||
mov register.ch_4_next_time, 4000
|
||||
mov register.ch_5_next_time, 5000
|
||||
mov register.ch_6_next_time, 6000
|
||||
mov register.ch_7_next_time, 7000
|
||||
mov register.ch_8_next_time, 8000
|
||||
mov register.ch_9_next_time, 9000
|
||||
mov register.ch_10_next_time, 10000
|
||||
mov register.ch_11_next_time, 11000
|
||||
mov register.ch_12_next_time, 12000
|
||||
|
||||
// Disable all PWMs
|
||||
mov register.ch_enable, 0x0
|
||||
sbco register.ch_enable, RAM, CH_ENABLE_RAM_OFFSET, 4
|
||||
|
||||
// Initialize PWM pulse (920us)
|
||||
mov register.temp, PWM_PULSE_DEFAULT
|
||||
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
|
||||
sbco register.temp, RAM, CH_5_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_6_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_7_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_8_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_9_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_10_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_11_PULSE_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_12_PULSE_TIME_RAM_OFFSET, 4
|
||||
|
||||
// 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_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
|
||||
sbco register.temp, RAM, CH_5_T_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_6_T_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_7_T_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_8_T_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_9_T_TIME_RAM_OFFSET, 4
|
||||
sbco register.temp, RAM, CH_10_T_TIME_RAM_OFFSET, 4
|
||||
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
|
||||
sbco register.temp, IEP, IEP_TMR_GLB_CFG, 4
|
||||
|
||||
// Reset counter
|
||||
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
|
||||
.endm
|
||||
|
||||
.origin 0
|
||||
init:
|
||||
INIT
|
||||
RCIN_ECAP_INIT
|
||||
mainloop:
|
||||
lbco register.ch_enable, RAM, CH_ENABLE_RAM_OFFSET, 4
|
||||
lbco register.time, IEP, IEP_TMR_CNT, 4
|
||||
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
|
||||
RCOUT_PWM RC_CH_4_PIN, register.ch_4_next_time, RC_CH_4_ENABLE, CH_4_PULSE_TIME_RAM_OFFSET, CH_4_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_5_PIN, register.ch_5_next_time, RC_CH_5_ENABLE, CH_5_PULSE_TIME_RAM_OFFSET, CH_5_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_6_PIN, register.ch_6_next_time, RC_CH_6_ENABLE, CH_6_PULSE_TIME_RAM_OFFSET, CH_6_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_7_PIN, register.ch_7_next_time, RC_CH_7_ENABLE, CH_7_PULSE_TIME_RAM_OFFSET, CH_7_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_8_PIN, register.ch_8_next_time, RC_CH_8_ENABLE, CH_8_PULSE_TIME_RAM_OFFSET, CH_8_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_9_PIN, register.ch_9_next_time, RC_CH_9_ENABLE, CH_9_PULSE_TIME_RAM_OFFSET, CH_9_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_10_PIN, register.ch_10_next_time, RC_CH_10_ENABLE, CH_10_PULSE_TIME_RAM_OFFSET, CH_10_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_11_PIN, register.ch_11_next_time, RC_CH_11_ENABLE, CH_11_PULSE_TIME_RAM_OFFSET, CH_11_T_TIME_RAM_OFFSET
|
||||
RCOUT_PWM RC_CH_12_PIN, register.ch_12_next_time, RC_CH_12_ENABLE, CH_12_PULSE_TIME_RAM_OFFSET, CH_12_T_TIME_RAM_OFFSET
|
||||
RCIN_ECAP
|
||||
// MAX_CYCLE_TIME
|
||||
jmp mainloop
|
||||
158
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c
Normal file
158
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRUTest.c
Normal file
@ -0,0 +1,158 @@
|
||||
// This program 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 program 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 <http://www.gnu.org/licenses/>.
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mman.h>
|
||||
|
||||
#include "RcAioPRU_bin.h"
|
||||
|
||||
#define NUM_RING_ENTRIES 300
|
||||
#define RCOUT_PRUSS_RAM_BASE 0x4a302000
|
||||
#define RCOUT_PRUSS_CTRL_BASE 0x4a324000
|
||||
#define RCOUT_PRUSS_IRAM_BASE 0x4a338000
|
||||
#define RCIN_PRUSS_RAM_BASE 0x4a303000
|
||||
|
||||
struct ring_buffer {
|
||||
volatile uint16_t ring_head;
|
||||
volatile uint16_t ring_tail;
|
||||
struct {
|
||||
uint32_t s1;
|
||||
uint32_t s0;
|
||||
} buffer[NUM_RING_ENTRIES];
|
||||
};
|
||||
|
||||
struct pwm {
|
||||
volatile uint32_t enable;
|
||||
volatile uint32_t ch1_hi_time;
|
||||
volatile uint32_t ch1_t_time;
|
||||
volatile uint32_t ch2_hi_time;
|
||||
volatile uint32_t ch2_t_time;
|
||||
volatile uint32_t ch3_hi_time;
|
||||
volatile uint32_t ch3_t_time;
|
||||
volatile uint32_t ch4_hi_time;
|
||||
volatile uint32_t ch4_t_time;
|
||||
volatile uint32_t ch5_hi_time;
|
||||
volatile uint32_t ch5_t_time;
|
||||
volatile uint32_t ch6_hi_time;
|
||||
volatile uint32_t ch6_t_time;
|
||||
volatile uint32_t ch7_hi_time;
|
||||
volatile uint32_t ch7_t_time;
|
||||
volatile uint32_t ch8_hi_time;
|
||||
volatile uint32_t ch8_t_time;
|
||||
volatile uint32_t ch9_hi_time;
|
||||
volatile uint32_t ch9_t_time;
|
||||
volatile uint32_t ch10_hi_time;
|
||||
volatile uint32_t ch10_t_time;
|
||||
volatile uint32_t ch11_hi_time;
|
||||
volatile uint32_t ch11_t_time;
|
||||
volatile uint32_t ch12_hi_time;
|
||||
volatile uint32_t ch12_t_time;
|
||||
volatile uint32_t max_cycle_time;
|
||||
};
|
||||
|
||||
volatile struct ring_buffer *ring_buffer;
|
||||
volatile struct pwm *pwm;
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
close(mem_fd);
|
||||
|
||||
// Reset PRU 1
|
||||
*ctrl = 0;
|
||||
usleep(5 * 1000);
|
||||
|
||||
// Load firmware
|
||||
for(i = 0; i < sizeof(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;
|
||||
|
||||
while(1) {
|
||||
for(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;}
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
280
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_bin.h
Normal file
280
Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_bin.h
Normal file
@ -0,0 +1,280 @@
|
||||
|
||||
|
||||
/* This file contains the PRU instructions in a C array which are to */
|
||||
/* be downloaded from the host CPU to the PRU instruction memory. */
|
||||
/* This file is generated by the PRU assembler. */
|
||||
|
||||
const unsigned int PRUcode[] = {
|
||||
0x240000fe,
|
||||
0x2effaf84,
|
||||
0x240000f9,
|
||||
0x81643899,
|
||||
0x241004f4,
|
||||
0x24012cf6,
|
||||
0x241000f7,
|
||||
0x241002f8,
|
||||
0x240000f9,
|
||||
0xe1001799,
|
||||
0xe1001899,
|
||||
0x2403e8e5,
|
||||
0x2407d0e6,
|
||||
0x240bb8e7,
|
||||
0x240fa0e8,
|
||||
0x241388e9,
|
||||
0x241770ea,
|
||||
0x241b58eb,
|
||||
0x241f40ec,
|
||||
0x242328ed,
|
||||
0x242710ee,
|
||||
0x242af8ef,
|
||||
0x242ee0f0,
|
||||
0x240000e4,
|
||||
0x81003884,
|
||||
0x240002d9,
|
||||
0x24cec099,
|
||||
0x81043899,
|
||||
0x810c3899,
|
||||
0x81143899,
|
||||
0x811c3899,
|
||||
0x81243899,
|
||||
0x812c3899,
|
||||
0x81343899,
|
||||
0x813c3899,
|
||||
0x81443899,
|
||||
0x814c3899,
|
||||
0x81543899,
|
||||
0x815c3899,
|
||||
0x24003dd9,
|
||||
0x24090099,
|
||||
0x81083899,
|
||||
0x81103899,
|
||||
0x81183899,
|
||||
0x81203899,
|
||||
0x81283899,
|
||||
0x81303899,
|
||||
0x81383899,
|
||||
0x81403899,
|
||||
0x81483899,
|
||||
0x81503899,
|
||||
0x81583899,
|
||||
0x81603899,
|
||||
0x240010f9,
|
||||
0x81003a99,
|
||||
0x24ffffd9,
|
||||
0x24ffff99,
|
||||
0x810c3a99,
|
||||
0x91003a99,
|
||||
0x1301f9f9,
|
||||
0x81003a99,
|
||||
0x24010bf9,
|
||||
0x81282399,
|
||||
0x240092f9,
|
||||
0x812a2399,
|
||||
0x91003884,
|
||||
0x910c3a91,
|
||||
0x04f1e5f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1e5,
|
||||
0xd10afe06,
|
||||
0x91043899,
|
||||
0x00f9e5e5,
|
||||
0xc900e408,
|
||||
0x1f0afefe,
|
||||
0x21005300,
|
||||
0x9104389a,
|
||||
0x91083899,
|
||||
0x04faf9f9,
|
||||
0x00f9e5e5,
|
||||
0x1d0afefe,
|
||||
0x04f1e6f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1e6,
|
||||
0xd108fe06,
|
||||
0x910c3899,
|
||||
0x00f9e6e6,
|
||||
0xc901e408,
|
||||
0x1f08fefe,
|
||||
0x21006300,
|
||||
0x910c389a,
|
||||
0x91103899,
|
||||
0x04faf9f9,
|
||||
0x00f9e6e6,
|
||||
0x1d08fefe,
|
||||
0x04f1e7f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1e7,
|
||||
0xd10bfe06,
|
||||
0x91143899,
|
||||
0x00f9e7e7,
|
||||
0xc902e408,
|
||||
0x1f0bfefe,
|
||||
0x21007300,
|
||||
0x9114389a,
|
||||
0x91183899,
|
||||
0x04faf9f9,
|
||||
0x00f9e7e7,
|
||||
0x1d0bfefe,
|
||||
0x04f1e8f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1e8,
|
||||
0xd109fe06,
|
||||
0x911c3899,
|
||||
0x00f9e8e8,
|
||||
0xc903e408,
|
||||
0x1f09fefe,
|
||||
0x21008300,
|
||||
0x911c389a,
|
||||
0x91203899,
|
||||
0x04faf9f9,
|
||||
0x00f9e8e8,
|
||||
0x1d09fefe,
|
||||
0x04f1e9f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1e9,
|
||||
0xd107fe06,
|
||||
0x91243899,
|
||||
0x00f9e9e9,
|
||||
0xc904e408,
|
||||
0x1f07fefe,
|
||||
0x21009300,
|
||||
0x9124389a,
|
||||
0x91283899,
|
||||
0x04faf9f9,
|
||||
0x00f9e9e9,
|
||||
0x1d07fefe,
|
||||
0x04f1eaf9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1ea,
|
||||
0xd106fe06,
|
||||
0x912c3899,
|
||||
0x00f9eaea,
|
||||
0xc905e408,
|
||||
0x1f06fefe,
|
||||
0x2100a300,
|
||||
0x912c389a,
|
||||
0x91303899,
|
||||
0x04faf9f9,
|
||||
0x00f9eaea,
|
||||
0x1d06fefe,
|
||||
0x04f1ebf9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1eb,
|
||||
0xd105fe06,
|
||||
0x91343899,
|
||||
0x00f9ebeb,
|
||||
0xc906e408,
|
||||
0x1f05fefe,
|
||||
0x2100b300,
|
||||
0x9134389a,
|
||||
0x91383899,
|
||||
0x04faf9f9,
|
||||
0x00f9ebeb,
|
||||
0x1d05fefe,
|
||||
0x04f1ecf9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1ec,
|
||||
0xd104fe06,
|
||||
0x913c3899,
|
||||
0x00f9ecec,
|
||||
0xc907e408,
|
||||
0x1f04fefe,
|
||||
0x2100c300,
|
||||
0x913c389a,
|
||||
0x91403899,
|
||||
0x04faf9f9,
|
||||
0x00f9ecec,
|
||||
0x1d04fefe,
|
||||
0x04f1edf9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1ed,
|
||||
0xd103fe06,
|
||||
0x91443899,
|
||||
0x00f9eded,
|
||||
0xc908e408,
|
||||
0x1f03fefe,
|
||||
0x2100d300,
|
||||
0x9144389a,
|
||||
0x91483899,
|
||||
0x04faf9f9,
|
||||
0x00f9eded,
|
||||
0x1d03fefe,
|
||||
0x04f1eef9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1ee,
|
||||
0xd102fe06,
|
||||
0x914c3899,
|
||||
0x00f9eeee,
|
||||
0xc909e408,
|
||||
0x1f02fefe,
|
||||
0x2100e300,
|
||||
0x914c389a,
|
||||
0x91503899,
|
||||
0x04faf9f9,
|
||||
0x00f9eeee,
|
||||
0x1d02fefe,
|
||||
0x04f1eff9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1ef,
|
||||
0xd101fe06,
|
||||
0x91543899,
|
||||
0x00f9efef,
|
||||
0xc90ae408,
|
||||
0x1f01fefe,
|
||||
0x2100f300,
|
||||
0x9154389a,
|
||||
0x91583899,
|
||||
0x04faf9f9,
|
||||
0x00f9efef,
|
||||
0x1d01fefe,
|
||||
0x04f1f0f9,
|
||||
0x24f000da,
|
||||
0x2400009a,
|
||||
0x60faf90d,
|
||||
0x10f1f1f0,
|
||||
0xd100fe06,
|
||||
0x915c3899,
|
||||
0x00f9f0f0,
|
||||
0xc90be408,
|
||||
0x1f00fefe,
|
||||
0x21010300,
|
||||
0x915c389a,
|
||||
0x91603899,
|
||||
0x04faf9f9,
|
||||
0x00f9f0f0,
|
||||
0x1d00fefe,
|
||||
0x912e2399,
|
||||
0xc902f90b,
|
||||
0x91086399,
|
||||
0xe1007499,
|
||||
0x240006f9,
|
||||
0x81302399,
|
||||
0xe1001895,
|
||||
0x0101f5f5,
|
||||
0x0108f4f4,
|
||||
0x48f5f603,
|
||||
0x241004f4,
|
||||
0x240000f5,
|
||||
0x21004100 };
|
||||
|
||||
Loading…
Reference in New Issue
Block a user