From 06322da4e2cf1fbee929a03ad89db682d66857f2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 27 Dec 2017 20:15:55 +1100
Subject: [PATCH] AP_IOMCU: added setting of output rates

---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 31 +++++++++++++++++++++++++------
 libraries/AP_IOMCU/AP_IOMCU.h   |  8 +++++++-
 2 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index b0079d289d..c327e19be2 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -57,20 +57,25 @@ enum iopage {
 
 // pending IO events to send, used as an event mask
 enum ioevents {
-    IOEVENT_SEND_PWM_OUT=1,
-    IOEVENT_INIT,
+    IOEVENT_INIT=1,
+    IOEVENT_SEND_PWM_OUT,
     IOEVENT_SET_DISARMED_PWM,
     IOEVENT_SET_FAILSAFE_PWM,
     IOEVENT_FORCE_SAFETY_OFF,
     IOEVENT_FORCE_SAFETY_ON,
     IOEVENT_SET_ONESHOT_ON,
     IOEVENT_SET_ONESHOT_OFF,
+    IOEVENT_SET_RATES,
 };
 
 // setup page registers
 #define PAGE_REG_SETUP_ARMING 1
-#define P_SETUP_IO_ARM_OK       (1<<0)
-#define P_SETUP_FMU_ARMED       (1<<1)
+#define P_SETUP_ARMING_IO_ARM_OK (1<<0)
+#define P_SETUP_ARMING_FMU_ARMED (1<<1)
+
+#define PAGE_REG_SETUP_PWM_RATE_MASK 2
+#define PAGE_REG_SETUP_DEFAULTRATE   3
+#define PAGE_REG_SETUP_ALTRATE       4
 
 #define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12
 #define PAGE_REG_SETUP_FORCE_SAFETY_ON  14
@@ -113,7 +118,9 @@ void AP_IOMCU::thread_main(void)
     uart.begin(1500*1000, 256, 256);
 
     // set IO_ARM_OK and FMU_ARMED
-    modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0, P_SETUP_IO_ARM_OK | P_SETUP_FMU_ARMED);
+    modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
+                    P_SETUP_ARMING_IO_ARM_OK |
+                    P_SETUP_ARMING_FMU_ARMED);
     
     while (true) {
         eventmask_t mask = chEvtWaitAnyTimeout(~0, MS2ST(10));
@@ -130,6 +137,12 @@ void AP_IOMCU::thread_main(void)
         if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
             write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC);
         }
+
+        
+        if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
+            write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq);
+            write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask);
+        }
         
         // check for regular timed events
         uint32_t now = AP_HAL::millis();
@@ -402,10 +415,16 @@ void AP_IOMCU::push(void)
 // set output frequency
 void AP_IOMCU::set_freq(uint16_t chmask, uint16_t freq)
 {
+    rate.freq = freq;
+    rate.chmask = chmask;
+    trigger_event(IOEVENT_SET_RATES);
 }
 
 // get output frequency
 uint16_t AP_IOMCU::get_freq(uint16_t chan)
 {
-    return 50;
+    if ((1U<<chan) & rate.chmask) {
+        return rate.freq;
+    }
+    return rate.default_freq;
 }
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 006a3d956d..2fc17be584 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -41,7 +41,6 @@ public:
     // force safety off
     void force_safety_off(void);
 
-    
 private:
     AP_HAL::UARTDriver &uart;
 
@@ -124,6 +123,13 @@ private:
     struct {
         uint16_t pwm[max_channels];
     } pwm_in;
+
+    // output rates
+    struct {
+        uint16_t freq;
+        uint16_t chmask;
+        uint16_t default_freq = 50;
+    } rate;
     
     bool corked;
 };