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

AP_KDECAN: new CAN protocol 

This library adds support for the KDE ESC supporting CAN
master
Francisco Ferreira 7 years ago
parent
2f1f35ef03
commit
79bb3decfc
No known key found for this signature in database
GPG Key ID: F63C20A6773E787E
5 changed files with 1269 additions and 0 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. 754
      libraries/AP_KDECAN/AP_KDECAN.cpp
  2. 137
      libraries/AP_KDECAN/AP_KDECAN.h
  3. 358
      libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp
  4. 13
      libraries/AP_KDECAN/examples/KDECAN_sniffer/README.md
  5. 7
      libraries/AP_KDECAN/examples/KDECAN_sniffer/wscript

754
libraries/AP_KDECAN/AP_KDECAN.cpp
Unescape View File

@ -0,0 +1,754 @@ @@ -0,0 +1,754 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_KDECAN.cpp
*
* Author: Francisco Ferreira
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_Common/AP_Common.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Math/AP_Math.h>
#include <AP_Motors/AP_Motors.h>
#include "AP_KDECAN.h"
extern const AP_HAL::HAL& hal;
#define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)
#define DEFAULT_NUM_POLES 14
// table of user settable CAN bus parameters
const AP_Param::GroupInfo AP_KDECAN::var_info[] = {
// @Param: NPOLE
// @DisplayName: Number of motor poles
// @Description: Sets the number of motor poles to calculate the correct RPM value
AP_GROUPINFO("NPOLE", 1, AP_KDECAN, _num_poles, DEFAULT_NUM_POLES),
AP_GROUPEND
};
AP_KDECAN::AP_KDECAN()
{
AP_Param::setup_object_defaults(this, var_info);
debug_can(2, "KDECAN: constructed\n\r");
}
AP_KDECAN *AP_KDECAN::get_kdecan(uint8_t driver_index)
{
if (driver_index >= AP::can().get_num_drivers() ||
AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_KDECAN) {
return nullptr;
}
return static_cast<AP_KDECAN*>(AP::can().get_driver(driver_index));
}
void AP_KDECAN::init(uint8_t driver_index)
{
_driver_index = driver_index;
debug_can(2, "KDECAN: starting init\n\r");
if (_initialized) {
debug_can(1, "KDECAN: already initialized\n\r");
return;
}
// get CAN manager instance
AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
if (can_mgr == nullptr) {
debug_can(1, "KDECAN: no mgr for this driver\n\r");
return;
}
if (!can_mgr->is_initialized()) {
debug_can(1, "KDECAN: mgr not initialized\n\r");
return;
}
// store pointer to CAN driver
_can_driver = can_mgr->get_driver();
if (_can_driver == nullptr) {
debug_can(1, "KDECAN: no CAN driver\n\r");
return;
}
// find available KDE ESCs
frame_id_t id = { { .object_address = ESC_INFO_OBJ_ADDR,
.destination_id = BROADCAST_NODE_ID,
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 };
if(!_can_driver->getIface(CAN_IFACE_INDEX)->send(frame, uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1000), 0)) {
debug_can(1, "KDECAN: couldn't send discovery message\n\r");
return;
}
debug_can(2, "KDECAN: discovery message sent\n\r");
uint32_t start = AP_HAL::millis();
// wait 1 second for answers
while (AP_HAL::millis() - start < 1000) {
uavcan::CanFrame esc_id_frame {};
uavcan::MonotonicTime time {};
uavcan::UtcTime utc_time {};
uavcan::CanIOFlags flags {};
int16_t n = _can_driver->getIface(CAN_IFACE_INDEX)->receive(esc_id_frame, time, utc_time, flags);
if (n != 1) {
continue;
}
if (!esc_id_frame.isExtended()) {
continue;
}
if (esc_id_frame.dlc != 5) {
continue;
}
id.value = esc_id_frame.id & uavcan::CanFrame::MaskExtID;
if (id.source_id == BROADCAST_NODE_ID ||
id.source_id >= (KDECAN_MAX_NUM_ESCS + ESC_NODE_ID_FIRST) ||
id.destination_id != AUTOPILOT_NODE_ID ||
id.object_address != ESC_INFO_OBJ_ADDR) {
continue;
}
_esc_present_bitmask |= (1 << (id.source_id - ESC_NODE_ID_FIRST));
_esc_max_node_id = id.source_id - ESC_NODE_ID_FIRST + 1;
debug_can(2, "KDECAN: found ESC id %u\n\r", id.source_id);
}
snprintf(_thread_name, sizeof(_thread_name), "kdecan_%u", driver_index);
// start thread for receiving and sending CAN frames
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
debug_can(1, "KDECAN: couldn't create thread\n\r");
return;
}
_initialized = true;
debug_can(2, "KDECAN: init done\n\r");
return;
}
void AP_KDECAN::loop()
{
uavcan::MonotonicTime timeout;
uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 };
const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { };
select_frames[CAN_IFACE_INDEX] = &empty_frame;
uint16_t output_buffer[KDECAN_MAX_NUM_ESCS] {};
enumeration_state_t enumeration_state = _enumeration_state;
uint64_t enumeration_start = 0;
uint8_t enumeration_esc_num = 0;
const uint32_t LOOP_INTERVAL_US = MIN(AP::scheduler().get_loop_period_us(), SET_PWM_MIN_INTERVAL_US);
uint64_t pwm_last_sent = 0;
uint8_t sending_esc_num = 0;
uint64_t telemetry_last_request = 0;
while (true) {
if (!_initialized) {
debug_can(2, "KDECAN: not initialized\n\r");
hal.scheduler->delay_microseconds(2000);
continue;
}
uavcan::CanSelectMasks inout_mask;
uint64_t now = AP_HAL::micros64();
// get latest enumeration state set from GCS
if (_enum_sem.take(1)) {
enumeration_state = _enumeration_state;
_enum_sem.give();
} else {
debug_can(2, "KDECAN: failed to get enumeration semaphore on loop\n\r");
}
if (enumeration_state != ENUMERATION_STOPPED) {
// check if enumeration timed out
if (enumeration_start != 0 && now - enumeration_start >= ENUMERATION_TIMEOUT_MS * 1000) {
enumeration_start = 0;
WITH_SEMAPHORE(_enum_sem);
// check if enumeration state didn't change or was set to stop
if (enumeration_state == _enumeration_state || _enumeration_state == ENUMERATION_STOP) {
enumeration_state = _enumeration_state = ENUMERATION_STOPPED;
}
continue;
}
timeout = uavcan::MonotonicTime::fromUSec(now + 1000);
switch (enumeration_state) {
case ENUMERATION_START: {
inout_mask.write = 1 << CAN_IFACE_INDEX;
// send broadcast frame to start enumeration
frame_id_t id = { { .object_address = ENUM_OBJ_ADDR,
.destination_id = BROADCAST_NODE_ID,
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
be16_t data = htobe16((uint16_t) ENUMERATION_TIMEOUT_MS);
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) };
uavcan::CanSelectMasks in_mask = inout_mask;
select_frames[CAN_IFACE_INDEX] = &frame;
// wait for write space to be available
_can_driver->select(inout_mask, select_frames, timeout);
select_frames[CAN_IFACE_INDEX] = &empty_frame;
if (in_mask.write & inout_mask.write) {
now = AP_HAL::micros64();
timeout = uavcan::MonotonicTime::fromUSec(now + ENUMERATION_TIMEOUT_MS * 1000);
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
if (res == 1) {
enumeration_start = now;
enumeration_esc_num = 0;
_esc_present_bitmask = 0;
_esc_max_node_id = 0;
WITH_SEMAPHORE(_enum_sem);
if (enumeration_state == _enumeration_state) {
enumeration_state = _enumeration_state = ENUMERATION_RUNNING;
}
} else if (res == 0) {
debug_can(1, "KDECAN: strange buffer full when starting ESC enumeration\n\r");
break;
} else {
debug_can(1, "KDECAN: error sending message to start ESC enumeration, result %d\n\r", res);
break;
}
} else {
break;
}
FALLTHROUGH;
}
case ENUMERATION_RUNNING: {
inout_mask.read = 1 << CAN_IFACE_INDEX;
inout_mask.write = 0;
// wait for enumeration messages from ESCs
uavcan::CanSelectMasks in_mask = inout_mask;
_can_driver->select(inout_mask, select_frames, timeout);
if (in_mask.read & inout_mask.read) {
uavcan::CanFrame recv_frame;
uavcan::MonotonicTime time;
uavcan::UtcTime utc_time;
uavcan::CanIOFlags flags {};
int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags);
if (res == 1) {
if (time.toUSec() < enumeration_start) {
// old message
break;
}
frame_id_t id { .value = recv_frame.id & uavcan::CanFrame::MaskExtID };
if (id.object_address == UPDATE_NODE_ID_OBJ_ADDR) {
// reply from setting new node ID
_esc_present_bitmask |= 1 << (id.source_id - ESC_NODE_ID_FIRST);
_esc_max_node_id = MAX(_esc_max_node_id, id.source_id - ESC_NODE_ID_FIRST + 1);
break;
} else if (id.object_address != ENUM_OBJ_ADDR) {
// discardable frame, only looking for enumeration
break;
}
// try to set node ID for the received ESC
while (AP_HAL::micros64() - enumeration_start < ENUMERATION_TIMEOUT_MS * 1000) {
inout_mask.read = 0;
inout_mask.write = 1 << CAN_IFACE_INDEX;
// wait for write space to be available
in_mask = inout_mask;
_can_driver->select(inout_mask, select_frames, timeout);
if (in_mask.write & inout_mask.write) {
id = { { .object_address = UPDATE_NODE_ID_OBJ_ADDR,
.destination_id = uint8_t(enumeration_esc_num + ESC_NODE_ID_FIRST),
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
uavcan::CanFrame send_frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &recv_frame.data, recv_frame.dlc };
timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000);
res = _can_driver->getIface(CAN_IFACE_INDEX)->send(send_frame, timeout, 0);
if (res == 1) {
enumeration_esc_num++;
break;
} else if (res == 0) {
debug_can(1, "KDECAN: strange buffer full when setting ESC node ID\n\r");
} else {
debug_can(1, "KDECAN: error sending message to set ESC node ID, result %d\n\r", res);
}
}
}
} else if (res == 0) {
debug_can(1, "KDECAN: strange failed read when getting ESC enumeration message\n\r");
} else {
debug_can(1, "KDECAN: error receiving ESC enumeration message, result %d\n\r", res);
}
}
break;
}
case ENUMERATION_STOP: {
inout_mask.write = 1 << CAN_IFACE_INDEX;
// send broadcast frame to stop enumeration
frame_id_t id = { { .object_address = ENUM_OBJ_ADDR,
.destination_id = BROADCAST_NODE_ID,
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
le16_t data = htole16((uint16_t) ENUMERATION_TIMEOUT_MS);
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) };
uavcan::CanSelectMasks in_mask = inout_mask;
select_frames[CAN_IFACE_INDEX] = &frame;
// wait for write space to be available
_can_driver->select(inout_mask, select_frames, timeout);
select_frames[CAN_IFACE_INDEX] = &empty_frame;
if (in_mask.write & inout_mask.write) {
timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000);
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
if (res == 1) {
enumeration_start = 0;
WITH_SEMAPHORE(_enum_sem);
if (enumeration_state == _enumeration_state) {
enumeration_state = _enumeration_state = ENUMERATION_STOPPED;
}
} else if (res == 0) {
debug_can(1, "KDECAN: strange buffer full when stop ESC enumeration\n\r");
} else {
debug_can(1, "KDECAN: error sending message to stop ESC enumeration, result %d\n\r", res);
}
}
break;
}
case ENUMERATION_STOPPED:
default:
debug_can(2, "KDECAN: something wrong happened, shouldn't be here, enumeration state: %u\n\r", enumeration_state);
break;
}
continue;
}
if (!_esc_present_bitmask) {
debug_can(1, "KDECAN: no valid ESC present");
hal.scheduler->delay(1000);
continue;
}
// always look for received frames
inout_mask.read = 1 << CAN_IFACE_INDEX;
timeout = uavcan::MonotonicTime::fromUSec(now + LOOP_INTERVAL_US);
// check if:
// - is currently sending throttle frames, OR
// - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR
// - it is time to send throttle frames again, regardless of new output values, OR
// - it is time to ask for telemetry information
if (sending_esc_num > 0 ||
(_new_output.load(std::memory_order_acquire) && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) ||
(pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US)) ||
(now - telemetry_last_request > TELEMETRY_INTERVAL_US)) {
inout_mask.write = 1 << CAN_IFACE_INDEX;
} else { // don't need to send frame, choose the maximum time we'll wait for receiving a frame
uint64_t next_action = MIN(now + LOOP_INTERVAL_US, telemetry_last_request + TELEMETRY_INTERVAL_US);
if (pwm_last_sent != 0) {
next_action = MIN(next_action, pwm_last_sent + SET_PWM_MIN_INTERVAL_US);
}
timeout = uavcan::MonotonicTime::fromUSec(next_action);
}
// wait for write space or receive frame
uavcan::CanSelectMasks in_mask = inout_mask;
_can_driver->select(inout_mask, select_frames, timeout);
if (in_mask.read & inout_mask.read) {
uavcan::CanFrame frame;
uavcan::MonotonicTime time;
uavcan::UtcTime utc_time;
uavcan::CanIOFlags flags {};
int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(frame, time, utc_time, flags);
if (res == 1) {
frame_id_t id { .value = frame.id & uavcan::CanFrame::MaskExtID };
// check if frame is valid: directed at autopilot, doesn't come from broadcast and ESC was detected before
if (id.destination_id == AUTOPILOT_NODE_ID &&
id.source_id != BROADCAST_NODE_ID &&
(1 << (id.source_id - ESC_NODE_ID_FIRST) & _esc_present_bitmask)) {
switch (id.object_address) {
case TELEMETRY_OBJ_ADDR: {
if (frame.dlc != 8) {
break;
}
if (!_telem_sem.take(1)) {
debug_can(2, "KDECAN: failed to get telemetry semaphore on write\n\r");
break;
}
_telemetry[id.source_id - ESC_NODE_ID_FIRST].time = time.toUSec();
_telemetry[id.source_id - ESC_NODE_ID_FIRST].voltage = frame.data[0] << 8 | frame.data[1];
_telemetry[id.source_id - ESC_NODE_ID_FIRST].current = frame.data[2] << 8 | frame.data[3];
_telemetry[id.source_id - ESC_NODE_ID_FIRST].rpm = frame.data[4] << 8 | frame.data[5];
_telemetry[id.source_id - ESC_NODE_ID_FIRST].temp = frame.data[6];
_telemetry[id.source_id - ESC_NODE_ID_FIRST].new_data = true;
_telem_sem.give();
break;
}
default:
// discard frame
break;
}
}
}
}
if (in_mask.write & inout_mask.write) {
now = AP_HAL::micros64();
bool new_output = _new_output.load(std::memory_order_acquire);
if (sending_esc_num > 0) {
// currently sending throttle frames, check it didn't timeout
if (now - pwm_last_sent > SET_PWM_TIMEOUT_US) {
debug_can(2, "KDECAN: timed-out after sending frame to ESC with ID %d\n\r", sending_esc_num - 1);
sending_esc_num = 0;
}
}
if (sending_esc_num == 0 && new_output) {
if (!_rc_out_sem.take(1)) {
debug_can(2, "KDECAN: failed to get PWM semaphore on read\n\r");
continue;
}
memcpy(output_buffer, _scaled_output, KDECAN_MAX_NUM_ESCS * sizeof(uint16_t));
_rc_out_sem.give();
}
// check if:
// - is currently sending throttle frames, OR
// - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR
// - it is time to send throttle frames again, regardless of new output values
if (sending_esc_num > 0 ||
(new_output && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) ||
(pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US))) {
for (uint8_t esc_num = sending_esc_num; esc_num < _esc_max_node_id; esc_num++) {
if ((_esc_present_bitmask & (1 << esc_num)) == 0) {
continue;
}
be16_t kde_pwm = htobe16(output_buffer[esc_num]);
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
kde_pwm = 0;
}
frame_id_t id = { { .object_address = SET_PWM_OBJ_ADDR,
.destination_id = uint8_t(esc_num + ESC_NODE_ID_FIRST),
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &kde_pwm, sizeof(kde_pwm) };
if (esc_num == 0) {
timeout = uavcan::MonotonicTime::fromUSec(now + SET_PWM_TIMEOUT_US);
} else {
timeout = uavcan::MonotonicTime::fromUSec(pwm_last_sent + SET_PWM_TIMEOUT_US);
}
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
if (res == 1) {
if (esc_num == 0) {
pwm_last_sent = now;
if (new_output) {
_new_output.store(false, std::memory_order_release);
}
}
sending_esc_num = (esc_num + 1) % _esc_max_node_id;
} else if (res == 0) {
debug_can(1, "KDECAN: strange buffer full when sending message to ESC with ID %d\n\r", esc_num + ESC_NODE_ID_FIRST);
} else {
debug_can(1, "KDECAN: error sending message to ESC with ID %d, result %d\n\r", esc_num + ESC_NODE_ID_FIRST, res);
}
break;
}
} else if (now - telemetry_last_request > TELEMETRY_INTERVAL_US) {
// broadcast telemetry request frame
frame_id_t id = { { .object_address = TELEMETRY_OBJ_ADDR,
.destination_id = BROADCAST_NODE_ID,
.source_id = AUTOPILOT_NODE_ID,
.priority = 0,
.unused = 0 } };
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 };
timeout = uavcan::MonotonicTime::fromUSec(now + TELEMETRY_TIMEOUT_US);
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
if (res == 1) {
telemetry_last_request = now;
} else if (res == 0) {
debug_can(1, "KDECAN: strange buffer full when sending message requesting telemetry\n\r");
} else {
debug_can(1, "KDECAN: error sending message requesting telemetry, result %d\n\r", res);
}
}
}
}
}
void AP_KDECAN::update()
{
if (_rc_out_sem.take(1)) {
for (uint8_t i = 0; i < KDECAN_MAX_NUM_ESCS; i++) {
if ((_esc_present_bitmask & (1 << i)) == 0) {
continue;
}
SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i);
if (SRV_Channels::function_assigned(motor_function)) {
float norm_output = SRV_Channels::get_output_norm(motor_function);
_scaled_output[i] = uint16_t((norm_output + 1.0f) / 2.0f * 2000.0f);
} else {
_scaled_output[i] = 0;
}
}
_rc_out_sem.give();
_new_output.store(true, std::memory_order_release);
} else {
debug_can(2, "KDECAN: failed to get PWM semaphore on write\n\r");
}
DataFlash_Class *df = DataFlash_Class::instance();
if (df == nullptr || !df->should_log(0xFFFFFFFF)) {
return;
}
if (!_telem_sem.take(1)) {
debug_can(2, "KDECAN: failed to get telemetry semaphore on DF read\n\r");
return;
}
telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS] {};
for (uint8_t i = 0; i < _esc_max_node_id; i++) {
if (_telemetry[i].new_data) {
telem_buffer[i] = _telemetry[i];
_telemetry[i].new_data = false;
}
}
_telem_sem.give();
uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES;
// log ESC telemetry data
for (uint8_t i = 0; i < _esc_max_node_id; i++) {
if (telem_buffer[i].new_data) {
struct log_Esc pkt = {
LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC1_MSG+i)),
time_us : telem_buffer[i].time,
rpm : int32_t(telem_buffer[i].rpm * 60UL * 2 / num_poles * 100),
voltage : telem_buffer[i].voltage,
current : telem_buffer[i].current,
temperature : int16_t(telem_buffer[i].temp * 100U),
current_tot : 0
};
df->WriteBlock(&pkt, sizeof(pkt));
}
}
}
bool AP_KDECAN::pre_arm_check(const char* &reason)
{
if (!_enum_sem.take(1)) {
debug_can(2, "KDECAN: failed to get enumeration semaphore on read\n\r");
reason = "KDECAN enumeration state unknown";
return false;
}
if (_enumeration_state != ENUMERATION_STOPPED) {
reason = "KDECAN enumeration running";
_enum_sem.give();
return false;
}
_enum_sem.give();
uint16_t motors_mask = 0;
AP_Motors *motors = AP_Motors::get_instance();
if (motors) {
motors_mask = motors->get_motor_mask();
}
uint8_t num_expected_motors = __builtin_popcount(motors_mask);
uint8_t num_present_escs = __builtin_popcount(_esc_present_bitmask);
if (num_present_escs < num_expected_motors) {
reason = "Not enough KDECAN ESCs detected";
return false;
}
if (num_present_escs > num_expected_motors) {
reason = "Too many KDECAN ESCs detected";
return false;
}
if (_esc_max_node_id != num_expected_motors) {
reason = "Wrong KDECAN node IDs, run enumeration";
return false;
}
return true;
}
void AP_KDECAN::send_mavlink(uint8_t chan)
{
if (!_telem_sem.take(1)) {
debug_can(2, "KDECAN: failed to get telemetry semaphore on MAVLink read\n\r");
return;
}
telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS];
memcpy(telem_buffer, _telemetry, sizeof(telemetry_info_t) * KDECAN_MAX_NUM_ESCS);
_telem_sem.give();
uint16_t voltage[4] {};
uint16_t current[4] {};
uint16_t rpm[4] {};
uint8_t temperature[4] {};
uint16_t totalcurrent[4] {};
uint16_t count[4] {};
uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES;
uint64_t now = AP_HAL::micros64();
for (uint8_t i = 0; i < _esc_max_node_id && i < 8; i++) {
uint8_t idx = i % 4;
if (telem_buffer[i].time && (now - telem_buffer[i].time < 1000000)) {
voltage[idx] = telem_buffer[i].voltage;
current[idx] = telem_buffer[i].current;
rpm[idx] = uint16_t(telem_buffer[i].rpm * 60UL * 2 / num_poles);
temperature[idx] = telem_buffer[i].temp;
} else {
voltage[idx] = 0;
current[idx] = 0;
rpm[idx] = 0;
temperature[idx] = 0;
}
if (idx == 3 || i == _esc_max_node_id - 1) {
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)chan, ESC_TELEMETRY_1_TO_4)) {
return;
}
if (i < 4) {
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count);
} else {
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count);
}
}
}
}
bool AP_KDECAN::run_enumeration(bool start_stop)
{
if (!_enum_sem.take(1)) {
debug_can(2, "KDECAN: failed to get enumeration semaphore on write\n\r");
return false;
}
if (start_stop) {
_enumeration_state = ENUMERATION_START;
} else if (_enumeration_state != ENUMERATION_STOPPED) {
_enumeration_state = ENUMERATION_STOP;
}
_enum_sem.give();
return true;
}
#endif // HAL_WITH_UAVCAN

