diff --git a/libraries/AP_SBusOut/AP_SBusOut.cpp b/libraries/AP_SBusOut/AP_SBusOut.cpp index 48c6860af7..f700784f97 100644 --- a/libraries/AP_SBusOut/AP_SBusOut.cpp +++ b/libraries/AP_SBusOut/AP_SBusOut.cpp @@ -152,7 +152,7 @@ void AP_SBusOut::init() { uint16_t rate = sbus_rate.get(); #if SBUS_DEBUG - ::printf("AP_SBusOut: init %d Hz\n", rate); + hal.console->printf("AP_SBusOut: init %d Hz\n", rate); #endif // subtract 500msec from requested frame interval to allow for latency diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index fa6e1466a6..bc57b901d5 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -18,6 +18,7 @@ #include #include #include +#include #define NUM_SERVO_CHANNELS 16 @@ -440,6 +441,10 @@ private: AP_Volz_Protocol volz = AP_Volz_Protocol::create(); static AP_Volz_Protocol *volz_ptr; + // support for SBUS protocol + AP_SBusOut sbus = AP_SBusOut::create(); + static AP_SBusOut *sbus_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 8a7755a58f..dbb5778677 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::instance; AP_Volz_Protocol *SRV_Channels::volz_ptr; +AP_SBusOut *SRV_Channels::sbus_ptr; bool SRV_Channels::disabled_passthrough; bool SRV_Channels::initialised; @@ -117,6 +118,10 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @Path: ../AP_Volz_Protocol/AP_Volz_Protocol.cpp AP_SUBGROUPINFO(volz, "_VOLZ_", 19, SRV_Channels, AP_Volz_Protocol), + // @Group: SBUS_ + // @Path: ../AP_SBusOut/AP_SBusOut.cpp + AP_SUBGROUPINFO(sbus, "_SBUS_", 20, SRV_Channels, AP_SBusOut), + AP_GROUPEND }; @@ -137,6 +142,7 @@ SRV_Channels::SRV_Channels(void) } volz_ptr = &volz; + sbus_ptr = &sbus; } /* @@ -201,4 +207,7 @@ void SRV_Channels::push() // give volz library a chance to update volz_ptr->update(); + + // give sbus library a chance to update + sbus_ptr->update(); }