From a0eef1039c49e59c73330bee14da512948e5805c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH] SRV_Channel: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- libraries/SRV_Channel/SRV_Channel.h | 15 +++--- libraries/SRV_Channel/SRV_Channel_aux.cpp | 2 +- libraries/SRV_Channel/SRV_Channels.cpp | 63 ++++++----------------- 3 files changed, 24 insertions(+), 56 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 24631a448d..e19c52a4bf 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -473,7 +473,7 @@ public: int16_t value, int16_t angle_min, int16_t angle_max); // assign and enable auxiliary channels - static void enable_aux_servos(void); + void enable_aux_servos(void); // enable channels by mask static void enable_by_mask(uint32_t mask); @@ -541,7 +541,7 @@ public: static void cork(); - static void push(); + void push(); // disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code static void set_disabled_channel_mask(uint32_t mask) { disabled_mask = mask; } @@ -570,7 +570,7 @@ public: static void zero_rc_outputs(); // initialize before any call to push - static void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE); + void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE); // return true if a channel is set to type GPIO static bool is_GPIO(uint8_t channel); @@ -610,30 +610,25 @@ private: #if AP_VOLZ_ENABLED // support for Volz protocol AP_Volz_Protocol volz; - static AP_Volz_Protocol *volz_ptr; #endif #if AP_SBUSOUTPUT_ENABLED // support for SBUS protocol AP_SBusOut sbus; - static AP_SBusOut *sbus_ptr; #endif #if AP_ROBOTISSERVO_ENABLED // support for Robotis servo protocol AP_RobotisServo robotis; - static AP_RobotisServo *robotis_ptr; #endif #if HAL_SUPPORT_RCOUT_SERIAL // support for BLHeli protocol AP_BLHeli blheli; - static AP_BLHeli *blheli_ptr; #endif #if AP_FETTEC_ONEWIRE_ENABLED AP_FETtecOneWire fetteconwire; - static AP_FETtecOneWire *fetteconwire_ptr; #endif // AP_FETTEC_ONEWIRE_ENABLED // mask of disabled channels @@ -692,3 +687,7 @@ private: // semaphore for multi-thread use of override_counter array HAL_Semaphore override_counter_sem; }; + +namespace AP { + SRV_Channels &srv(); +}; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index c117915e8a..3fb4e92274 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -270,7 +270,7 @@ void SRV_Channels::enable_aux_servos() hal.rcout->update_channel_masks(); #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->update(); + blheli.update(); #endif } diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 67a9af5b00..3ef0c7e343 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -41,28 +41,8 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::_singleton; -#if AP_VOLZ_ENABLED -AP_Volz_Protocol *SRV_Channels::volz_ptr; -#endif - -#if AP_SBUSOUTPUT_ENABLED -AP_SBusOut *SRV_Channels::sbus_ptr; -#endif - -#if AP_ROBOTISSERVO_ENABLED -AP_RobotisServo *SRV_Channels::robotis_ptr; -#endif - -#if AP_FETTEC_ONEWIRE_ENABLED -AP_FETtecOneWire *SRV_Channels::fetteconwire_ptr; -#endif - uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS]; -#if HAL_SUPPORT_RCOUT_SERIAL -AP_BLHeli *SRV_Channels::blheli_ptr; -#endif - uint32_t SRV_Channels::disabled_mask; uint32_t SRV_Channels::digital_mask; uint32_t SRV_Channels::reversible_mask; @@ -379,26 +359,6 @@ SRV_Channels::SRV_Channels(void) } #endif } - -#if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr = &fetteconwire; -#endif - -#if AP_VOLZ_ENABLED - volz_ptr = &volz; -#endif - -#if AP_SBUSOUTPUT_ENABLED - sbus_ptr = &sbus; -#endif - -#if AP_ROBOTISSERVO_ENABLED - robotis_ptr = &robotis; -#endif // AP_ROBOTISSERVO_ENABLED - -#if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr = &blheli; -#endif } // SRV_Channels initialization @@ -406,7 +366,7 @@ void SRV_Channels::init(uint32_t motor_mask, AP_HAL::RCOutput::output_mode mode) { // initialize BLHeli late so that all of the masks it might setup don't get trodden on by motor initialization #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->init(motor_mask, mode); + blheli.init(motor_mask, mode); #endif #ifndef HAL_BUILD_AP_PERIPH hal.rcout->set_dshot_rate(_singleton->dshot_rate, AP::scheduler().get_loop_rate_hz()); @@ -519,26 +479,26 @@ void SRV_Channels::push() #if AP_VOLZ_ENABLED // give volz library a chance to update - volz_ptr->update(); + volz.update(); #endif #if AP_SBUSOUTPUT_ENABLED // give sbus library a chance to update - sbus_ptr->update(); + sbus.update(); #endif #if AP_ROBOTISSERVO_ENABLED // give robotis library a chance to update - robotis_ptr->update(); + robotis.update(); #endif #if HAL_SUPPORT_RCOUT_SERIAL // give blheli telemetry a chance to update - blheli_ptr->update_telemetry(); + blheli.update_telemetry(); #endif #if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr->update(); + fetteconwire.update(); #endif #if AP_KDECAN_ENABLED @@ -589,7 +549,7 @@ void SRV_Channels::zero_rc_outputs() for (uint8_t i=0; iwrite(i, 0); } - push(); + AP::srv().push(); } /* @@ -619,3 +579,12 @@ void SRV_Channels::set_emergency_stop(bool state) { #endif emergency_stop = state; } + +namespace AP { + +SRV_Channels &srv() +{ + return *SRV_Channels::get_singleton(); +} + +};