diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index fdb3b94625..fa6e1466a6 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -17,6 +17,7 @@ #include #include #include +#include #define NUM_SERVO_CHANNELS 16 @@ -30,6 +31,7 @@ class SRV_Channels; class SRV_Channel { public: friend class SRV_Channels; + // constructor SRV_Channel(void); @@ -414,6 +416,10 @@ public: return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8))); } + static void cork(); + + static void push(); + private: struct { bool k_throttle_reversible:1; @@ -429,6 +435,11 @@ private: // this static arrangement is to avoid having static objects in AP_Param tables static SRV_Channel *channels; static SRV_Channels *instance; + + // support for Volz protocol + AP_Volz_Protocol volz = AP_Volz_Protocol::create(); + static AP_Volz_Protocol *volz_ptr; + SRV_Channel obj_channels[NUM_SERVO_CHANNELS]; static struct srv_function { diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 5abbf05088..8a7755a58f 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -26,6 +26,8 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::instance; +AP_Volz_Protocol *SRV_Channels::volz_ptr; + bool SRV_Channels::disabled_passthrough; bool SRV_Channels::initialised; Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions}; @@ -111,6 +113,10 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @Units: Hz AP_GROUPINFO("_RATE", 18, SRV_Channels, default_rate, 50), + // @Group: VOLZ_ + // @Path: ../AP_Volz_Protocol/AP_Volz_Protocol.cpp + AP_SUBGROUPINFO(volz, "_VOLZ_", 19, SRV_Channels, AP_Volz_Protocol), + AP_GROUPEND }; @@ -129,6 +135,8 @@ SRV_Channels::SRV_Channels(void) for (uint8_t i=0; icork() + */ +void SRV_Channels::cork() +{ + hal.rcout->cork(); +} + +/* + wrapper around hal.rcout->push() + */ +void SRV_Channels::push() +{ + hal.rcout->push(); + + // give volz library a chance to update + volz_ptr->update(); +}