Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Decode and use ESC_TELEMETRY_X_TO_Y mavlink packets as an AP_ESC_Telem_Backend #19668

Closed
wants to merge 2 commits into from
Closed

Decode and use ESC_TELEMETRY_X_TO_Y mavlink packets as an AP_ESC_Telem_Backend #19668

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fb6c7f7
AP_ESC_Telem: decode and use ESC_TELEMETRY_X_TO_Y mavlink packets as …
amilcarlucas Jan 3, 2022
80b93b6
GCS_MAVLink: decode and use ESC_TELEMETRY_X_TO_Y mavlink packets as a…
amilcarlucas Jan 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Open in desktop
Expand Up @@ -16,6 +16,7 @@ static_assert(ESC_TELEM_MAX_ESCS > 0, "Cannot have 0 ESC telemetry instances");
class AP_ESC_Telem {
public:
friend class AP_ESC_Telem_Backend;
friend class MAVLink_routing;

AP_ESC_Telem();

Expand Down
100 changes: 100 additions & 0 deletions libraries/GCS_MAVLink/MAVLink_routing.cpp
View file
Open in desktop
Expand Up @@ -23,6 +23,7 @@
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include "GCS.h"
#include "MAVLink_routing.h"

Expand Down Expand Up @@ -135,6 +136,105 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess
return true;
}

#if HAL_WITH_ESC_TELEM
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is really in the wrong place, should be in AP_ESC_Telem_Backend, and only when enabled

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @tridge, Could you explain to me why these lines are in the wrong place? I have built this code and It works fine. I'm sorry. I'm a newbie for this.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

these messages are not targeted to a specific sysid, so I don't think they are safe to use in this way.
imagine 2 vehicles on the radio link, both generating these msgs, they would consume each others esc telem msgs and get the wrong RPMs to notch filter

AP_ESC_Telem& telem = AP::esc_telem();
if (msg.msgid == MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4) {
const uint32_t now = AP_HAL::millis();
constexpr uint16_t datamask = AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION;
mavlink_esc_telemetry_1_to_4_t mav;
mavlink_msg_esc_telemetry_1_to_4_get_voltage(&msg, mav.voltage);
mavlink_msg_esc_telemetry_1_to_4_get_current(&msg, mav.current);
mavlink_msg_esc_telemetry_1_to_4_get_totalcurrent(&msg, mav.totalcurrent);
mavlink_msg_esc_telemetry_1_to_4_get_rpm(&msg, mav.rpm);
mavlink_msg_esc_telemetry_1_to_4_get_count(&msg, mav.count);
mavlink_msg_esc_telemetry_1_to_4_get_temperature(&msg, mav.temperature);
for (int i = 0; i < 4; i++) {
AP_ESC_Telem_Backend::TelemetryData t {
.temperature_cdeg = int16_t(mav.temperature[i] * 100), // centi-degrees C, negative values allowed
.voltage = mav.voltage[i] * 0.01f, // Volt
.current = mav.current[i] * 0.01f, // Ampere
.consumption_mah = float(mav.totalcurrent[i]), // milli-Ampere.hours
.usage_s = 0, // usage seconds
.motor_temp_cdeg = 0, // centi-degrees C, negative values allowed
.last_update_ms = now, // last update time in miliseconds, determines whether active
.types = datamask, // telemetry types present
.count = mav.count[i], // number of times updated
};
// inject received MAVLink ESC telemetry data into the autopilot
telem.update_telem_data(i, t, datamask);
telem.update_rpm(i, mav.rpm[i], 0.0f);
}
return true;
}

if (msg.msgid == MAVLINK_MSG_ID_ESC_TELEMETRY_5_TO_8) {
const uint32_t now = AP_HAL::millis();
constexpr uint16_t datamask = AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION;
mavlink_esc_telemetry_5_to_8_t mav;
mavlink_msg_esc_telemetry_5_to_8_get_voltage(&msg, mav.voltage);
mavlink_msg_esc_telemetry_5_to_8_get_current(&msg, mav.current);
mavlink_msg_esc_telemetry_5_to_8_get_totalcurrent(&msg, mav.totalcurrent);
mavlink_msg_esc_telemetry_5_to_8_get_rpm(&msg, mav.rpm);
mavlink_msg_esc_telemetry_5_to_8_get_count(&msg, mav.count);
mavlink_msg_esc_telemetry_5_to_8_get_temperature(&msg, mav.temperature);
for (int i = 0; i < 4; i++) {
AP_ESC_Telem_Backend::TelemetryData t {
.temperature_cdeg = int16_t(mav.temperature[i] * 100), // centi-degrees C, negative values allowed
.voltage = mav.voltage[i] * 0.01f, // Volt
.current = mav.current[i] * 0.01f, // Ampere
.consumption_mah = float(mav.totalcurrent[i]), // milli-Ampere.hours
.usage_s = 0, // usage seconds
.motor_temp_cdeg = 0, // centi-degrees C, negative values allowed
.last_update_ms = now, // last update time in miliseconds, determines whether active
.types = datamask, // telemetry types present
.count = mav.count[i], // number of times updated
};
// inject received MAVLink ESC telemetry data into the autopilot
telem.update_telem_data(4+i, t, datamask);
telem.update_rpm(4+i, mav.rpm[i], 0.0f);
}
return true;
}

if (msg.msgid == MAVLINK_MSG_ID_ESC_TELEMETRY_9_TO_12) {
const uint32_t now = AP_HAL::millis();
constexpr uint16_t datamask = AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION;
mavlink_esc_telemetry_9_to_12_t mav;
mavlink_msg_esc_telemetry_9_to_12_get_voltage(&msg, mav.voltage);
mavlink_msg_esc_telemetry_9_to_12_get_current(&msg, mav.current);
mavlink_msg_esc_telemetry_9_to_12_get_totalcurrent(&msg, mav.totalcurrent);
mavlink_msg_esc_telemetry_9_to_12_get_rpm(&msg, mav.rpm);
mavlink_msg_esc_telemetry_9_to_12_get_count(&msg, mav.count);
mavlink_msg_esc_telemetry_9_to_12_get_temperature(&msg, mav.temperature);
for (int i = 0; i < 4; i++) {
AP_ESC_Telem_Backend::TelemetryData t {
.temperature_cdeg = int16_t(mav.temperature[i] * 100), // centi-degrees C, negative values allowed
.voltage = mav.voltage[i] * 0.01f, // Volt
.current = mav.current[i] * 0.01f, // Ampere
.consumption_mah = float(mav.totalcurrent[i]), // milli-Ampere.hours
.usage_s = 0, // usage seconds
.motor_temp_cdeg = 0, // centi-degrees C, negative values allowed
.last_update_ms = now, // last update time in miliseconds, determines whether active
.types = datamask, // telemetry types present
.count = mav.count[i], // number of times updated
};
// inject received MAVLink ESC telemetry data into the autopilot
telem.update_telem_data(8+i, t, datamask);
telem.update_rpm(8+i, mav.rpm[i], 0.0f);
}
return true;
}
#endif

// extract the targets for this packet
int16_t target_system = -1;
int16_t target_component = -1;
Expand Down