From 26d6947bf625dd762037d6aca17f53047c212f7f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 6 Jan 2018 20:42:47 +1100
Subject: [PATCH] HAL_ChibiOS: fixed PWM output

---
 libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp |  3 ---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp          | 15 ++++++---------
 libraries/AP_HAL_ChibiOS/RCOutput.h            |  1 -
 3 files changed, 6 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
index 61ed0b162b..bf050f26b3 100644
--- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
+++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
@@ -113,10 +113,7 @@ static THD_FUNCTION(main_loop,arg)
     hal.uartA->begin(115200);
     hal.uartB->begin(38400);
     hal.uartC->begin(57600);
-    hal.rcin->init();
-    hal.rcout->init();
     hal.analogin->init();
-    hal.gpio->init();
     hal.scheduler->init();
 
     /*
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index c33caecbc5..df98122f70 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -29,11 +29,12 @@ extern AP_IOMCU iomcu;
 
 #define PWM_CLK_FREQ        8000000
 #define PWM_US_WIDTH_FROM_CLK(x) ((PWM_CLK_FREQ/1000000)*x)
+
 const struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = 
 {
     //Group 1 Config
     {   //Channels in the Group and respective mapping
-        {PWM_CHAN_MAP(0) , PWM_CHAN_MAP(1) , PWM_CHAN_MAP(2) , PWM_CHAN_MAP(3)}, 
+        {0, 1, 2, 3},
         //Group Initial Config
         {
           8000000,                                  /* 8MHz PWM clock frequency.   */
@@ -96,7 +97,7 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
     }
     
     for (uint8_t i = 0; i < _num_groups; i++ ) {
-        grp_ch_mask = PWM_CHAN_MAP(0) | PWM_CHAN_MAP(1) | PWM_CHAN_MAP(2) | PWM_CHAN_MAP(3);
+        grp_ch_mask = 0XF;
         if ((grp_ch_mask & chmask) == grp_ch_mask) {
             update_mask |= grp_ch_mask;
             pwmChangePeriod(pwm_group_list[i].pwm_drv, 
@@ -138,13 +139,7 @@ void ChibiRCOutput::enable_ch(uint8_t chan)
     for (uint8_t i = 0; i < _num_groups; i++ ) {
         for (uint8_t j = 0; j < 4; j++) {
             if ((pwm_group_list[i].chan[j] == chan) && !(en_mask & 1<<chan)) {
-                pwmEnableChannel(pwm_group_list[i].pwm_drv, j, PWM_US_WIDTH_FROM_CLK(900));
                 en_mask |= 1<<chan;
-                if(_output_mode == MODE_PWM_BRUSHED) {
-                    period[chan] = 0;
-                } else {
-                    period[chan] = 900;
-                }
             }
         }
     }
@@ -197,7 +192,9 @@ void ChibiRCOutput::push_local(void)
     if (num_channels == 0) {
         return;
     }
-    uint16_t outmask = (1U<<(num_channels-1));
+    uint16_t outmask = (1U<<num_channels)-1;
+    outmask &= en_mask;
+    
     for (uint8_t i = 0; i < _num_groups; i++ ) {
         for (uint8_t j = 0; j < 4; j++) {
             uint8_t chan = pwm_group_list[i].chan[j];
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index d2838b57e4..be3b55e79d 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -19,7 +19,6 @@
 #include "AP_HAL_ChibiOS.h"
 #include "ch.h"
 #include "hal.h"
-#define PWM_CHAN_MAP(n) (1 << n)
 
 class ChibiOS::ChibiRCOutput : public AP_HAL::RCOutput {
 public: