Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 17:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: add esc telemetry updates

Browse Source 
remove send_esc_telemetry_mavlink()
remove datastructures and semaphore obsoleted by AP_ESC_Telem* (<amilcar.lucas@iav.de>)
record volts, amps and consumption as floats
This commit is contained in:
Andy Piper 2021-02-27 16:44:01 +00:00 committed by Andrew Tridgell
parent 677d863401
commit f513b1d1c2
2 changed files with 14 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -137,9 +137,6 @@ static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_stat
UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[];
HAL_Semaphore AP_UAVCAN::_telem_sem;
AP_UAVCAN::AP_UAVCAN() :
_node_allocator()
@ -349,79 +346,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
}
#pragma GCC diagnostic pop
// send ESC telemetry messages over MAVLink
void AP_UAVCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
{
#ifndef HAL_NO_GCS
static const uint8_t MAV_ESC_GROUPS = 3;
static const uint8_t MAV_ESC_PER_GROUP = 4;
for (uint8_t i = 0; i < MAV_ESC_GROUPS; i++) {
// arrays to hold output
uint8_t temperature[MAV_ESC_PER_GROUP] {};
uint16_t voltage[MAV_ESC_PER_GROUP] {};
uint16_t current[MAV_ESC_PER_GROUP] {};
uint16_t current_tot[MAV_ESC_PER_GROUP] {};
uint16_t rpm[MAV_ESC_PER_GROUP] {};
uint16_t count[MAV_ESC_PER_GROUP] {};
// if at least one of the ESCs in the group is availabe, the group
// is considered to be available too, and will be sent over MAVlink
bool group_available = false;
// fill in output arrays of ESCs sensors with available data.
for (uint8_t j = 0; j < MAV_ESC_PER_GROUP; j++) {
const uint8_t esc_idx = i * MAV_ESC_PER_GROUP + j;
if (!is_esc_data_index_valid(esc_idx)) {
return;
}
WITH_SEMAPHORE(_telem_sem);
if (!_escs_data[esc_idx].available) {
continue;
}
_escs_data[esc_idx].available = false;
temperature[j] = _escs_data[esc_idx].temp;
voltage[j] = _escs_data[esc_idx].voltage;
current[j] = _escs_data[esc_idx].current;
current_tot[j] = 0; // currently not implemented
rpm[j] = _escs_data[esc_idx].rpm;
count[j] = _escs_data[esc_idx].count;
group_available = true;
}
if (!group_available) {
continue;
}
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) {
return;
}
// send messages
switch (i) {
case 0:
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
case 1:
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
case 2:
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
default:
break;
}
}
#endif // HAL_NO_GCS
}
void AP_UAVCAN::loop(void)
{
while (true) {
@ -866,31 +790,22 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb)
{
const uint8_t esc_index = cb.msg->esc_index;
// log as CESC message
AP::logger().Write_ESCStatus(AP_HAL::native_micros64(),
cb.msg->esc_index,
cb.msg->error_count,
cb.msg->voltage,
cb.msg->current,
cb.msg->temperature - C_TO_KELVIN,
cb.msg->rpm,
cb.msg->power_rating_pct);
WITH_SEMAPHORE(_telem_sem);
if (!is_esc_data_index_valid(esc_index)) {
return;
}
esc_data &esc = _escs_data[esc_index];
esc.available = true;
esc.temp = (cb.msg->temperature - C_TO_KELVIN);
esc.voltage = cb.msg->voltage*100;
esc.current = cb.msg->current*100;
esc.rpm = cb.msg->rpm;
esc.count++;
TelemetryData t {
.temperature_cdeg = int16_t((cb.msg->temperature - C_TO_KELVIN) * 100),
.voltage = cb.msg->voltage,
.current = cb.msg->current,
};
ap_uavcan->update_rpm(esc_index, cb.msg->rpm);
ap_uavcan->update_telem_data(esc_index, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
}
bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {

23
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -27,6 +27,7 @@
#include <AP_CANManager/AP_CANDriver.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
@ -71,7 +72,7 @@ class DebugCb;
/*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
the Callback will invoke registery to register the node as separate backend.
the Callback will invoke registry to register the node as separate backend.
*/
#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
@ -84,7 +85,7 @@ class DebugCb;
DISABLE_W_CAST_FUNCTION_TYPE_POP \
}
class AP_UAVCAN : public AP_CANDriver {
class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
public:
AP_UAVCAN();
~AP_UAVCAN();
@ -96,9 +97,6 @@ public:
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
// send ESC telemetry messages over MAVLink
void send_esc_telemetry_mavlink(uint8_t mav_chan);
uavcan::Node<0>* get_node() { return _node; }
uint8_t get_driver_index() const { return _driver_index; }
@ -125,7 +123,7 @@ public:
public:
RegistryBinder() :
_uc(),
_uc(),
_ffunc(),
msg() {}
@ -226,19 +224,6 @@ private:
static HAL_Semaphore _telem_sem;
struct esc_data {
uint8_t temp;
uint16_t voltage;
uint16_t current;
uint16_t total_current;
uint16_t rpm;
uint16_t count; //count of telemetry packets received (wraps at 65535).
bool available;
};
static esc_data _escs_data[UAVCAN_SRV_NUMBER];
// safety status send state
uint32_t _last_safety_state_ms;