diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 43ab9a3f03..fab05e8949 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -504,6 +504,8 @@ public: return _singleton; } + static void zero_rc_outputs(); + private: static bool disabled_passthrough; diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index e590cdc4e8..4ba3d7760e 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -328,3 +328,17 @@ void SRV_Channels::push() } #endif // HAL_NUM_CAN_IFACES } + +void SRV_Channels::zero_rc_outputs() +{ + /* Send an invalid signal to the motors to prevent spinning due to + * neutral (1500) pwm pulse being cut short. For that matter, + * send an invalid signal to all channels to prevent + * undesired/unexpected behavior + */ + cork(); + for (uint8_t i=0; iwrite(i, 0); + } + push(); +}