137
libraries/AP_KDECAN/AP_KDECAN.h
Unescape View File

@ -0,0 +1,137 @@ @@ -0,0 +1,137 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_KDECAN.h
*
* Author: Francisco Ferreira
*/
#pragma once
#include <AP_HAL/CAN.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <atomic>
// there are 12 motor functions in SRV_Channel but CAN driver can't keep up
#define KDECAN_MAX_NUM_ESCS 8
class AP_KDECAN : public AP_HAL::CANProtocol {
public:
AP_KDECAN();
/* Do not allow copies */
AP_KDECAN(const AP_KDECAN &other) = delete;
AP_KDECAN &operator=(const AP_KDECAN&) = delete;
static const struct AP_Param::GroupInfo var_info[];
// Return KDECAN from @driver_index or nullptr if it's not ready or doesn't exist
static AP_KDECAN *get_kdecan(uint8_t driver_index);
void init(uint8_t driver_index) override;
// called from SRV_Channels
void update();
// check that arming can happen
bool pre_arm_check(const char* &reason);
// send MAVLink telemetry packets
void send_mavlink(uint8_t chan);
// caller checks that vehicle isn't armed
// start_stop: true to start, false to stop
bool run_enumeration(bool start_stop);
private:
void loop();
bool _initialized;
char _thread_name[9];
uint8_t _driver_index;
uavcan::ICanDriver* _can_driver;
AP_Int8 _num_poles;
// ESC detected information
uint16_t _esc_present_bitmask;
uint8_t _esc_max_node_id;
// enumeration
HAL_Semaphore _enum_sem;
enum enumeration_state_t : uint8_t {
ENUMERATION_STOPPED,
ENUMERATION_START,
ENUMERATION_STOP,
ENUMERATION_RUNNING
} _enumeration_state = ENUMERATION_STOPPED;
// PWM output
HAL_Semaphore _rc_out_sem;
std::atomic<bool> _new_output;
uint16_t _scaled_output[KDECAN_MAX_NUM_ESCS];
// telemetry input
HAL_Semaphore _telem_sem;
struct telemetry_info_t {
uint64_t time;
uint16_t voltage;
uint16_t current;
uint16_t rpm;
uint8_t temp;
bool new_data;
} _telemetry[KDECAN_MAX_NUM_ESCS];
union frame_id_t {
struct {
uint8_t object_address;
uint8_t destination_id;
uint8_t source_id;
uint8_t priority:5;
uint8_t unused:3;
};
uint32_t value;
};
static const uint8_t AUTOPILOT_NODE_ID = 0;
static const uint8_t BROADCAST_NODE_ID = 1;
static const uint8_t ESC_NODE_ID_FIRST = 2;
static const uint8_t ESC_INFO_OBJ_ADDR = 0;
static const uint8_t SET_PWM_OBJ_ADDR = 1;
static const uint8_t VOLTAGE_OBJ_ADDR = 2;
static const uint8_t CURRENT_OBJ_ADDR = 3;
static const uint8_t RPM_OBJ_ADDR = 4;
static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
static const uint8_t MCU_ID_OBJ_ADDR = 8;
static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
static const uint8_t ENUM_OBJ_ADDR = 10;
static const uint8_t TELEMETRY_OBJ_ADDR = 11;
static const uint16_t SET_PWM_MIN_INTERVAL_US = 2500;
static const uint32_t TELEMETRY_INTERVAL_US = 100000;
static const uint32_t SET_PWM_TIMEOUT_US = 2000;
static const uint16_t TELEMETRY_TIMEOUT_US = 500;
static const uint16_t ENUMERATION_TIMEOUT_MS = 30000;
static const uint8_t CAN_IFACE_INDEX = 0;
};

