mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mavlink: esc: fix ESC_STATUS and ESC_INFO message emission. (#25849)
* mavlink: esc: fix ESC_STATUS and ESC_INFO message emission. Fixes mavlink messages emission for ESC messages. Actuator --> MotorNumber mapping was not respected, the mavlink messages should be reporting the ESC status in motor number order not actuator order. * Update src/modules/mavlink/streams/ESC_STATUS.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update src/modules/mavlink/streams/ESC_INFO.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * remove dependency on mixer_module/output_functions.hpp * add actuator function definitions to EscReport.msg * clean up * add missing header --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This commit is contained in:
Jacob Dahl
GitHub
parent
229b53d25d
commit
efbc9e64a4
@ -11,6 +11,19 @@ uint8 esc_state # State of ESC - depend on Vendor
|
||||
|
||||
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
|
||||
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR2 = 102
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR3 = 103
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR4 = 104
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR5 = 105
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR6 = 106
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR7 = 107
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR8 = 108
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR9 = 109
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR10 = 110
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR11 = 111
|
||||
uint8 ACTUATOR_FUNCTION_MOTOR12 = 112
|
||||
|
||||
uint16 failures # Bitmask to indicate the internal ESC faults
|
||||
int8 esc_power # Applied power 0-100 in % (negative values reserved)
|
||||
|
||||
|
||||
@ -1813,9 +1813,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
|
||||
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
|
||||
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f);
|
||||
configure_stream_local("ESC_INFO", 0.2f);
|
||||
configure_stream_local("ESC_STATUS", 0.5f);
|
||||
|
||||
configure_stream_local("ESC_INFO", 1.0f);
|
||||
configure_stream_local("ESC_STATUS", 2.0f);
|
||||
configure_stream_local("ADSB_VEHICLE", 1.0f);
|
||||
configure_stream_local("ATTITUDE_TARGET", 0.5f);
|
||||
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||
|
||||
@ -34,7 +34,9 @@
|
||||
#ifndef ESC_INFO_HPP
|
||||
#define ESC_INFO_HPP
|
||||
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
class MavlinkStreamESCInfo : public MavlinkStream
|
||||
{
|
||||
@ -49,50 +51,121 @@ public:
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0;
|
||||
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
return _esc_status_subs.advertised_count() * message_size;
|
||||
}
|
||||
|
||||
private:
|
||||
explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||
|
||||
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
|
||||
uint8_t _number_of_batches{0};
|
||||
uORB::SubscriptionMultiArray<esc_status_s> _esc_status_subs{ORB_ID::esc_status};
|
||||
|
||||
static constexpr uint8_t MAX_ESC_OUTPUTS = 12; // See output_functions.hpp
|
||||
static constexpr uint8_t ESCS_PER_MSG = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN;
|
||||
static constexpr uint8_t MAX_NUM_MSGS = MAX_ESC_OUTPUTS / ESCS_PER_MSG;
|
||||
|
||||
static constexpr hrt_abstime ESC_TIMEOUT = 100000;
|
||||
|
||||
struct EscOutputInterfaceInfo {
|
||||
uint16_t counter;
|
||||
uint8_t esc_count;
|
||||
uint8_t esc_connectiontype;
|
||||
uint8_t esc_online_flags;
|
||||
};
|
||||
|
||||
struct EscInfo {
|
||||
hrt_abstime timestamp;
|
||||
uint16_t failure_flags;
|
||||
uint32_t error_count;
|
||||
int16_t temperature;
|
||||
bool online;
|
||||
};
|
||||
|
||||
int _total_esc_count = {};
|
||||
EscOutputInterfaceInfo _interface[MAX_NUM_MSGS] = {};
|
||||
EscInfo _escs[MAX_ESC_OUTPUTS] = {};
|
||||
|
||||
void update_data() override
|
||||
{
|
||||
int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
|
||||
|
||||
for (int i = 0; i < subscriber_count; i++) {
|
||||
esc_status_s esc = {};
|
||||
|
||||
if (_esc_status_subs[i].update(&esc)) {
|
||||
_interface[i].counter = esc.counter;
|
||||
_interface[i].esc_count = esc.esc_count;
|
||||
_interface[i].esc_connectiontype = esc.esc_connectiontype;
|
||||
|
||||
// Capture online_flags, we will map from index to motor number
|
||||
uint8_t online_flags = esc.esc_online_flags;
|
||||
_interface[i].esc_online_flags = 0;
|
||||
|
||||
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
|
||||
bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) &&
|
||||
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12);
|
||||
|
||||
if (is_motor) {
|
||||
// Map OutputFunction number to index
|
||||
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
|
||||
_escs[index].online = online_flags & (1 << j);
|
||||
_escs[index].failure_flags = esc.esc[j].failures;
|
||||
_escs[index].error_count = esc.esc[j].esc_errorcount;
|
||||
_escs[index].timestamp = esc.esc[j].timestamp;
|
||||
_escs[index].temperature = esc.esc[j].esc_temperature * 100.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int count = 0;
|
||||
|
||||
for (int i = 0; i < MAX_NUM_MSGS; i++) {
|
||||
count += _interface[i].esc_count;
|
||||
}
|
||||
|
||||
_total_esc_count = count;
|
||||
}
|
||||
|
||||
bool send() override
|
||||
{
|
||||
static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN;
|
||||
esc_status_s esc_status;
|
||||
bool updated = false;
|
||||
|
||||
if (_esc_status_sub.update(&esc_status)) {
|
||||
mavlink_esc_info_t msg{};
|
||||
for (int i = 0; i < MAX_NUM_MSGS; i++) {
|
||||
|
||||
msg.time_usec = esc_status.timestamp;
|
||||
msg.counter = esc_status.counter;
|
||||
msg.count = esc_status.esc_count;
|
||||
msg.connection_type = esc_status.esc_connectiontype;
|
||||
msg.info = esc_status.esc_online_flags;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc
|
||||
_number_of_batches = ceilf((float)esc_status.esc_count / batch_size);
|
||||
mavlink_esc_info_t msg = {};
|
||||
msg.index = i * ESCS_PER_MSG;
|
||||
msg.time_usec = now;
|
||||
msg.counter = _interface[i].counter;
|
||||
msg.count = _total_esc_count;
|
||||
msg.connection_type = _interface[i].esc_connectiontype;
|
||||
msg.info = _interface[i].esc_online_flags;
|
||||
|
||||
for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) {
|
||||
msg.index = batch_number * batch_size;
|
||||
bool atleast_one_esc_updated = false;
|
||||
|
||||
for (int esc_index = 0; esc_index < batch_size ; esc_index++) {
|
||||
msg.failure_flags[esc_index] = esc_status.esc[esc_index].failures;
|
||||
msg.error_count[esc_index] = esc_status.esc[esc_index].esc_errorcount;
|
||||
msg.temperature[esc_index] = static_cast<int16_t>(esc_status.esc[esc_index].esc_temperature *
|
||||
100.f); // convert to centiDegrees
|
||||
for (int j = 0; j < ESCS_PER_MSG; j++) {
|
||||
|
||||
EscInfo &esc = _escs[i * ESCS_PER_MSG + j];
|
||||
|
||||
msg.info |= (esc.online << j);
|
||||
|
||||
if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) {
|
||||
msg.failure_flags[j] = esc.failure_flags;
|
||||
msg.error_count[j] = esc.error_count;
|
||||
msg.temperature[j] = esc.temperature;
|
||||
atleast_one_esc_updated = true;
|
||||
}
|
||||
|
||||
mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
|
||||
}
|
||||
|
||||
return true;
|
||||
if (atleast_one_esc_updated) {
|
||||
mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
|
||||
updated = true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
return updated;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -34,7 +34,9 @@
|
||||
#ifndef ESC_STATUS_HPP
|
||||
#define ESC_STATUS_HPP
|
||||
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
class MavlinkStreamESCStatus : public MavlinkStream
|
||||
{
|
||||
@ -49,46 +51,88 @@ public:
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0;
|
||||
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
return _esc_status_subs.advertised_count() * message_size;
|
||||
}
|
||||
|
||||
private:
|
||||
explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||
|
||||
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
|
||||
uint8_t _number_of_batches{0};
|
||||
uORB::SubscriptionMultiArray<esc_status_s> _esc_status_subs{ORB_ID::esc_status};
|
||||
|
||||
static constexpr uint8_t MAX_ESC_OUTPUTS = 12; // See output_functions.hpp
|
||||
static constexpr uint8_t ESCS_PER_MSG = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN;
|
||||
static constexpr uint8_t MAX_NUM_MSGS = MAX_ESC_OUTPUTS / ESCS_PER_MSG;
|
||||
static constexpr hrt_abstime ESC_TIMEOUT = 100000;
|
||||
|
||||
struct EscStatus {
|
||||
hrt_abstime timestamp;
|
||||
int32_t rpm;
|
||||
float voltage;
|
||||
float current;
|
||||
};
|
||||
|
||||
EscStatus _escs[MAX_ESC_OUTPUTS] = {};
|
||||
|
||||
void update_data() override
|
||||
{
|
||||
int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
|
||||
|
||||
for (int i = 0; i < subscriber_count; i++) {
|
||||
esc_status_s esc = {};
|
||||
|
||||
if (_esc_status_subs[i].update(&esc)) {
|
||||
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
|
||||
|
||||
bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) &&
|
||||
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12);
|
||||
|
||||
if (is_motor) {
|
||||
// Map OutputFunction number to index
|
||||
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
|
||||
_escs[index].timestamp = esc.esc[j].timestamp;
|
||||
_escs[index].rpm = esc.esc[j].esc_rpm;
|
||||
_escs[index].voltage = esc.esc[j].esc_voltage;
|
||||
_escs[index].current = esc.esc[j].esc_current;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool send() override
|
||||
{
|
||||
static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN;
|
||||
esc_status_s esc_status;
|
||||
bool updated = false;
|
||||
|
||||
if (_esc_status_sub.update(&esc_status)) {
|
||||
mavlink_esc_status_t msg{};
|
||||
for (int i = 0; i < MAX_NUM_MSGS; i++) {
|
||||
|
||||
msg.time_usec = esc_status.timestamp;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc
|
||||
_number_of_batches = ceilf((float)esc_status.esc_count / batch_size);
|
||||
mavlink_esc_status_t msg = {};
|
||||
msg.index = i * ESCS_PER_MSG;
|
||||
msg.time_usec = now;
|
||||
|
||||
for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) {
|
||||
msg.index = batch_number * batch_size;
|
||||
bool atleast_one_esc_updated = false;
|
||||
|
||||
for (int esc_index = 0; esc_index < batch_size
|
||||
&& msg.index + esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {
|
||||
msg.rpm[esc_index] = esc_status.esc[msg.index + esc_index].esc_rpm;
|
||||
msg.voltage[esc_index] = esc_status.esc[msg.index + esc_index].esc_voltage;
|
||||
msg.current[esc_index] = esc_status.esc[msg.index + esc_index].esc_current;
|
||||
for (int j = 0; j < ESCS_PER_MSG; j++) {
|
||||
|
||||
EscStatus &esc = _escs[i * ESCS_PER_MSG + j];
|
||||
|
||||
if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) {
|
||||
msg.rpm[j] = esc.rpm;
|
||||
msg.voltage[j] = esc.voltage;
|
||||
msg.current[j] = esc.current;
|
||||
atleast_one_esc_updated = true;
|
||||
}
|
||||
|
||||
mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
|
||||
}
|
||||
|
||||
return true;
|
||||
if (atleast_one_esc_updated) {
|
||||
mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
|
||||
updated = true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
return updated;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user