AP_Periph:add ap periph rc out current sampling and telemary support

Author: Huibean

Tools/AP_Periph/AP_Periph.cpp

@@ -315,6 +315,11 @@ void AP_Periph_FW::init()
 #if AP_SCRIPTING_ENABLED
     scripting.init();
 #endif
+
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    actuator_telem.init();
+#endif
+
     start_ms = AP_HAL::millis();
 }
 
@@ -505,6 +510,10 @@ void AP_Periph_FW::update()
     rcin_update();
 #endif
 
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    actuator_telem.update();
+#endif
+
 #if AP_PERIPH_BATTERY_BALANCE_ENABLED
     batt_balance_update();
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -41,6 +41,7 @@
 #include "rc_in.h"
 #include "batt_balance.h"
 #include "battery_tag.h"
+#include "actuator_telem.h"
 #include "networking.h"
 #include "serial_options.h"
 #if AP_SIM_ENABLED
@@ -395,6 +396,10 @@ class AP_Periph_FW {
 #if AP_PERIPH_BATTERY_TAG_ENABLED
     BatteryTag battery_tag;
 #endif
+
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    ActuatorTelem actuator_telem;
+#endif
 
 #if AP_PERIPH_SERIAL_OPTIONS_ENABLED
     SerialOptions serial_options;

Tools/AP_Periph/Parameters.cpp

@@ -617,6 +617,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(g_rcin, "RC",  Parameters_RCIN),
 #endif
 
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+    // @Group: ACT
+    // @Path: actuator_telem.cpp
+    GOBJECT(actuator_telem, "ACT", ActuatorTelem),
+#endif
+
 #if AP_PERIPH_BATTERY_BALANCE_ENABLED
     // @Group: BAL
     // @Path: batt_balance.cpp

Tools/AP_Periph/Parameters.h

@@ -79,6 +79,7 @@ class Parameters {
         k_param_networking_periph,
         k_param_rpm_sensor,
         k_param_g_rcin,
+        k_param_actuator_telem,
         k_param_sitl,
         k_param_ahrs,
         k_param_battery_balance,

Tools/AP_Periph/actuator_telem.cpp

@@ -0,0 +1,199 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Periph.h"
+
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+
+#include <dronecan_msgs.h>
+
+extern const AP_HAL::HAL &hal;
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT 10
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_NUM_CHANNELS_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_NUM_CHANNELS_DEFAULT 0
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_CURR_PIN_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_CURR_PIN_DEFAULT -1
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_OFFSET_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_OFFSET_DEFAULT 0.0f
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_PERVLT_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_PERVLT_DEFAULT 10
+#endif
+
+#ifndef AP_PERIPH_ACTUATOR_TELEM_CURR_MAX_DEFAULT
+#define AP_PERIPH_ACTUATOR_TELEM_CURR_MAX_DEFAULT 2.5f
+#endif
+
+const AP_Param::GroupInfo ActuatorTelem::var_info[] = {
+    // @Param: _TELEM_RATE
+    // @DisplayName: Actuator Telemetry rate
+    // @Description: Actuator Telemetry update rate in Hz. Set to 0 to disable.
+    // @Units: Hz
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("_TELEM_RATE", 1, ActuatorTelem, rate, AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT),
+
+    // @Param: _NUM_CHANS
+    // @DisplayName: Number of actuator channels
+    // @Description: Number of actuator channels to monitor for telemetry.
+    // @Range: 0 4 
+    // @User: Standard
+    AP_GROUPINFO("_NUM_CHANS", 2, ActuatorTelem, num_chans, AP_PERIPH_ACTUATOR_TELEM_NUM_CHANNELS_DEFAULT),
+
+    // @Param: _CURR_PIN1
+    // @DisplayName: Current sensing pin 1
+    // @Description: Analog input pin number for current sensing on channel 1. Set to -1 to disable.
+    // @Values: -1:Disabled
+    // @Range: -1 127
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("_CURR_PIN1", 3, ActuatorTelem, curr_pin1, AP_PERIPH_ACTUATOR_TELEM_CURR_PIN_DEFAULT),
+
+    // @Param: _AMP_OFFSET
+    // @DisplayName: Current sensor offset
+    // @Description: Voltage offset at zero current on the current sensor.
+    // @Units: V
+    // @User: Standard
+    AP_GROUPINFO("AMP_OFFSET", 4, ActuatorTelem, curr_amp_offset, AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_OFFSET_DEFAULT),
+
+    // @Param: _AMP_PERVLT
+    // @DisplayName: Amps per volt
+    // @Description: Current sensor scale factor.
+    // @Units: A/V
+    // @User: Standard
+    AP_GROUPINFO("AMP_PERVLT", 5, ActuatorTelem, curr_amp_per_volt, AP_PERIPH_ACTUATOR_TELEM_CURR_AMP_PERVLT_DEFAULT),
+
+    // @Param: _CURR_MAX
+    // @DisplayName: Maximum current
+    // @Description: Maximum expected current for this channel.
+    // @Units: A
+    // @User: Standard
+    AP_GROUPINFO("CURR_MAX", 6, ActuatorTelem, curr_max, AP_PERIPH_ACTUATOR_TELEM_CURR_MAX_DEFAULT),
+
+    AP_GROUPEND
+};
+
+ActuatorTelem::ActuatorTelem(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    for (uint8_t i = 0; i < HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS; i++) {
+        analog_sources[i] = nullptr;
+    }
+}
+
+void ActuatorTelem::init(void)
+{
+    const int8_t curr_pin1_val = curr_pin1.get();
+    const int8_t num_chans_val = num_chans.get();
+    
+    if (curr_pin1_val >= 0 && num_chans_val > 0) {
+        for (uint8_t i = 0; i < MIN(HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS, num_chans_val); i++) {
+            const int8_t pin = curr_pin1_val + i;
+            if (pin >= 0) {
+                analog_sources[i] = hal.analogin->channel(pin);
+            }
+        }
+    }
+}
+
+void ActuatorTelem::send_telemetry(uint8_t channel_index, uint8_t actuator_id)
+{
+    // Check if this channel is enabled
+    if (channel_index >= HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS) {
+        return;
+    }
+
+    AP_HAL::AnalogSource *source = analog_sources[channel_index];
+    if (source == nullptr) {
+        return;
+    }
+    // Read current from analog input
+    float current_amp = 0;
+    float adc_voltage = source->voltage_average();
+    current_amp = (adc_voltage - curr_amp_offset) * curr_amp_per_volt;
+
+    uavcan_equipment_actuator_Status pkt {}; 
+
+    pkt.actuator_id = actuator_id;
+    pkt.position = nanf(""); // Not available
+    pkt.force = nanf("");    // Not available
+    pkt.speed = nanf("");    // Not available
+
+    // Calculate power rating percentage from current
+    const float max_current = curr_max;
+    if (max_current > 0 && current_amp >= 0) {
+        pkt.power_rating_pct = constrain_int16(current_amp / max_current * 100.0f, 0, 100);
+    } else {
+        pkt.power_rating_pct = UAVCAN_EQUIPMENT_ACTUATOR_STATUS_POWER_RATING_PCT_UNKNOWN;
+    }
+
+    // encode and broadcast
+    uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE];
+    uint16_t total_size = uavcan_equipment_actuator_Status_encode(&pkt, buffer, !periph.canfdout());
+
+    periph.canard_broadcast(UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE,
+                            UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID,
+                            CANARD_TRANSFER_PRIORITY_LOW,
+                            &buffer[0],
+                            total_size);
+}
+
+void ActuatorTelem::update()
+{
+    // Check global telemetry rate
+    const float telem_rate = rate.get();
+    if (telem_rate <= 0) {
+        return;  // telemetry disabled
+    }
+
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_telem_update_ms < 1000.0f / telem_rate) {
+        return;
+    }
+    last_telem_update_ms = now;
+
+#if HAL_PWM_COUNT > 0
+    const int8_t num_chans_val = num_chans.get();
+    if (num_chans_val > 0) {
+        for (uint8_t i = 0; i < MIN(HAL_PWM_COUNT, num_chans_val); i++) {
+            const auto *srv_channel = periph.servo_channels.srv_channel(i);
+            if (srv_channel == nullptr) {
+                continue;
+            }
+
+            const SRV_Channel::Function function = srv_channel->get_function();
+            // Only send for configured actuator functions
+            if (function < SRV_Channel::k_rcin1 || function > SRV_Channel::k_rcin16) {
+                continue;
+            }
+
+            const uint8_t actuator_id = function - SRV_Channel::k_rcin1 + 1;
+
+            send_telemetry(i, actuator_id);
+        }
+    }
+#endif
+}
+
+#endif  // AP_PERIPH_ACTUATOR_TELEM_ENABLED

Tools/AP_Periph/actuator_telem.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+
+#ifndef HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS
+#define HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS 4 
+#endif
+
+// Manager for all actuator channels
+class ActuatorTelem {
+public:
+    friend class AP_Periph_FW;
+    ActuatorTelem(void);
+
+    void init(void);
+    void send_telemetry(uint8_t channel_index, uint8_t actuator_id);
+    void update(void);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    AP_Int16 rate;
+    AP_Int8 num_chans;
+    AP_Int8 curr_pin1;
+    AP_Float curr_amp_offset;
+    AP_Float curr_amp_per_volt;
+    AP_Float curr_max;
+    uint32_t last_telem_update_ms;
+    AP_HAL::AnalogSource *analog_sources[HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS];
+};
+
+#endif // AP_PERIPH_ACTUATOR_TELEM_ENABLED