mirror of https://github.com/ArduPilot/ardupilot
HAL_Linux: Add initial RCOutput driver
The functions in RCOutput.cpp are defined to access PWM over PRU sysfs device from userspace using open, read, write and close methods.
This commit is contained in:
bugobliterator
Andrew Tridgell
parent
3354fa9fc8
commit
a7de5e3d0f
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,310 @@
|
|||
/*
|
||||
* Copyright (C) 2013 Pantelis Antoniou <panto@antoniou-consulting.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*/
|
||||
/dts-v1/;
|
||||
/plugin/;
|
||||
|
||||
/ {
|
||||
compatible = "ti,beaglebone", "ti,beaglebone-black";
|
||||
|
||||
/* identification */
|
||||
part-number = "BB-BONE-PRU-04";
|
||||
version = "00A0";
|
||||
|
||||
/* state the resources this cape uses */
|
||||
exclusive-use =
|
||||
/* the pin header uses */
|
||||
"P9.27", /* pru0: pr1_pru0_pru_r30_5 */
|
||||
|
||||
"P8.11", /* pru0: pr1_pru0_pru_r30_15 */
|
||||
"P8.12", /* pru0: pr1_pru0_pru_r30_14 */
|
||||
"P9.25", /* pru0: pr1_pru0_pru_r30_7 */
|
||||
"P9.41", /* pru0: pr1_pru0_pru_r30_6 */
|
||||
"P9.42", /* pru0: pr1_pru0_pru_r30_4 */
|
||||
"P9.28", /* pru0: pr1_pru0_pru_r30_3 */
|
||||
"P9.30", /* pru0: pr1_pru0_pru_r30_2 */
|
||||
"P9.29", /* pru0: pr1_pru0_pru_r30_1 */
|
||||
"P9.31", /* pru0: pr1_pru0_pru_r30_0 */
|
||||
/* pru0: pr1_pru0_pru_r30_13 is on MMC0_CMD */
|
||||
/* pru0: pr1_pru0_pru_r30_12 is on MMC0_CLK */
|
||||
/* pru0: pr1_pru0_pru_r30_11 is on MMC0_DAT0 */
|
||||
/* pru0: pr1_pru0_pru_r30_10 is on MMC0_DAT1 */
|
||||
/* pru0: pr1_pru0_pru_r30_9 is on MMC0_DAT2 */
|
||||
/* pru0: pr1_pru0_pru_r30_8 is on MMC0_DAT3 */
|
||||
|
||||
//"P8.20", /* pru1: pr1_pru1_pru_r30_13 */
|
||||
|
||||
//"P8.21", /* pru1: pr1_pru1_pru_r30_12 */
|
||||
"P8.27", /* pru1: pr1_pru1_pru_r30_8 */
|
||||
"P8.28", /* pru1: pr1_pru1_pru_r30_10 */
|
||||
"P8.29", /* pru1: pr1_pru1_pru_r30_9 */
|
||||
"P8.30", /* pru1: pr1_pru1_pru_r30_11 */
|
||||
"P8.39", /* pru1: pr1_pru1_pru_r30_6 */
|
||||
"P8.40", /* pru1: pr1_pru1_pru_r30_7 */
|
||||
"P8.41", /* pru1: pr1_pru1_pru_r30_4 */
|
||||
"P8.42", /* pru1: pr1_pru1_pru_r30_5 */
|
||||
"P8.43", /* pru1: pr1_pru1_pru_r30_2 */
|
||||
"P8.44", /* pru1: pr1_pru1_pru_r30_3 */
|
||||
"P8.45", /* pru1: pr1_pru1_pru_r30_0 */
|
||||
"P8.46", /* pru1: pr1_pru1_pru_r30_1 */
|
||||
/* pru1: pr1_pru1_pru_r30_14 is on UART0_RXD */
|
||||
/* pru1: pr1_pru1_pru_r30_15 is on UART0_TXD */
|
||||
/* the hardware IP uses */
|
||||
"pru0",
|
||||
"pru1";
|
||||
|
||||
fragment@0 {
|
||||
target = <&am33xx_pinmux>;
|
||||
__overlay__ {
|
||||
|
||||
pru_gpio_pins: pinmux_pru_gpio_pins {
|
||||
pinctrl-single,pins = <
|
||||
0x1a4 0x0f /* P9 27 GPIO3_19: mcasp0_fsr.gpio3[19] | MODE7 | OUTPUT */
|
||||
>;
|
||||
};
|
||||
|
||||
pru_pru_pins: pinmux_pru_pru_pins {
|
||||
pinctrl-single,pins = <
|
||||
0x1a4 0x25 /* mcasp0_fsr.pr1_pru0_pru_r30_5, MODE5 | OUTPUT | PRU */
|
||||
0x034 0x26 /* gpmc_ad13.pr1_pru0_pru_r30_15, MODE6 | OUTPUT | PRU */
|
||||
0x030 0x26 /* gpmc_ad12.pr1_pru0_pru_r30_14, MODE6 | OUTPUT | PRU */
|
||||
0x1ac 0x25 /* mcasp0_ahclkx.pr1_pru0_pru_r30_7, MODE5 | OUTPUT | PRU */
|
||||
0x1a8 0x25 /* mcasp0_axr1.pr1_pru0_pru_r30_6, MODE5 | OUTPUT | PRU */
|
||||
0x1a0 0x25 /* mcasp0_aclkr.pr1_pru0_pru_r30_4, MODE5 | OUTPUT | PRU */
|
||||
0x19c 0x25 /* mcasp0_ahclkr.pr1_pru0_pru_r30_3, MODE5 | OUTPUT | PRU */
|
||||
0x198 0x25 /* mcasp0_axr0.pr1_pru0_pru_r30_2, MODE5 | OUTPUT | PRU */
|
||||
0x194 0x25 /* mcasp0_fsx.pr1_pru0_pru_r30_1, MODE5 | OUTPUT | PRU */
|
||||
0x190 0x25 /* mcasp0_aclkx.pr1_pru0_pru_r30_0, MODE5 | OUTPUT | PRU */
|
||||
|
||||
//0x084 0x25 /* gpmc_csn2.pr1_pru1_pru_r30_13, MODE5 | OUTPUT | PRU */
|
||||
//0x080 0x25 /* gpmc_csn1.pr1_pru1_pru_r30_12, MODE5 | OUTPUT | PRU */
|
||||
0x0e0 0x25 /* lcd_vsync.pr1_pru1_pru_r30_8, MODE5 | OUTPUT | PRU */
|
||||
0x0e8 0x25 /* lcd_pclk.pr1_pru1_pru_r30_10, MODE5 | OUTPUT | PRU */
|
||||
0x0e4 0x25 /* lcd_hsync.pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU */
|
||||
0x0ec 0x25 /* lcd_ac_bias_en.pr1_pru1_pru_r30_11, MODE5 | OUTPUT | PRU */
|
||||
0x0b8 0x25 /* pr1_pru1_pru_r30_6, MODE5 | OUTPUT | PRU */
|
||||
0x0bc 0x25 /* lcd_data7.pr1_pru1_pru_r30_7, MODE5 | OUTPUT | PRU */
|
||||
0x0b0 0x25 /* lcd_data4.pr1_pru1_pru_r30_4, MODE5 | OUTPUT | PRU */
|
||||
0x0b4 0x25 /* lcd_data5.pr1_pru1_pru_r30_5, MODE5 | OUTPUT | PRU */
|
||||
0x0a8 0x25 /* pr1_pru1_pru_r31_2, MODE5 | OUTPUT | PRU */
|
||||
0x0ac 0x25 /* lcd_data3.pr1_pru1_pru_r30_3, MODE5 | OUTPUT | PRU */
|
||||
0x0a0 0x25 /* lcd_data0.pr1_pru1_pru_r30_0, MODE5 | OUTPUT | PRU */
|
||||
0x0a4 0x25 /* lcd_data1.pr1_pru1_pru_r30_1, MODE5 | OUTPUT | PRU */
|
||||
>;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
fragment@2 {
|
||||
target = <&ocp>;
|
||||
|
||||
__overlay__ {
|
||||
|
||||
/* avoid stupid warning */
|
||||
#address-cells = <1>;
|
||||
#size-cells = <1>;
|
||||
|
||||
prurproc {
|
||||
compatible = "ti,pru-rproc";
|
||||
|
||||
pinctrl-names = "default";
|
||||
pinctrl-0 = <&pru_pru_pins>;
|
||||
|
||||
reg = <0x4a300000 0x080000>;
|
||||
|
||||
status = "okay";
|
||||
|
||||
ti,hwmods = "pruss";
|
||||
ti,deassert-hard-reset = "pruss", "pruss";
|
||||
interrupt-parent = <&intc>;
|
||||
|
||||
/* interrupts on the host */
|
||||
interrupts = <20 21 22 23 24 25 26 27>;
|
||||
|
||||
/* events these interrupts map to (host interrupt) */
|
||||
events = <2 3 4 5 6 7 8 9>;
|
||||
|
||||
/* PRU interrupt controller offset */
|
||||
pintc = <0x20000>;
|
||||
|
||||
/* 12K Shared Data RAM global, size, local */
|
||||
pdram = <0x10000 0x03000 0x10000>;
|
||||
|
||||
/*
|
||||
* SYSEVENT ids
|
||||
*
|
||||
* - PRU/ARM communication
|
||||
* PRU0_PRU1 17
|
||||
* PRU1_PRU0 18
|
||||
* PRU0_ARM 19
|
||||
* PRU1_ARM 20
|
||||
* ARM_PRU0 21
|
||||
* ARM_PRU1 22
|
||||
*
|
||||
* Full SYSEVENT list
|
||||
*
|
||||
* parity_err_intr_pend 0
|
||||
* pru0_r31_status_cnt16 1
|
||||
* pru1_r31_status_cnt16 2
|
||||
* uart_urxevt_intr_req 4
|
||||
* uart_utxevt_intr_req 5
|
||||
* uart_uint_intr_req 6
|
||||
* iep_tim_cap_cmp_pend 7
|
||||
* ecap_intr_req 15
|
||||
* pru_mst_intr[0-15]_intr_req 16-31
|
||||
* nirq 32 (UART1)
|
||||
* mcasp_x_intr_pend 33 (MCASP1)
|
||||
* mcasp_r_intr_pend 34 (MCASP1)
|
||||
* ecap_intr_intr_pend 35 (ECAP1)
|
||||
* ecap_intr_intr_pend 36 (ECAP2)
|
||||
* epwm_intr_intr_pend 37 (eHRPWM2)
|
||||
* dcan_uerr 38 (DCAN0)
|
||||
* dcan_int1 39 (DCAN0)
|
||||
* dcan_intr 40 (DCAN0)
|
||||
* POINTRPEND 41 (I2C0)
|
||||
* ecap_intr_intr_pend 42 (ECAP0)
|
||||
* epwm_intr_intr_pend 43 (eHRPWM0)
|
||||
* SINTERRUPTN 44 (McSPI0)
|
||||
* eqep_intr_intr_pend 45 (eQEP0)
|
||||
* epwm_intr_intr_pend 46 (eHRPWM1)
|
||||
* c0_misc_pend 47 3PGSW (GEMAC)
|
||||
* c0_tx_pend 48 3PGSW (GEMAC)
|
||||
* c0_rx_pend 49 3PGSW (GEMAC)
|
||||
* c0_rx_thresh_pend 50 3PGSW (GEMAC)
|
||||
* nirq 51 (UART0)
|
||||
* nirq 52 (UART2)
|
||||
* gen_intr_pend 53 (ADC_TSC)
|
||||
* mcasp_r_intr_pend 54 (McASP0)
|
||||
* mcasp_x_intr_pend 55 (McASP1)
|
||||
* pwm_trip_zone 56 (eHRPWM0/eHRPWM1/eHRP WM2)
|
||||
* POINTRPEND1 57 (GPIO0)
|
||||
* Emulation Suspend Signal 58 (Debugss)
|
||||
* initiator_sinterrupt_q_n2 59 (Mbox0 - mail_u2_irq (mailbox interrupt for pru1))
|
||||
* initiator_sinterrupt_q_n1 60 (Mbox0 - mail_u1_irq (mailbox interrupt for pru0))
|
||||
* tptc_erint_pend_po 61 (TPTC0 (EDMA))
|
||||
* tpcc_errint_pend_po 62 (TPCC (EDMA))
|
||||
* tpcc_int_pend_po1 63 (TPCC (EDMA))
|
||||
*
|
||||
* HOST interrupt ids
|
||||
*
|
||||
* PRU0 0
|
||||
* PRU1 1
|
||||
* EVTOUT0-7 2-9
|
||||
*/
|
||||
|
||||
/* sysevent map to intc channel */
|
||||
sysevent-to-channel-map =
|
||||
<17 1>, /* PRU0_PRU1 -> CH1 */
|
||||
<18 0>, /* PRU1_PRU0 -> CH0 */
|
||||
<19 2>, /* PRU0_ARM -> CH2 */
|
||||
<20 3>, /* PRU1_ARM -> CH3 */
|
||||
<21 0>, /* ARM_PRU0 -> CH0 */
|
||||
<22 1>, /* ARM_PRU1 -> CH1 */
|
||||
<24 4>, /* VRING Host->PRU0 -> CH4 */
|
||||
<25 5>, /* VRING PRU0->Host -> CH5 */
|
||||
<26 6>, /* VRING Host->PRU1 -> CH6 */
|
||||
<27 7>; /* VRING PRU1->Host -> CH7 */
|
||||
|
||||
/* channel to host interrupt map */
|
||||
channel-to-host-interrupt-map =
|
||||
<0 0>, /* CH0 -> PRU0 */
|
||||
<1 1>, /* CH1 -> PRU1 */
|
||||
<2 2>, /* CH2 -> EVTOUT0 */
|
||||
<3 3>, /* CH3 -> EVTOUT1 */
|
||||
<4 0>, /* CH4 -> PRU0 */
|
||||
<5 6>, /* CH5 -> EVTOUT4 */
|
||||
<6 1>, /* CH6 -> PRU1 */
|
||||
<7 7>; /* CH7 -> EVTOUT5 */
|
||||
|
||||
/* indices are ARM=0, PRU0=1, PRU1=2 */
|
||||
target-to-sysevent-map =
|
||||
<0xffffffff 21 22>, /* ARM: DONTCARE, ARM_PRU0, ARM_PRU1 */
|
||||
< 19 0xffffffff 17>, /* PRU0: PRU0_ARM, DONTCARE, PRU0_PRU1 */
|
||||
< 20 18 0xffffffff>; /* PRU1: PRU1_ARM, PRU1_PRU0, DONTCARE */
|
||||
|
||||
/* both the PRUs have 200MHz frequency so period is 5ns */
|
||||
clock-freq = <200000000>;
|
||||
|
||||
/* the linux pwms we support */
|
||||
pru-pwm-channels = <0 1 2 3 4 5 6 7 8 9 10 11 12 13 21>;
|
||||
/* first PRU controls the PWMs */
|
||||
pru-pwm-controller = <0>;
|
||||
/* the IDs of the downcalls the firmware expects for PWMs */
|
||||
/* CONFIG=0, ENABLE=1, DISABLE=2 */
|
||||
pru-pwm-dc-ids = <0 1 2>;
|
||||
|
||||
/* definition for the first PRU */
|
||||
pru0 {
|
||||
pru-index = <0>;
|
||||
|
||||
/* offset, size, local */
|
||||
iram = <0x34000 0x02000 0x00000>; /* code ram (8K) */
|
||||
|
||||
/* offset, size, local, other */
|
||||
dram = <0x00000 0x02000 0x00000 0x10000>; /* data ram (8K) */
|
||||
|
||||
pctrl = <0x22000>;
|
||||
pdbg = <0x22400>;
|
||||
|
||||
firmware-elf;
|
||||
firmware = "testpru0";
|
||||
|
||||
/* sysevents signaling ring activity (host, pru)*/
|
||||
vring-sysev = <24 25>;
|
||||
|
||||
resource_table {
|
||||
resource-table;
|
||||
version = <1>;
|
||||
|
||||
pru0_rproc_serial: pru0_vdev_rproc_serial {
|
||||
vdev-rproc-serial;
|
||||
|
||||
/* notification IDs are totally bogus */
|
||||
/* rproc will idr_alloc anyway */
|
||||
|
||||
notifyid = <8>; /* <- bogus */
|
||||
/* da align num notifyid */
|
||||
vring-0 = <0 16 8 0>;
|
||||
vring-1 = <0 16 8 0>;
|
||||
};
|
||||
|
||||
// pru0_rproc_rpmsg: pru0_vdev_rproc_rpmsg {
|
||||
// vdev-rpmsg;
|
||||
//
|
||||
// /* notification IDs are totally bogus */
|
||||
// /* rproc will idr_alloc anyway */
|
||||
//
|
||||
// notifyid = <9>; /* <- bogus */
|
||||
// /* da align num notifyid */
|
||||
// vring-0 = <0 16 512 0>;
|
||||
// vring-1 = <0 16 512 0>;
|
||||
// };
|
||||
};
|
||||
};
|
||||
|
||||
/* definition for the second PRU */
|
||||
pru1 {
|
||||
pru-index = <1>;
|
||||
|
||||
/* offset, size, local */
|
||||
iram = <0x38000 0x02000 0x00000>; /* code ram (8K) */
|
||||
|
||||
/* offset, size, local, other */
|
||||
dram = <0x02000 0x02000 0x00000 0x10000>; /* data ram (8K) */
|
||||
|
||||
pctrl = <0x24000>;
|
||||
pdbg = <0x24400>;
|
||||
|
||||
firmware-elf;
|
||||
firmware = "testpru1";
|
||||
|
||||
/* NOTE: no resource table, no vrings for this one */
|
||||
};
|
||||
};
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -1,36 +1,236 @@
|
|||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_ERLE
|
||||
|
||||
#include "RCOutput.h"
|
||||
|
||||
using namespace Linux;
|
||||
using namespace std;
|
||||
|
||||
void LinuxRCOutput::init(void* machtnichts) {}
|
||||
#define PWM_DIR "/sys/class/pwm"
|
||||
#define PWM_DIR_EXPORT "/sys/class/pwm/export"
|
||||
#define PWM_CHAN_COUNT 12
|
||||
|
||||
void LinuxRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {}
|
||||
int chan_pru_map[]= {10,8,11,9,7,6,5,4,3,2,1,0}; //chan_pru_map[CHANNEL_NUM] = PRU_REG_R30/31_NUM;
|
||||
int pru_chan_map[]= {11,10,9,8,7,6,5,4,1,3,0,2}; //pru_chan_map[PRU_REG_R30/31_NUM] = CHANNEL_NUM;
|
||||
|
||||
uint16_t LinuxRCOutput::get_freq(uint8_t ch) {
|
||||
return 50;
|
||||
void LinuxRCOutput::init()
|
||||
{
|
||||
DIR* _pwm_dir = opendir(PWM_DIR);
|
||||
int _pwm_fd;
|
||||
char chan[2];
|
||||
struct dirent *dir;
|
||||
int pru_load_success=0;
|
||||
int exported_count=0;
|
||||
int i,pru_r30;
|
||||
|
||||
if (_pwm_dir){
|
||||
while ((dir = readdir(_pwm_dir)) != NULL){
|
||||
if(strcmp(dir->d_name,"pwmchip0")==0){
|
||||
pru_load_success = 1;
|
||||
closedir(_pwm_dir);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(!pru_load_success){
|
||||
perror("PRU PWM LOAD failed: PRU might not be enabled");
|
||||
}
|
||||
|
||||
}
|
||||
else{
|
||||
perror("PWM DIR not found");
|
||||
}
|
||||
|
||||
_pwm_fd = open(PWM_DIR_EXPORT,O_WRONLY);
|
||||
|
||||
if(_pwm_fd < 0){
|
||||
perror("PWM_EXPORT not found");
|
||||
}
|
||||
|
||||
for(i=0;i<PWM_CHAN_COUNT;i++){
|
||||
sprintf(chan, "%d",chan_pru_map[i]);
|
||||
write(_pwm_fd,&chan,sizeof(chan));
|
||||
}
|
||||
|
||||
close(_pwm_fd);
|
||||
_pwm_dir = opendir(PWM_DIR);
|
||||
|
||||
while ((dir = readdir(_pwm_dir)) != NULL){
|
||||
pru_r30=strtol(dir->d_name+3,NULL,10);
|
||||
if(pru_r30>0 && pru_r30<12){
|
||||
write1(pru_chan_map[pru_r30],10); //writing absolute minimum to duty_ns necessary for pwm_sysfs to start working
|
||||
//printf("CHANNEL %d",pru_chan_map[pru_r30]);
|
||||
//printf(" exported\n");
|
||||
exported_count++;
|
||||
}
|
||||
else if(strcmp(dir->d_name,"pwm0")==0){
|
||||
write1(pru_chan_map[pru_r30],10);
|
||||
//printf("CHANNEL %d",pru_chan_map[0]);
|
||||
//printf(" exported \n");
|
||||
exported_count++;
|
||||
}
|
||||
}
|
||||
if(exported_count != PWM_CHAN_COUNT){
|
||||
printf("WARNING: All channels not exported\n");
|
||||
}
|
||||
closedir(_pwm_dir);
|
||||
}
|
||||
|
||||
void LinuxRCOutput::set_freq(uint32_t chmask, uint32_t freq_hz) //LSB corresponds to CHAN_1
|
||||
{
|
||||
int _fd, ret,i;
|
||||
unsigned long period_ns=1000000000/freq_hz;
|
||||
char period_sysfs_path[20],buffer[15];
|
||||
sprintf(buffer,"%lu",(unsigned long)period_ns);
|
||||
|
||||
for(i=0;i<12;i++){
|
||||
if(chmask&(1<<i)){
|
||||
sprintf(period_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[i],"/period_ns");
|
||||
_fd=open(period_sysfs_path,O_WRONLY);
|
||||
if(_fd < 0){
|
||||
perror("Period/Frequency file open failed");
|
||||
}
|
||||
|
||||
ret = write(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("Period/Frequency write failed");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t LinuxRCOutput::get_freq(uint8_t ch)
|
||||
{
|
||||
int _fd, ret;
|
||||
int freq_hz;
|
||||
char period_sysfs_path[20],buffer[15];
|
||||
sprintf(period_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch],"/period_ns");
|
||||
|
||||
_fd=open(period_sysfs_path,O_RDONLY);
|
||||
if(_fd < 0){
|
||||
perror("period_ns: file open failed");
|
||||
}
|
||||
|
||||
ret = read(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("period_ns: read failed");
|
||||
}
|
||||
freq_hz=1000000000/(unsigned long)strtol(buffer,NULL,10);
|
||||
|
||||
return freq_hz;
|
||||
}
|
||||
|
||||
void LinuxRCOutput::enable_ch(uint8_t ch)
|
||||
{}
|
||||
{
|
||||
int _fd, ret;
|
||||
char run_sysfs_path[20],buffer[]="1";
|
||||
sprintf(run_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch],"/run");
|
||||
|
||||
_fd=open(run_sysfs_path,O_RDONLY);
|
||||
if(_fd < 0){
|
||||
perror("run: file open failed");
|
||||
}
|
||||
|
||||
ret = read(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("run: read failed");
|
||||
}
|
||||
}
|
||||
|
||||
void LinuxRCOutput::disable_ch(uint8_t ch)
|
||||
{}
|
||||
{
|
||||
int _fd, ret;
|
||||
char run_sysfs_path[20],buffer[]="0";
|
||||
sprintf(run_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch],"/run");
|
||||
|
||||
_fd=open(run_sysfs_path,O_WRONLY);
|
||||
if(_fd < 0){
|
||||
perror("run: file open failed");
|
||||
}
|
||||
|
||||
ret = write(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("run: read failed");
|
||||
}
|
||||
}
|
||||
|
||||
void LinuxRCOutput::write(uint8_t ch, uint16_t period_us)
|
||||
{}
|
||||
{
|
||||
int _fd, ret;
|
||||
char duty_sysfs_path[20],buffer[15];
|
||||
sprintf(buffer,"%lu",(unsigned long)period_us*1000);
|
||||
sprintf(duty_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch],"/duty_ns");
|
||||
|
||||
_fd=open(duty_sysfs_path,O_WRONLY);
|
||||
if(_fd < 0){
|
||||
perror("duty_ns: file open failed");
|
||||
}
|
||||
|
||||
ret = write(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("duty_ns: read failed");
|
||||
}
|
||||
}
|
||||
|
||||
void LinuxRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
|
||||
{}
|
||||
{
|
||||
int _fd, ret,i;
|
||||
char duty_sysfs_path[25],buffer[15];
|
||||
for(i=0;i<len;i++){
|
||||
sprintf(buffer,"%lu",(unsigned long)period_us[i]*1000);
|
||||
sprintf(duty_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch+i],"/duty_ns");
|
||||
|
||||
_fd=open(duty_sysfs_path,O_WRONLY);
|
||||
if(_fd < 0){
|
||||
perror("duty_ns: file open failed");
|
||||
}
|
||||
|
||||
ret = write(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("duty_ns: read failed");
|
||||
}
|
||||
close(_fd);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t LinuxRCOutput::read(uint8_t ch) {
|
||||
return 900;
|
||||
int _fd, ret;
|
||||
char duty_sysfs_path[25],buffer[15];
|
||||
|
||||
sprintf(duty_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[ch],"/duty_ns");
|
||||
|
||||
_fd=open(duty_sysfs_path,O_RDONLY);
|
||||
if(_fd < 0){
|
||||
perror("duty_ns: file open failed");
|
||||
}
|
||||
|
||||
ret = read(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("duty_ns: read failed");
|
||||
}
|
||||
close(_fd);
|
||||
return strtol(buffer,NULL,10)/1000;
|
||||
|
||||
}
|
||||
|
||||
void LinuxRCOutput::read(uint16_t* period_us, uint8_t len)
|
||||
{}
|
||||
{
|
||||
int _fd, ret ,i;
|
||||
char duty_sysfs_path[25],buffer[15];
|
||||
for(i=0;i<len;i++){
|
||||
sprintf(duty_sysfs_path,"%s/pwm%d%s",PWM_DIR,chan_pru_map[i],"/duty_ns");
|
||||
|
||||
_fd=open(duty_sysfs_path,O_RDONLY);
|
||||
if(_fd < 0){
|
||||
perror("duty_ns: file open failed");
|
||||
}
|
||||
|
||||
ret = read(_fd,&buffer,sizeof(buffer));
|
||||
if(ret < 0){
|
||||
perror("duty_ns: read failed");
|
||||
}
|
||||
period_us[i]=strtol(buffer,NULL,10)/1000;
|
||||
close(_fd);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_HAL_BOARD
|
||||
|
|
|
|||
|
|
@ -3,6 +3,14 @@
|
|||
#define __AP_HAL_LINUX_RCOUTPUT_H__
|
||||
|
||||
#include <AP_HAL_Linux.h>
|
||||
#include <stdio.h>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <dirent.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
class Linux::LinuxRCOutput : public AP_HAL::RCOutput {
|
||||
void init(void* machtnichts);
|
||||
|
|
|
|||
Loading…
Reference in New Issue