SRV_Channel: Add servo chan override w/ timeout

Adds a feature to override a servo output for a given time. Scripting
bindings and example included.

libraries/SRV_Channel/SRV_Channel.h

@@ -317,6 +317,9 @@ public:
     // set output value for a specific function channel as a pwm value
     static void set_output_pwm_chan(uint8_t chan, uint16_t value);
 
+    // set output value for a specific function channel as a pwm value for specified override time in ms
+    static void set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms);
+
     // set output value for a function channel as a scaled value. This
     // calls calc_pwm() to also set the pwm value
     static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value);
@@ -531,6 +534,9 @@ private:
 
     SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
 
+    // override loop counter
+    static uint16_t override_counter[NUM_SERVO_CHANNELS];
+
     static struct srv_function {
         // mask of what channels this applies to
         SRV_Channel::servo_mask_t channel_mask;
@@ -548,4 +554,7 @@ private:
     }
 
     static bool emergency_stop;
+
+    // semaphore for multi-thread use of override_counter array
+    HAL_Semaphore override_counter_sem;
 };

libraries/SRV_Channel/SRV_Channels.cpp

@@ -41,6 +41,7 @@ SRV_Channels *SRV_Channels::_singleton;
 AP_Volz_Protocol *SRV_Channels::volz_ptr;
 AP_SBusOut *SRV_Channels::sbus_ptr;
 AP_RobotisServo *SRV_Channels::robotis_ptr;
+uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS];
 
 #if HAL_SUPPORT_RCOUT_SERIAL
 AP_BLHeli *SRV_Channels::blheli_ptr;
@@ -153,7 +154,7 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
     // @Group: _ROB_
     // @Path: ../AP_RobotisServo/AP_RobotisServo.cpp
     AP_SUBGROUPINFO(robotis, "_ROB_",  22, SRV_Channels, AP_RobotisServo),
-    
+
     AP_GROUPEND
 };
 
@@ -208,8 +209,16 @@ void SRV_Channels::setup_failsafe_trim_all_non_motors(void)
  */
 void SRV_Channels::calc_pwm(void)
 {
+    WITH_SEMAPHORE(_singleton->override_counter_sem);
+
     for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
-        channels[i].calc_pwm(functions[channels[i].function].output_scaled);
+        // check if channel has been locked out for this loop
+        // if it has, decrement the loop count for that channel
+        if (override_counter[i] == 0) {
+            channels[i].calc_pwm(functions[channels[i].function].output_scaled);
+        } else {
+            override_counter[i]--;
+        }
     }
 }
 
@@ -221,6 +230,22 @@ void SRV_Channels::set_output_pwm_chan(uint8_t chan, uint16_t value)
     }
 }
 
+// set output value for a specific function channel as a pwm value with loop based timeout
+// timeout_ms of zero will clear override of the channel
+// minimum override is 1 MAIN_LOOP
+void SRV_Channels::set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms)
+{
+    WITH_SEMAPHORE(_singleton->override_counter_sem);
+
+    if (chan < NUM_SERVO_CHANNELS) {
+        const uint32_t loop_period_us = AP::scheduler().get_loop_period_us();
+        // round up so any non-zero requested value will result in at least one loop
+        const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us;
+        override_counter[chan] = constrain_int32(loop_count, 0, UINT16_MAX);
+        SRV_Channels::set_output_pwm_chan(chan, value);
+    }
+}
+
 /*
   wrapper around hal.rcout->cork()
  */