mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
HAL_Linux: Make asynchronous PRU write and reduce the size of data exchange
Change to sending Total Period and hi Period, rather than Hi and Lo Period Change PRU firmware accordingly
This commit is contained in:
parent
fa6c5ba80e
commit
5bcdb039e2
Binary file not shown.
Binary file not shown.
@ -31,83 +31,37 @@ void LinuxRCOutput::init(void* machtnicht)
|
||||
mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
|
||||
sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, PRUSS_SHAREDRAM_BASE);
|
||||
close(mem_fd);
|
||||
sharedMem_cmd->cmd = PWM_CMD_CONFIG;
|
||||
sharedMem_cmd->u.cfg.enmask = 0xFFF;
|
||||
for(int i=0;i<PWM_CHAN_COUNT;i++){
|
||||
sharedMem_cmd->u.cfg.hilo[i][0] = TICK_PER_US*1000;
|
||||
sharedMem_cmd->u.cfg.hilo[i][1] = (TICK_PER_S/490)-TICK_PER_US*1000;
|
||||
}
|
||||
sharedMem_cmd->magic = PWM_CMD_MAGIC;
|
||||
}
|
||||
|
||||
void LinuxRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) //LSB corresponds to CHAN_1
|
||||
{
|
||||
int i;
|
||||
unsigned long tick=200000000/(unsigned long)freq_hz;
|
||||
unsigned long tick=TICK_PER_S/(unsigned long)freq_hz;
|
||||
for(i=0;i<12;i++){
|
||||
if(chmask&(1<<i)){
|
||||
period[chan_pru_map[i]]=tick;
|
||||
sharedMem_cmd->periodhi[chan_pru_map[i]][0]=tick;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t LinuxRCOutput::get_freq(uint8_t ch)
|
||||
{
|
||||
return TICK_PER_S/period[chan_pru_map[ch]];
|
||||
return TICK_PER_S/sharedMem_cmd->periodhi[chan_pru_map[ch]][0];
|
||||
}
|
||||
|
||||
void LinuxRCOutput::enable_ch(uint8_t ch)
|
||||
{
|
||||
int i;
|
||||
while(sharedMem_cmd->magic != PWM_REPLY_MAGIC && i < 5){
|
||||
usleep(2);
|
||||
i++;
|
||||
}
|
||||
if(i == 5){
|
||||
hal.console->println("RCOutput: PWM Write Failed!");
|
||||
return;
|
||||
}
|
||||
|
||||
sharedMem_cmd->cmd = PWM_CMD_ENABLE;
|
||||
sharedMem_cmd->pwm_nr = chan_pru_map[ch];
|
||||
sharedMem_cmd->magic = PWM_CMD_MAGIC;
|
||||
sharedMem_cmd->enmask |= 1<<chan_pru_map[ch];
|
||||
}
|
||||
|
||||
void LinuxRCOutput::disable_ch(uint8_t ch)
|
||||
{
|
||||
int i;
|
||||
while(sharedMem_cmd->magic != PWM_REPLY_MAGIC && i < 5){
|
||||
usleep(2);
|
||||
i++;
|
||||
}
|
||||
if(i == 5){
|
||||
hal.console->println("RCOutput: PWM Write Failed!");
|
||||
return;
|
||||
}
|
||||
|
||||
sharedMem_cmd->cmd = PWM_CMD_DISABLE;
|
||||
sharedMem_cmd->pwm_nr = chan_pru_map[ch];
|
||||
sharedMem_cmd->magic = PWM_CMD_MAGIC;
|
||||
sharedMem_cmd->enmask &= !(1<<chan_pru_map[ch]);
|
||||
}
|
||||
|
||||
void LinuxRCOutput::write(uint8_t ch, uint16_t period_us)
|
||||
{
|
||||
int i;
|
||||
pwm_hi[chan_pru_map[ch]]=period_us*TICK_PER_US;
|
||||
|
||||
while(sharedMem_cmd->magic != PWM_REPLY_MAGIC && i < 5){
|
||||
usleep(2);
|
||||
i++;
|
||||
}
|
||||
if(i == 5){
|
||||
hal.console->println("RCOutput: PWM Write Failed!");
|
||||
return;
|
||||
}
|
||||
|
||||
sharedMem_cmd->cmd = PWM_CMD_MODIFY;
|
||||
sharedMem_cmd->pwm_nr = chan_pru_map[ch];
|
||||
sharedMem_cmd->u.hilo[0] = pwm_hi[chan_pru_map[ch]];
|
||||
sharedMem_cmd->u.hilo[1] = period[chan_pru_map[ch]] - pwm_hi[chan_pru_map[ch]];
|
||||
sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us;
|
||||
sharedMem_cmd->magic = PWM_CMD_MAGIC;
|
||||
}
|
||||
|
||||
@ -121,14 +75,14 @@ void LinuxRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
|
||||
|
||||
uint16_t LinuxRCOutput::read(uint8_t ch)
|
||||
{
|
||||
return (pwm_hi[chan_pru_map[ch]]/TICK_PER_US);
|
||||
return (sharedMem_cmd->periodhi[chan_pru_map[ch]][1]/TICK_PER_US);
|
||||
}
|
||||
|
||||
void LinuxRCOutput::read(uint16_t* period_us, uint8_t len)
|
||||
{
|
||||
int i;
|
||||
for(i=0;i<len;i++){
|
||||
period_us[i] = pwm_hi[chan_pru_map[i]]/TICK_PER_US;
|
||||
period_us[i] = sharedMem_cmd->periodhi[chan_pru_map[i]][1]/TICK_PER_US;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -29,26 +29,14 @@ class Linux::LinuxRCOutput : public AP_HAL::RCOutput {
|
||||
void read(uint16_t* period_us, uint8_t len);
|
||||
|
||||
private:
|
||||
uint32_t period[MAX_PWMS];
|
||||
uint32_t pwm_hi[MAX_PWMS];
|
||||
struct pwm_multi_config {
|
||||
uint32_t enmask; /* enable mask */
|
||||
uint32_t offmsk; /* state when pwm is off */
|
||||
uint32_t hilo[MAX_PWMS][2];
|
||||
};
|
||||
|
||||
struct pwm_cmd {
|
||||
uint32_t magic;
|
||||
uint8_t cmd;
|
||||
uint8_t pwm_nr; /* in case it is required */
|
||||
uint8_t pad[2];
|
||||
union {
|
||||
struct pwm_multi_config cfg;
|
||||
uint32_t hilo[2];
|
||||
}u;
|
||||
};
|
||||
|
||||
struct pwm_cmd *sharedMem_cmd;
|
||||
uint32_t enmask; /* enable mask */
|
||||
uint32_t offmsk; /* state when pwm is off */
|
||||
uint32_t periodhi[MAX_PWMS][2];
|
||||
uint32_t hilo_read[MAX_PWMS][2];
|
||||
uint32_t enmask_read;
|
||||
}*sharedMem_cmd;
|
||||
|
||||
};
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user