From d72f6598e0bb086302e4d29eeff647fa96aeb86c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 19 Dec 2018 14:28:35 +1100
Subject: [PATCH] SRV_Channel: added RobotisServo protocol support

---
 libraries/SRV_Channel/SRV_Channel.h    | 5 +++++
 libraries/SRV_Channel/SRV_Channels.cpp | 9 +++++++++
 2 files changed, 14 insertions(+)

diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index 88a90da794e..5c83720106b 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -18,6 +18,7 @@
 #include <AP_RCMapper/AP_RCMapper.h>
 #include <AP_Common/Bitmask.h>
 #include <AP_Volz_Protocol/AP_Volz_Protocol.h>
+#include <AP_RobotisServo/AP_RobotisServo.h>
 #include <AP_SBusOut/AP_SBusOut.h>
 #include <AP_BLHeli/AP_BLHeli.h>
 
@@ -480,6 +481,10 @@ private:
     AP_SBusOut sbus;
     static AP_SBusOut *sbus_ptr;
 
+    // support for Robotis servo protocol
+    AP_RobotisServo robotis;
+    static AP_RobotisServo *robotis_ptr;
+    
 #if HAL_SUPPORT_RCOUT_SERIAL
     // support for BLHeli protocol
     AP_BLHeli blheli;
diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp
index fdce20e38f9..3c21c82eb3f 100644
--- a/libraries/SRV_Channel/SRV_Channels.cpp
+++ b/libraries/SRV_Channel/SRV_Channels.cpp
@@ -33,6 +33,7 @@ SRV_Channel *SRV_Channels::channels;
 SRV_Channels *SRV_Channels::instance;
 AP_Volz_Protocol *SRV_Channels::volz_ptr;
 AP_SBusOut *SRV_Channels::sbus_ptr;
+AP_RobotisServo *SRV_Channels::robotis_ptr;
 
 #if HAL_SUPPORT_RCOUT_SERIAL
 AP_BLHeli *SRV_Channels::blheli_ptr;
@@ -141,6 +142,10 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
     AP_SUBGROUPINFO(blheli, "_BLH_",  21, SRV_Channels, AP_BLHeli),
 #endif
 
+    // @Group: _ROB_
+    // @Path: ../AP_RobotisServo/AP_RobotisServo.cpp
+    AP_SUBGROUPINFO(robotis, "_ROB_",  22, SRV_Channels, AP_RobotisServo),
+    
     AP_GROUPEND
 };
 
@@ -162,6 +167,7 @@ SRV_Channels::SRV_Channels(void)
 
     volz_ptr = &volz;
     sbus_ptr = &sbus;
+    robotis_ptr = &robotis;
 #if HAL_SUPPORT_RCOUT_SERIAL
     blheli_ptr = &blheli;
 #endif
@@ -228,6 +234,9 @@ void SRV_Channels::push()
     // give sbus library a chance to update
     sbus_ptr->update();
 
+    // give robotis library a chance to update
+    robotis_ptr->update();
+    
 #if HAL_SUPPORT_RCOUT_SERIAL
     // give blheli telemetry a chance to update
     blheli_ptr->update_telemetry();