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

libraries/AP_HAL_Linux/RCOutput.cpp

@@ -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;
+    sharedMem_cmd->enmask |= 1<<chan_pru_map[ch];
-    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;
 }
 
 void LinuxRCOutput::disable_ch(uint8_t ch)
 {
-    int i;
+    sharedMem_cmd->enmask &= !(1<<chan_pru_map[ch]);
-    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;
 }
 
 void LinuxRCOutput::write(uint8_t ch, uint16_t period_us)
 {
-    int i;
+    sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us;
-    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->magic = PWM_CMD_MAGIC;
 }
 
@@ -119,16 +73,16 @@ void LinuxRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
     }
 }
 
-uint16_t LinuxRCOutput::read(uint8_t ch) 
+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;
     }
 }
 

libraries/AP_HAL_Linux/RCOutput.h

@@ -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;
+        uint32_t magic;
-       uint8_t cmd;
+        uint32_t enmask;     /* enable mask */
-       uint8_t pwm_nr; /* in case it is required */
+        uint32_t offmsk;     /* state when pwm is off */
-       uint8_t pad[2];
+        uint32_t periodhi[MAX_PWMS][2];
-       union {
+        uint32_t hilo_read[MAX_PWMS][2];
-          struct pwm_multi_config cfg;
+        uint32_t enmask_read;
-          uint32_t hilo[2];
+    }*sharedMem_cmd;
-       }u;
-    };
-
-    struct pwm_cmd *sharedMem_cmd;
 
 };