mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-07 21:30:53 +08:00
200 lines
6.6 KiB
C++
200 lines
6.6 KiB
C++
/*
|
|
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.get();
|
|
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
|