358
libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp
Unescape View File

@ -0,0 +1,358 @@ @@ -0,0 +1,358 @@
/*
simple KDECAN network sniffer as an ArduPilot firmware
*/
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#if (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) && HAL_WITH_UAVCAN
#include <AP_HAL/CAN.h>
#include <AP_HAL/Semaphores.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/CAN.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/CAN.h>
#endif
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#include <cinttypes>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define debug_can(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
#define NUM_ESCS 8
class KDECAN_sniffer {
public:
KDECAN_sniffer() {
for (uint8_t i = 0; i < NUM_ESCS; i++) {
_esc_info[i].mcu_id = 0xA5961824E7BD3C00 | i;
}
}
void init(void);
void loop(void);
void print_stats(void);
void send_enumeration(uint8_t num);
private:
uint8_t _driver_index = 0;
uint8_t _interface = 0;
uavcan::ICanDriver* _can_driver;
uint8_t _mask_received_pwm = 0;
struct esc_info {
uint8_t node_id;
uint64_t mcu_id;
uint64_t enum_timeout;
esc_info() : node_id(1), mcu_id(0), enum_timeout(0) {}
} _esc_info[NUM_ESCS];
uint8_t _max_node_id = 0;
static const uint8_t BROADCAST_NODE_ID = 1;
static const uint8_t ESC_INFO_OBJ_ADDR = 0;
static const uint8_t SET_PWM_OBJ_ADDR = 1;
static const uint8_t VOLTAGE_OBJ_ADDR = 2;
static const uint8_t CURRENT_OBJ_ADDR = 3;
static const uint8_t RPM_OBJ_ADDR = 4;
static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
static const uint8_t MCU_ID_OBJ_ADDR = 8;
static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
static const uint8_t START_ENUM_OBJ_ADDR = 10;
static const uint8_t TELEMETRY_OBJ_ADDR = 11;
};
static struct {
uint32_t frame_id;
uint32_t count;
} counters[100];
static void count_msg(uint32_t frame_id)
{
for (uint16_t i=0; i<ARRAY_SIZE(counters); i++) {
if (counters[i].frame_id == frame_id) {
counters[i].count++;
break;
}
if (counters[i].frame_id == 0) {
counters[i].frame_id = frame_id;
counters[i].count++;
break;
}
}
}
void KDECAN_sniffer::init(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
AP_HAL::CANManager* can_mgr = new ChibiOS::CANManager;
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
AP_HAL::CANManager* can_mgr = new Linux::CANManager;
#endif
if (can_mgr == nullptr) {
AP_HAL::panic("Couldn't allocate CANManager, something is very wrong");
}
const_cast <AP_HAL::HAL&> (hal).can_mgr[_driver_index] = can_mgr;
can_mgr->begin(1000000, _interface);
can_mgr->initialized(true);
if (!can_mgr->is_initialized()) {
debug_can("Can not initialised\n");
return;
}
_can_driver = can_mgr->get_driver();
if (_can_driver == nullptr) {
debug_can("KDECAN: no CAN driver\n\r");
return;
}
debug_can("KDECAN: init done\n\r");
}
void KDECAN_sniffer::loop(void)
{
if (_can_driver == nullptr) {
return;
}
uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 };
const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { &empty_frame };
uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);
uavcan::CanSelectMasks inout_mask;
inout_mask.read = 1 << _interface;
uavcan::CanSelectMasks in_mask = inout_mask;
_can_driver->select(inout_mask, select_frames, timeout);
if (in_mask.read & inout_mask.read) {
uavcan::CanFrame frame;
uavcan::MonotonicTime time;
uavcan::UtcTime utc_time;
uavcan::CanIOFlags flags {};
int16_t res = _can_driver->getIface(_interface)->receive(frame, time, utc_time, flags);
if (res == 1) {
uint32_t id = frame.id & uavcan::CanFrame::MaskExtID;
uint8_t object_address = id & 0xFF;
uint8_t esc_num = uint8_t((id >> 8) & 0xFF);
count_msg(id);
uint8_t i = 0;
uint8_t n = NUM_ESCS;
if (esc_num != BROADCAST_NODE_ID) {
for (; i < NUM_ESCS; i++) {
if (object_address == UPDATE_NODE_ID_OBJ_ADDR) {
if (_esc_info[i].mcu_id == be64toh(*((be64_t*) &(frame.data[0])))) {
n = i + 1;
break;
}
} else if (_esc_info[i].node_id == esc_num) {
n = i + 1;
break;
}
}
}
while (i < n) {
uavcan::CanFrame res_frame;
switch (object_address) {
case ESC_INFO_OBJ_ADDR: {
uint8_t info[5] { 1, 2, 3, 4, 0 };
res_frame.dlc = 5;
memcpy(res_frame.data, info, 5);
break;
}
case SET_PWM_OBJ_ADDR: {
if ((1 << (esc_num - 2) & _mask_received_pwm) && _mask_received_pwm != ((1 << _max_node_id) - 1)) {
count_msg(0xFFFFFFF0);
_mask_received_pwm = 0;
}
_mask_received_pwm |= 1 << (esc_num - 2);
if (_mask_received_pwm == ((1 << _max_node_id) - 1)) {
count_msg(0xFFFFFFFF);
_mask_received_pwm = 0;
}
res_frame.dlc = 0;
break;
}
case UPDATE_NODE_ID_OBJ_ADDR: {
if (_esc_info[i].enum_timeout != 0 && _esc_info[i].enum_timeout >= AP_HAL::micros64()) {
_esc_info[i].node_id = esc_num;
_max_node_id = MAX(_max_node_id, esc_num - 2 + 1);
hal.console->printf("Set node ID %d for ESC %d\n", esc_num, i);
}
_esc_info[i].enum_timeout = 0;
res_frame.dlc = 1;
memcpy(res_frame.data, &(_esc_info[i].node_id), 1);
break;
}
case START_ENUM_OBJ_ADDR: {
_esc_info[i].enum_timeout = AP_HAL::micros64() + be16toh(*((be16_t*) &(frame.data[0]))) * 1000;
hal.console->printf("Starting enumeration for ESC %d, timeout %" PRIu64 "\n", i, _esc_info[i].enum_timeout);
i++;
continue;
}
case TELEMETRY_OBJ_ADDR: {
uint8_t data[7] {};
*((be16_t*) &data[0]) = htobe16(get_random16());
*((be16_t*) &data[2]) = htobe16(get_random16());
*((be16_t*) &data[4]) = htobe16(get_random16());
data[6] = uint8_t(float(rand()) / RAND_MAX * 40.0f + 15);
res_frame.dlc = 7;
memcpy(res_frame.data, data, 7);
break;
}
case VOLTAGE_OBJ_ADDR:
case CURRENT_OBJ_ADDR:
case RPM_OBJ_ADDR:
case TEMPERATURE_OBJ_ADDR:
case GET_PWM_INPUT_OBJ_ADDR:
case GET_PWM_OUTPUT_OBJ_ADDR:
case MCU_ID_OBJ_ADDR:
default:
// discard frame
return;
}
res_frame.id = (_esc_info[i].node_id << 16) | object_address | uavcan::CanFrame::FlagEFF;
timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::millis() + 500);
int16_t res2 = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);
if (res2 == 1) {
i++;
}
}
}
}
}
void KDECAN_sniffer::print_stats(void)
{
hal.console->printf("%lu\n", AP_HAL::micros());
for (uint16_t i=0;i<100;i++) {
if (counters[i].frame_id == 0) {
break;
}
hal.console->printf("0x%08" PRIX32 ": %" PRIu32 "\n", counters[i].frame_id, counters[i].count);
counters[i].count = 0;
}
hal.console->printf("\n");
}
void KDECAN_sniffer::send_enumeration(uint8_t num)
{
if (_esc_info[num].enum_timeout == 0 || AP_HAL::micros64() > _esc_info[num].enum_timeout) {
_esc_info[num].enum_timeout = 0;
hal.console->printf("Not running enumeration for ESC %d\n", num);
return;
}
while (true) {
uint8_t mcu[8] {};
*((be64_t*) mcu) = htobe64(_esc_info[num].mcu_id);
uavcan::CanFrame res_frame { (_esc_info[num].node_id << 16) | START_ENUM_OBJ_ADDR | uavcan::CanFrame::FlagEFF,
mcu,
8 };
uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);
int16_t res = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);
if (res == 1) {
return;
}
}
}
static KDECAN_sniffer sniffer;
void setup(void)
{
hal.scheduler->delay(2000);
hal.console->printf("Starting KDECAN sniffer\n");
sniffer.init();
}
void loop(void)
{
sniffer.loop();
static uint32_t last_print_ms;
uint32_t now = AP_HAL::millis();
if (now - last_print_ms >= 1000) {
last_print_ms = now;
sniffer.print_stats();
}
if (hal.console->available() >= 3) {
char c = hal.console->read();
if (c == 'e') {
c = hal.console->read();
if (c == ' ') {
c = hal.console->read();
if (c >= '0' && c < '9') {
uint8_t num = c - '0';
sniffer.send_enumeration(num);
}
}
} else if (c == 'r') {
hal.console->printf("rebooting\n");
hal.scheduler->reboot(false);
}
}
// auto-reboot for --upload
if (hal.console->available() > 50) {
hal.console->printf("rebooting\n");
hal.scheduler->reboot(false);
}
}
AP_HAL_MAIN();
#else
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static void loop() { }
static void setup()
{
printf("Board not currently supported\n");
}
AP_HAL_MAIN();
#endif

13
libraries/AP_KDECAN/examples/KDECAN_sniffer/README.md
Unescape View File

@ -0,0 +1,13 @@ @@ -0,0 +1,13 @@
This is a KDECAN sniffer designed to run on an ArduPilot board. It can
be used to watch traffic on a KDECAN bus as well as simulate a 8 ESCs (macro changeable).
To build and upload for a Pixhawk style board run this:
```
./waf configure --board fmuv3
./waf --target examples/KDECAN_sniffer --upload
```
then connect on the USB console. You will see 1Hz packet stats.
You can respond to enumeration by writing 'e X', with X being the ESC number, zero index based.

7
libraries/AP_KDECAN/examples/KDECAN_sniffer/wscript
Unescape View File

@ -0,0 +1,7 @@ @@ -0,0 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_example(
use='ap',
)
Write Preview
Loading…
Cancel
Save