SRV_Channel:add ap periph servo power output currect sampling and dronecan telemary support

Author: Huibean

libraries/SRV_Channel/SRV_Channel.cpp

@@ -171,6 +171,50 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = {
     // @RebootRequired: True
     AP_GROUPINFO("FUNCTION",  5, SRV_Channel, function, 0),
 
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    // @Param: TELEM_RATE
+    // @DisplayName: Actuator Telemetry rate
+    // @Description: Actuator Telemetry update rate in Hz for this servo channel.
+    // @Units: Hz
+    // @Range: 1 100
+    // @User: Standard
+    AP_GROUPINFO("TELEM_RATE",  6, SRV_Channel, _telem_rate, AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT),
+
+    // @Param: CURR_PIN
+    // @DisplayName: Current sensing pin
+    // @Description: Analog input pin number for current sensing on this servo channel. Set to -1 to disable current sensing.
+    // @Values: -1:Disabled
+    // @Range: -1 15
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("CURR_PIN",    7, SRV_Channel, _curr_pin, -1),
+
+    // @Param: AMP_OFFSET
+    // @DisplayName: Current sensor offset
+    // @Description: Voltage offset at zero current on the current sensor for this servo channel. Used to calibrate analog current sensors.
+    // @Units: V
+    // @Range: -10 10
+    // @User: Standard
+    AP_GROUPINFO("AMP_OFFSET",  8, SRV_Channel, _curr_amp_offset, 0),
+
+    // @Param: AMP_PERVLT
+    // @DisplayName: Amps per volt
+    // @Description: Number of amps that a 1V reading on the current sensor corresponds to for this servo channel. This is the current sensor scale factor.
+    // @Units: A/V
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("AMP_PERVLT",  9, SRV_Channel, _curr_amp_per_volt, 0),
+
+
+    // @Param: CURR_MAX
+    // @DisplayName: Maximum current
+    // @Description: Maximum expected current draw for this servo channel in amps. Used for scaling and safety limits.
+    // @Units: A
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("CURR_MAX",    10, SRV_Channel, _curr_max, 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -399,3 +443,33 @@ int8_t SRV_Channel::get_motor_num(void) const
     }
     return -1;
 }
+
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+// return the current in amps for this channel
+float SRV_Channel::get_current_ampere(void) const
+{
+    // Check if ADC channel is configured
+    if (_curr_pin > 0 && _curr_pin_analog_source == nullptr) {
+        _curr_pin_analog_source = hal.analogin->channel(_curr_pin);
+        if (_curr_pin_analog_source == nullptr) {
+            return 0.0f;
+        }
+        if (!_curr_pin_analog_source->set_pin(_curr_pin)) {
+            return 0.0f;
+        }
+    } else if (_curr_pin <= 0 || _curr_pin_analog_source == nullptr) {
+        return 0.0f;
+    }
+
+    float adc_voltage = _curr_pin_analog_source->voltage_average();
+    float current = (adc_voltage - _curr_amp_offset) * _curr_amp_per_volt;
+
+    return current;
+}
+
+//return preset maximum current in amps for this channel
+float SRV_Channel::get_current_max_ampere(void) const
+{
+    return _curr_max;
+}
+#endif

libraries/SRV_Channel/SRV_Channel.h

@@ -313,6 +313,15 @@ class SRV_Channel {
     // used by DO_SET_SERVO commands
     void ignore_small_rcin_changes() { ign_small_rcin_changes = true; }
 
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    // get current in amperes by reading ADC
+    float get_current_ampere(void) const;
+    // get maximum expected current in amperes
+    float get_current_max_ampere(void) const;
+    // get telemetry update rate in Hz
+    int16_t get_telem_rate(void) const { return _telem_rate; }
+#endif
+
 private:
     AP_Int16 servo_min;
     AP_Int16 servo_max;
@@ -321,6 +330,15 @@ class SRV_Channel {
     AP_Int8 reversed;
     AP_Enum16<Function> function;
 
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    AP_Int16 _telem_rate;        // telemetry update rate in Hz
+    AP_Int8 _curr_pin;           // current sensor ADC channel
+    AP_Float _curr_amp_offset;   // current sensor offset
+    AP_Float _curr_amp_per_volt; // current sensor scale in amps per volt
+    AP_Float _curr_max;          // maximum expected current in amps
+    mutable AP_HAL::AnalogSource *_curr_pin_analog_source;  // analog source for current (mutable for lazy init)
+#endif
+
     // a pending output value as PWM
     uint16_t output_pwm;
 

libraries/SRV_Channel/SRV_Channel_config.h

@@ -19,3 +19,11 @@
         #define NUM_SERVO_CHANNELS 16
     #endif
 #endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_ENABLED
+#define AP_PERIPH_ACTUATOR_TELEM_ENABLED 0
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT 25
+#endif