Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_CANManager/AP_CANTester.cpp

721 lines
26 KiB
C++
Raw Blame History

/*
* This file 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 file 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/>.
*
* Code by Siddharth Bharat Purohit
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_CANManager.h"
#include "AP_CANTester.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_CANTester_KDECAN.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo CANTester::var_info[] = {
// @Param: ID
// @DisplayName: CAN Test Index
// @Description: Selects the Index of Test that needs to be run recursively, this value gets reset to 0 at boot.
// @Range: 0 4
// @Values: 0:TEST_NONE, 1:TEST_LOOPBACK,2:TEST_BUSOFF_RECOVERY,3:TEST_UAVCAN_DNA,5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC, 7:TEST_UAVCAN_FD_ESC
// @User: Advanced
AP_GROUPINFO("ID", 1, CANTester, _test_id, 0),
// @Param: LPR8
// @DisplayName: CANTester LoopRate
// @Description: Selects the Looprate of Test methods
// @Units: us
// @User: Advanced
AP_GROUPINFO("LPR8", 2, CANTester, _loop_rate, 10000),
AP_GROUPEND
};
#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "CANTester", fmt, ##args); } while (0)
bool CANTester::add_interface(AP_HAL::CANIface* can_iface)
{
if (_num_ifaces >= HAL_NUM_CAN_IFACES) {
debug_can(AP_CANManager::LOG_ERROR, "Max Number of CanIfaces exceeded");
return false;
}
_can_ifaces[_num_ifaces] = can_iface;
if (_can_ifaces[_num_ifaces] == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "CAN driver not found");
return false;
}
if (!_can_ifaces[_num_ifaces]->is_initialized()) {
debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized");
return false;
}
_num_ifaces++;
return true;
}
void CANTester::init(uint8_t driver_index, bool enable_filters)
{
_driver_index = driver_index;
// Reset Test mask
_test_id.set_and_save(0);
debug_can(AP_CANManager::LOG_DEBUG, "starting init");
if (_initialized) {
debug_can(AP_CANManager::LOG_ERROR, "already initialized");
return;
}
if (_can_ifaces[0] == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "Interface not found");
return;
}
// kick start tester thread
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&CANTester::main_thread, void), "can_tester", 4096, AP_HAL::Scheduler::PRIORITY_CAN, 1)) {
debug_can(AP_CANManager::LOG_ERROR, "couldn't create thread");
return;
}
_initialized = true;
debug_can(AP_CANManager::LOG_DEBUG, "init done");
return;
}
// write frame on CAN bus
bool CANTester::write_frame(uint8_t iface, AP_HAL::CANFrame &out_frame, uint64_t timeout)
{
if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
return false;
}
// wait for space in buffer to send command
bool read_select = false;
bool write_select = true;
out_frame.id += iface; // distinguish between multiple ifaces
bool ret = _can_ifaces[iface]->select(read_select, write_select, &out_frame, AP_HAL::native_micros64() + timeout);
if (!ret || !write_select) {
return false;
}
uint64_t deadline = AP_HAL::native_micros64() + 2000000;
// hal.console->printf("%x TDEAD: %lu\n", out_frame.id, deadline);
// send frame and return success
return (_can_ifaces[iface]->send(out_frame, deadline, AP_HAL::CANIface::AbortOnError) == 1);
}
// read frame on CAN bus, returns true on success
bool CANTester::read_frame(uint8_t iface, AP_HAL::CANFrame &recv_frame, uint64_t timeout, AP_HAL::CANIface::CanIOFlags &flags)
{
if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
return false;
}
// wait for space in buffer to read
bool read_select = true;
bool write_select = false;
bool ret = _can_ifaces[iface]->select(read_select, write_select, nullptr, AP_HAL::native_micros64() + timeout);
if (!ret || !read_select) {
// return false if no data is available to read
return false;
}
uint64_t time;
// read frame and return success
return (_can_ifaces[iface]->receive(recv_frame, time, flags) == 1);
}
void CANTester::main_thread()
{
while (true) {
switch (_test_id) {
case CANTester::TEST_LOOPBACK:
if (_can_ifaces[1] != nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running Loopback Test*******");
if (test_loopback(_loop_rate)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Pass*******");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Fail*******");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Loopback Test with single iface");
}
break;
case CANTester::TEST_BUSOFF_RECOVERY:
if (_can_ifaces[1] != nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running Busoff Recovery Test********");
if (test_busoff_recovery()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Busoff Recovery Test with single iface");
}
break;
case CANTester::TEST_UAVCAN_DNA:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN DNA Test********");
if (test_uavcan_dna()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_DNA_TEST");
}
break;
case CANTester::TEST_KDE_CAN:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running KDE CAN Test********");
if (test_kdecan()) {
gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for TEST_KDE_CAN");
}
break;
case CANTester::TEST_UAVCAN_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********");
if (test_uavcan_esc(false)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST");
}
break;
case CANTester::TEST_UAVCAN_FD_ESC:
if (_can_ifaces[1] == nullptr) {
gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN FD ESC Test********");
if (test_uavcan_esc(true)) {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Pass********");
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Fail********");
}
} else {
gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_FD_ESC_TEST");
}
break;
default:
break;
}
for (uint8_t i = 0; i < 2; i++) {
if (_can_ifaces[i] != nullptr) {
_can_ifaces[i]->flush_tx();
}
}
hal.scheduler->delay(5000);
for (uint8_t i = 0; i < 2; i++) {
if (_can_ifaces[i] != nullptr) {
_can_ifaces[i]->clear_rx();
}
}
}
}
/*****************************************
* Loopback Test *
* ***************************************/
#define NUM_LOOPBACK_RUNS 1000UL
#define LOOPBACK_MAGIC 0x34567819UL
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define NUM_MAX_TX_FRAMES 1
#else
#define NUM_MAX_TX_FRAMES 64 // arbitrary value to max out the buffers
#endif
bool CANTester::test_loopback(uint32_t loop_rate)
{
AP_HAL::CANFrame frame;
AP_HAL::CANIface::CanIOFlags flags;
uint32_t num_loops = NUM_LOOPBACK_RUNS;
memset(&_loopback_stats[0], 0, sizeof(_loopback_stats[0]));
memset(&_loopback_stats[1], 0, sizeof(_loopback_stats[1]));
while (num_loops--) {
for (uint8_t i = 0; i < 2; i++) {
// Write as many frames as we can on an iface
for (uint32_t tx_frames = 0; tx_frames < NUM_MAX_TX_FRAMES; tx_frames++) {
create_loopback_frame(_loopback_stats[i], frame);
if (write_frame(i, frame, 0)) {
_loopback_stats[i].tx_seq++;
_loopback_stats[i].num_tx++;
} else {
break;
}
}
// Also read as much as we can from the second iface
while (true) {
reset_frame(frame);
if (read_frame((i+1)%2, frame, 0, flags)) {
if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
continue;
}
check_loopback_frame(_loopback_stats[i], frame);
_loopback_stats[i].num_rx++;
} else {
break;
}
}
}
hal.scheduler->delay_microseconds(loop_rate);
}
_can_ifaces[0]->flush_tx();
hal.scheduler->delay_microseconds(1000);
_can_ifaces[1]->flush_tx();
hal.scheduler->delay_microseconds(1000);
// flush the rx data still buffered in the interface
for (uint8_t i = 0; i < 2; i++) {
while (true) {
reset_frame(frame);
if (read_frame((i+1)%2, frame, 0, flags)) {
if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
continue;
}
check_loopback_frame(_loopback_stats[i], frame);
_loopback_stats[i].num_flushed_rx++;
} else {
break;
}
}
}
for (uint8_t i = 0; i < _num_ifaces; i++) {
DEV_PRINTF("Loopback Test Results %d->%d:\n", i, (i+1)%2);
DEV_PRINTF("num_tx: %lu, failed_tx: %lu\n",
(long unsigned int)_loopback_stats[i].num_tx, (long unsigned int)_loopback_stats[i].failed_tx);
DEV_PRINTF("num_rx: %lu, flushed_rx: %lu, bad_rx_data: %lu, bad_rx_seq: %lu\n",
(long unsigned int)_loopback_stats[i].num_rx, (long unsigned int)_loopback_stats[i].num_flushed_rx,
(long unsigned int)_loopback_stats[i].bad_rx_data, (long unsigned int)_loopback_stats[i].bad_rx_seq);
if (_loopback_stats[i].num_rx < 0.9f * _loopback_stats[i].num_tx ||
_loopback_stats[i].failed_tx || _loopback_stats[i].bad_rx_seq ||
_loopback_stats[i].bad_rx_data) {
return false;
}
}
return true;
}
void CANTester::reset_frame(AP_HAL::CANFrame& frame)
{
frame.id = 0;
memset(frame.data, 0, sizeof(frame.data));
frame.dlc = 0;
}
void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
frame.id = 13 | AP_HAL::CANFrame::FlagEFF;
frame.dlc = 8;
frame.setCanFD(stats.tx_seq%2); //every other frame is canfd if supported
memcpy(frame.data, &stats.tx_seq, sizeof(stats.tx_seq));
uint32_t loopback_magic = LOOPBACK_MAGIC;
memcpy(&frame.data[4], &loopback_magic, sizeof(loopback_magic));
}
void CANTester::check_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
if (frame.dlc != 8) {
stats.bad_rx_data++;
}
uint32_t loopback_magic = LOOPBACK_MAGIC;
if (memcmp(&frame.data[4], &loopback_magic, sizeof(loopback_magic)) != 0) {
stats.bad_rx_data++;
return;
}
uint16_t curr_seq;
memcpy(&curr_seq, frame.data, sizeof(curr_seq));
if (stats.next_valid_seq != curr_seq) {
stats.bad_rx_seq++;
}
stats.next_valid_seq = curr_seq + 1;
}
/*****************************************
* Busoff Recovery Test *
* ***************************************/
bool CANTester::test_busoff_recovery()
{
uint32_t num_busoff_runs = 100000;
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
AP_HAL::CANFrame bo_frame;
bo_frame.id = (10 | AP_HAL::CANFrame::FlagEFF);
memset(bo_frame.data, 0xA, sizeof(bo_frame.data));
bo_frame.dlc =8;//AP_HAL::CANFrame::MaxDataLen;
bool bus_off_detected = false;
// Bus Fault can be introduced by shorting CANH and CANL
gcs().send_text(MAV_SEVERITY_ERROR, "Introduce Bus Off Fault on the bus.");
while (num_busoff_runs--) {
if (bus_off_detected) {
break;
}
//Spam the bus with same frame
_can_ifaces[0]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
_can_ifaces[1]->receive(bo_frame, timestamp, flags);
_can_ifaces[1]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
_can_ifaces[0]->receive(bo_frame, timestamp, flags);
bus_off_detected = _can_ifaces[0]->is_busoff() || _can_ifaces[1]->is_busoff();
hal.scheduler->delay_microseconds(50);
}
if (!bus_off_detected) {
gcs().send_text(MAV_SEVERITY_ERROR, "BusOff not detected on the bus");
return false;
}
gcs().send_text(MAV_SEVERITY_ERROR, "BusOff detected remove Fault.");
hal.scheduler->delay(4000);
gcs().send_text(MAV_SEVERITY_ERROR, "Running Loopback test.");
//Send Dummy Frames to clear the error
while (!write_frame(0, bo_frame,100)) {}
bo_frame.id -= 1;
while (!write_frame(1, bo_frame,100)) {}
//Clear the CAN bus Rx Buffer
hal.scheduler->delay(1000);
_can_ifaces[0]->clear_rx();
_can_ifaces[1]->clear_rx();
return test_loopback(_loop_rate);
}
/*****************************************
* UAVCAN DNA Test *
* ***************************************/
bool CANTester::test_uavcan_dna()
{
uavcan::CanIfaceMgr _uavcan_iface_mgr {};
if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
return false;
}
auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (!node) {
return false;
}
node->setName("org.ardupilot.dnatest");
uavcan::protocol::HardwareVersion hw_version;
const uint8_t uid_buf_len = hw_version.unique_id.capacity();
uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
unique_id[uid_len - 1] -= 5;
uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
}
node->setHardwareVersion(hw_version); // Copying the value to the node's internals
/*
* Starting the node normally, in passive mode (i.e. without node ID assigned).
*/
const int node_start_res = node->start();
if (node_start_res < 0) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
delete node;
return false;
}
/*
* Initializing the dynamic node ID allocation client.
* By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
* this can be overriden through the third argument to the start() method.
*/
auto *client = new uavcan::DynamicNodeIDClient(*node);
if (!client) {
delete node;
return false;
}
int expected_node_id = 100;
int client_start_res = client->start(node->getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE
expected_node_id);
if (client_start_res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
}
/*
* Waiting for the client to obtain for us a node ID.
* This may take a few seconds.
*/
gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
uint32_t num_runs = 100;
while (!client->isAllocationComplete() && num_runs--) {
const int res = node->spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter
if (res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
}
}
gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
int(client->getAllocatedNodeID().get()),
int(client->getAllocatorNodeID().get()));
if (client->getAllocatedNodeID().get() != expected_node_id) {
gcs().send_text(MAV_SEVERITY_ALERT, "Unexpected Node Id, expected %d", expected_node_id);
delete client;
delete node;
return false;
}
delete client;
delete node;
return true;
}
/*****************************************
* KDE CAN Test *
*****************************************/
bool CANTester::test_kdecan()
{
AP_CANTester_KDECAN* kdecan_test = new AP_CANTester_KDECAN;
if (kdecan_test == nullptr) {
gcs().send_text(MAV_SEVERITY_ERROR, "Failed to allocate KDECAN Tester");
return false;
}
kdecan_test->init(_can_ifaces[0]);
while (true) {
kdecan_test->loop();
static uint32_t last_print_ms;
static uint32_t last_enumsend_ms;
static uint8_t enum_count = 0;
uint32_t now = AP_HAL::millis();
if (now - last_print_ms >= 1000) {
last_print_ms = now;
kdecan_test->print_stats();
}
if (!_kdecan_enumeration) {
enum_count = 0;
}
if (now - last_enumsend_ms >= 2000 && enum_count < 4 && _kdecan_enumeration) {
last_enumsend_ms = now;
if (kdecan_test->send_enumeration(enum_count)) {
enum_count++;
}
}
}
return true;
}
bool CANTester::run_kdecan_enumeration(bool start_stop)
{
_kdecan_enumeration = start_stop;
return true;
}
/***********************************************
* UAVCAN ESC *
* *********************************************/
#define NUM_ESCS 4
static uavcan::Publisher<uavcan::equipment::esc::Status>* esc_status_publisher;
static uavcan::Subscriber<uavcan::equipment::esc::RawCommand> *esc_command_listener;
static uint16_t uavcan_esc_command_rate = 0;
void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg);
void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg)
{
static uint16_t num_received = 0;
static uint32_t last_millis;
if (num_received == 0) {
last_millis = AP_HAL::millis();
}
num_received++;
// update rate every 50 packets
if (num_received == 50) {
uavcan_esc_command_rate = 50000/(AP_HAL::millis() - last_millis);
num_received = 0;
}
}
bool CANTester::test_uavcan_esc(bool enable_canfd)
{
bool ret = true;
uavcan::CanIfaceMgr _uavcan_iface_mgr {};
if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
return false;
}
uavcan::Node<0> *node = nullptr;
{
node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (node == nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node");
ret = false;
goto exit;
} else {
node->setName("org.ardupilot.esctest");
}
}
{
uavcan::protocol::HardwareVersion hw_version;
const uint8_t uid_buf_len = hw_version.unique_id.capacity();
uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
// Generate random uid
unique_id[uid_len - 1] += 5;
uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
}
node->setHardwareVersion(hw_version); // Copying the value to the node's internals
}
/*
* Starting the node normally, in passive mode (i.e. without node ID assigned).
*/
{
const int node_start_res = node->start();
if (node_start_res < 0) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
ret = false;
goto exit;
}
}
{
/*
* Initializing the dynamic node ID allocation client.
* By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
* this can be overriden through the third argument to the start() method.
*/
uavcan::DynamicNodeIDClient client(*node);
int client_start_res = client.start(node->getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE
uavcan::NodeID(0));
if (client_start_res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
ret = false;
goto exit;
}
/*
* Waiting for the client to obtain for us a node ID.
* This may take a few seconds.
*/
gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
while (!client.isAllocationComplete()) {
const int res = node->spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter
if (res < 0) {
gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
}
}
gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
int(client.getAllocatedNodeID().get()),
int(client.getAllocatorNodeID().get()));
if (client.getAllocatedNodeID().get() == 255) {
gcs().send_text(MAV_SEVERITY_ALERT, "Node Allocation Failed");
ret = false;
goto exit;
}
esc_command_listener = new uavcan::Subscriber<uavcan::equipment::esc::RawCommand>(*node);
if (esc_command_listener) {
esc_command_listener->start(handle_raw_command);
} else {
ret = false;
goto exit;
}
esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node);
if (esc_status_publisher == nullptr) {
ret = false;
goto exit;
}
esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
if (enable_canfd) {
node->enableCanFd();
} else {
node->disableCanFd();
}
node->setNodeID(client.getAllocatedNodeID());
node->setModeOperational();
}
// Allocations done lets begin
if (ret) {
while (true) {
node->spin(uavcan::MonotonicDuration::fromMSec(1000));
gcs().send_text(MAV_SEVERITY_ALERT, "UC ESC Command Rate: %d", uavcan_esc_command_rate);
uavcan_esc_command_rate = 0;
// send fake ESC stats as well
for (uint8_t i = 0; i < NUM_ESCS; i++) {
uavcan::equipment::esc::Status status_msg;
status_msg.esc_index = i;
status_msg.error_count = 0;
status_msg.voltage = 30 + 2*((float)get_random16()/INT16_MAX);
status_msg.current = 10 + 10*((float)get_random16()/INT16_MAX);
status_msg.temperature = C_TO_KELVIN(124 + i);
status_msg.rpm = 1200 + 300*((float)get_random16()/INT16_MAX);
status_msg.power_rating_pct = 70 + 20*((float)get_random16()/INT16_MAX);
esc_status_publisher->broadcast(status_msg);
}
}
}
exit:
// Clean up!
delete node;
if (esc_command_listener != nullptr) {
delete esc_command_listener;
esc_command_listener = nullptr;
}
if (esc_status_publisher != nullptr) {
delete esc_status_publisher;
esc_status_publisher = nullptr;
}
return ret;
}
CANTester *CANTester::get_cantester(uint8_t driver_index)
{
if (driver_index >= AP::can().get_num_drivers() ||
AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_CANTester) {
return nullptr;
}
return static_cast<CANTester*>(AP::can().get_driver(driver_index));
}
#endif //#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS
Reference in New Issue View Git Blame Copy Permalink