fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

esc_report: add actuator_function

Browse Source
This commit is contained in:
Beat Küng 2022-01-24 11:20:27 +01:00
parent aab2feb8f5
commit dfd934fbdb
9 changed files with 35 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/actuator_motors.msg
View File

@ -4,6 +4,8 @@ uint64 timestamp_sample # the timestamp the data this control response is ba
uint16 reversible_flags # bitset which motors are configured to be reversible
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 NUM_CONTROLS = 12
float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,
# -1 maximum negative (if not supported by the output, <0 maps to NaN),

2
msg/esc_report.msg
View File

@ -8,6 +8,8 @@ uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set
uint8 esc_state # State of ESC - depend on Vendor
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint16 failures # Bitmask to indicate the internal ESC faults
uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0)

27
src/drivers/dshot/DShot.cpp
View File

@ -247,7 +247,10 @@ void DShot::update_telemetry_num_motors()
if (_mixing_output.useDynamicMixing()) {
for (unsigned i = 0; i < _num_outputs; ++i) {
motor_count += _mixing_output.isFunctionSet(i);
if (_mixing_output.isFunctionSet(i)) {
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
++motor_count;
}
}
} else {
@ -281,24 +284,26 @@ void DShot::init_telemetry(const char *device)
update_telemetry_num_motors();
}
void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data)
void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
{
// fill in new motor data
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
esc_status.esc_online_flags |= 1 << motor_index;
if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
esc_status.esc_online_flags |= 1 << telemetry_index;
esc_status.esc[motor_index].timestamp = data.time;
esc_status.esc[motor_index].esc_rpm = (static_cast<int>(data.erpm) * 100) / (_param_mot_pole_count.get() / 2);
esc_status.esc[motor_index].esc_voltage = static_cast<float>(data.voltage) * 0.01f;
esc_status.esc[motor_index].esc_current = static_cast<float>(data.current) * 0.01f;
esc_status.esc[motor_index].esc_temperature = static_cast<float>(data.temperature);
esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
esc_status.esc[telemetry_index].timestamp = data.time;
esc_status.esc[telemetry_index].esc_rpm = (static_cast<int>(data.erpm) * 100) /
(_param_mot_pole_count.get() / 2);
esc_status.esc[telemetry_index].esc_voltage = static_cast<float>(data.voltage) * 0.01f;
esc_status.esc[telemetry_index].esc_current = static_cast<float>(data.current) * 0.01f;
esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
// TODO: accumulate consumption and use for battery estimation
}
// publish when motor index wraps (which is robust against motor timeouts)
if (motor_index <= _telemetry->last_motor_index) {
if (telemetry_index <= _telemetry->last_telemetry_index) {
esc_status.timestamp = hrt_absolute_time();
esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
esc_status.esc_count = _telemetry->handler.numMotors();
@ -314,7 +319,7 @@ void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetr
esc_status.esc_online_flags = 0;
}
_telemetry->last_motor_index = motor_index;
_telemetry->last_telemetry_index = telemetry_index;
}
int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)

5
src/drivers/dshot/DShot.h
View File

@ -127,14 +127,15 @@ private:
struct Telemetry {
DShotTelemetry handler{};
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
int last_motor_index{-1};
int last_telemetry_index{-1};
uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
};
void enable_dshot_outputs(const bool enabled);
void init_telemetry(const char *device);
void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
int request_esc_info();

2
src/drivers/tap_esc/TAP_ESC.cpp
View File

@ -290,6 +290,8 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
_esc_feedback.esc[feed_back_data.channelID].timestamp = hrt_absolute_time();
_esc_feedback.esc[feed_back_data.channelID].actuator_function = (uint8_t)_mixing_output.outputFunction(
feed_back_data.channelID);
_esc_feedback.esc[feed_back_data.channelID].esc_errorcount = 0;
_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
#if defined(ESC_HAVE_VOLTAGE_SENSOR)

2
src/drivers/telemetry/hott/messages.cpp
View File

@ -48,6 +48,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_gps_position.h>
@ -114,6 +115,7 @@ publish_gam_message(const uint8_t *buffer)
esc.esc_count = 1;
esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM;
esc.esc[0].actuator_function = actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1 - 20);
esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;

2
src/drivers/uavcan/actuators/esc.hpp
View File

@ -78,6 +78,8 @@ public:
static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
esc_status_s &esc_status() { return _esc_status; }
private:
/**
* ESC status message reception will be reported via this callback.

4
src/drivers/uavcan/uavcan_main.cpp
View File

@ -1003,6 +1003,10 @@ void UavcanMixingInterfaceESC::mixerChanged()
if (_mixing_output.useDynamicMixing()) {
for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
rotor_count += _mixing_output.isFunctionSet(i);
if (i < esc_status_s::CONNECTED_ESC_MAX) {
_esc_controller.esc_status().esc[i].actuator_function = (uint8_t)_mixing_output.outputFunction(i);
}
}
} else {

2
src/lib/mixer_module/functions.hpp
View File

@ -116,6 +116,8 @@ public:
static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
"Unexpected num motors");
static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
FunctionMotors(const Context &context);
static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }