zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_PiccoloCAN: add ESC telemetry support 

remove send_esc_telemetry_mavlink()
log telemetry data in frontend
record volts, amps and consumption as floats
c415-sdk
Andy Piper 4 years ago committed by Andrew Tridgell
parent
45e1b56f17
commit
70fae36a89
2 changed files with 20 additions and 108 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 120
      libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
  2. 8
      libraries/AP_PiccoloCAN/AP_PiccoloCAN.h

120
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
Unescape View File

@ -245,8 +245,6 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout) @@ -245,8 +245,6 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout)
// called from SRV_Channels
void AP_PiccoloCAN::update()
{
uint64_t timestamp = AP_HAL::micros64();
/* Read out the ESC commands from the channel mixer */
for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
@ -263,106 +261,8 @@ void AP_PiccoloCAN::update() @@ -263,106 +261,8 @@ void AP_PiccoloCAN::update()
}
}
}
AP_Logger *logger = AP_Logger::get_singleton();
// Push received telemtry data into the logging system
if (logger && logger->logging_enabled()) {
WITH_SEMAPHORE(_telem_sem);
for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
PiccoloESC_Info_t &esc = _esc_info[i];
if (esc.newTelemetry) {
logger->Write_ESC(i, timestamp,
(int32_t) esc.rpm * 100,
esc.voltage,
esc.current,
(int16_t) esc.fetTemperature,
0, // TODO - Accumulated current
(int16_t) esc.motorTemperature);
esc.newTelemetry = false;
}
}
}
}
// send ESC telemetry messages over MAVLink
void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
{
// Arrays to store ESC telemetry data
uint8_t temperature[4] {};
uint16_t voltage[4] {};
uint16_t rpm[4] {};
uint16_t count[4] {};
uint16_t current[4] {};
uint16_t totalcurrent[4] {};
bool dataAvailable = false;
uint8_t idx = 0;
WITH_SEMAPHORE(_telem_sem);
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
// Calculate index within storage array
idx = (ii % 4);
PiccoloESC_Info_t &esc = _esc_info[idx];
// Has the ESC been heard from recently?
if (is_esc_present(ii)) {
dataAvailable = true;
temperature[idx] = esc.fetTemperature;
voltage[idx] = esc.voltage;
current[idx] = esc.current;
totalcurrent[idx] = 0;
rpm[idx] = esc.rpm;
count[idx] = 0;
} else {
temperature[idx] = 0;
voltage[idx] = 0;
current[idx] = 0;
totalcurrent[idx] = 0;
rpm[idx] = 0;
count[idx] = 0;
}
// Send ESC telemetry in groups of 4
if ((ii % 4) == 3) {
if (dataAvailable) {
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) {
continue;
}
switch (ii) {
case 3:
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
case 7:
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
case 11:
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
break;
default:
break;
}
}
dataAvailable = false;
}
}
}
// send ESC messages over CAN
void AP_PiccoloCAN::send_esc_messages(void)
{
@ -483,6 +383,7 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) @@ -483,6 +383,7 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
// Throw the packet against each decoding routine
if (decodeESC_StatusAPacket(&frame, &esc.mode, &esc.status, &esc.setpoint, &esc.rpm)) {
esc.newTelemetry = true;
update_rpm(addr, esc.rpm);
} else if (decodeESC_LegacyStatusAPacket(&frame, &esc.mode, &legacyStatus, &legacyWarnings, &legacyErrors, &esc.setpoint, &esc.rpm)) {
// The status / warning / error bits need to be converted to modern values
// Note: Not *all* of the modern status bits are available in the Gen-1 packet
@ -507,8 +408,22 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) @@ -507,8 +408,22 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
// There are no common error bits between the Gen-1 and Gen-2 ICD
} else if (decodeESC_StatusBPacket(&frame, &esc.voltage, &esc.current, &esc.dutyCycle, &esc.escTemperature, &esc.motorTemperature)) {
TelemetryData t {
.temperature_cdeg = int16_t(esc.escTemperature * 100),
.voltage = float(esc.voltage) * 0.01f,
.current = float(esc.current) * 0.01f,
};
update_telem_data(addr, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
esc.newTelemetry = true;
} else if (decodeESC_StatusCPacket(&frame, &esc.fetTemperature, &esc.pwmFrequency, &esc.timingAdvance)) {
TelemetryData t { };
t.motor_temp_cdeg = int16_t(esc.fetTemperature * 100);
update_telem_data(addr, t, AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
esc.newTelemetry = true;
} else if (decodeESC_WarningErrorStatusPacket(&frame, &esc.warnings, &esc.errors)) {
esc.newTelemetry = true;
@ -530,7 +445,6 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) @@ -530,7 +445,6 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
return result;
}
/**
* Check if a given ESC channel is "active" (has been configured correctly)
*/
@ -555,13 +469,13 @@ bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan) @@ -555,13 +469,13 @@ bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan)
/**
* Determine if an ESC is present on the CAN bus (has telemetry data been received)
*/
bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms)
bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) const
{
if (chan >= PICCOLO_CAN_MAX_NUM_ESC) {
return false;
}
PiccoloESC_Info_t &esc = _esc_info[chan];
const PiccoloESC_Info_t &esc = _esc_info[chan];
// No messages received from this ESC
if (esc.last_rx_msg_timestamp == 0) {

8
libraries/AP_PiccoloCAN/AP_PiccoloCAN.h
Unescape View File

@ -22,6 +22,7 @@ @@ -22,6 +22,7 @@
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include "piccolo_protocol/ESCPackets.h"
#include "piccolo_protocol/LegacyESCPackets.h"
@ -40,7 +41,7 @@ @@ -40,7 +41,7 @@
#define PICCOLO_MSG_RATE_HZ_MAX 500
#define PICCOLO_MSG_RATE_HZ_DEFAULT 50
class AP_PiccoloCAN : public AP_CANDriver
class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend
{
public:
AP_PiccoloCAN();
@ -79,14 +80,11 @@ public: @@ -79,14 +80,11 @@ public:
// called from SRV_Channels
void update();
// send ESC telemetry messages over MAVLink
void send_esc_telemetry_mavlink(uint8_t mav_chan);
// return true if a particular ESC is 'active' on the Piccolo interface
bool is_esc_channel_active(uint8_t chan);
// return true if a particular ESC has been detected
bool is_esc_present(uint8_t chan, uint64_t timeout_ms = 2000);
bool is_esc_present(uint8_t chan, uint64_t timeout_ms = 2000) const;
// return true if a particular ESC is enabled
bool is_esc_enabled(uint8_t chan);

Write Preview
Loading…
Cancel
Save