mirror of https://github.com/rusefi/bldc.git
367 lines
12 KiB
C
367 lines
12 KiB
C
/*
|
|
Copyright 2019 Benjamin Vedder benjamin@vedder.se
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
The VESC firmware 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.
|
|
|
|
The VESC firmware 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/>.
|
|
*/
|
|
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
#include "canard_driver.h"
|
|
#include "canard.h"
|
|
#include "uavcan/equipment/esc/Status.h"
|
|
#include "uavcan/equipment/esc/RawCommand.h"
|
|
#include "uavcan/equipment/esc/RPMCommand.h"
|
|
|
|
#include "conf_general.h"
|
|
#include "app.h"
|
|
#include "comm_can.h"
|
|
#include "commands.h"
|
|
#include "mc_interface.h"
|
|
#include "hw.h"
|
|
#include "timeout.h"
|
|
#include "terminal.h"
|
|
|
|
// Constants
|
|
#define APP_NODE_NAME "org.vesc." HW_NAME
|
|
|
|
#define UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE ((3015 + 7) / 8)
|
|
#define UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE 0xee468a8121c46a9e
|
|
#define UAVCAN_GET_NODE_INFO_DATA_TYPE_ID 1
|
|
|
|
#define UAVCAN_NODE_STATUS_MESSAGE_SIZE 7
|
|
#define UAVCAN_NODE_STATUS_DATA_TYPE_ID 341
|
|
#define UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE 0x0f0868d0c1a7c6f1
|
|
|
|
#define UAVCAN_NODE_HEALTH_OK 0
|
|
#define UAVCAN_NODE_HEALTH_WARNING 1
|
|
#define UAVCAN_NODE_HEALTH_ERROR 2
|
|
#define UAVCAN_NODE_HEALTH_CRITICAL 3
|
|
|
|
#define UAVCAN_NODE_MODE_OPERATIONAL 0
|
|
#define UAVCAN_NODE_MODE_INITIALIZATION 1
|
|
|
|
#define UNIQUE_ID_LENGTH_BYTES 16
|
|
|
|
#define STATUS_MSGS_TO_STORE 10
|
|
|
|
// Private datatypes
|
|
typedef struct {
|
|
int id;
|
|
systime_t rx_time;
|
|
uavcan_equipment_esc_Status msg;
|
|
} status_msg_wrapper_t;
|
|
|
|
// Private variables
|
|
static CanardInstance canard;
|
|
static uint8_t canard_memory_pool[1024];
|
|
static uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
|
|
static uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL;
|
|
static int debug_level;
|
|
static status_msg_wrapper_t stat_msgs[STATUS_MSGS_TO_STORE];
|
|
|
|
// Threads
|
|
static THD_WORKING_AREA(canard_thread_wa, 2048);
|
|
static THD_FUNCTION(canard_thread, arg);
|
|
|
|
// Private functions
|
|
static void sendEscStatus(void);
|
|
static void readUniqueID(uint8_t* out_uid);
|
|
static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]);
|
|
static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);
|
|
static bool shouldAcceptTransfer(const CanardInstance* ins,
|
|
uint64_t* out_data_type_signature,
|
|
uint16_t data_type_id,
|
|
CanardTransferType transfer_type,
|
|
uint8_t source_node_id);
|
|
static void terminal_debug_on(int argc, const char **argv);
|
|
|
|
void canard_driver_init(void) {
|
|
debug_level = 0;
|
|
|
|
for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) {
|
|
stat_msgs[i].id = -1;
|
|
}
|
|
|
|
chThdCreateStatic(canard_thread_wa, sizeof(canard_thread_wa), NORMALPRIO, canard_thread, NULL);
|
|
|
|
terminal_register_command_callback(
|
|
"uavcan_debug",
|
|
"Enable UAVCAN debug prints (0 = off)",
|
|
"[level]",
|
|
terminal_debug_on);
|
|
}
|
|
|
|
static void sendEscStatus(void) {
|
|
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];
|
|
uavcan_equipment_esc_Status status;
|
|
status.current = mc_interface_get_tot_current();
|
|
status.error_count = mc_interface_get_fault();
|
|
status.esc_index = app_get_configuration()->uavcan_esc_index;
|
|
status.power_rating_pct = (fabsf(mc_interface_get_tot_current()) /
|
|
mc_interface_get_configuration()->l_current_max *
|
|
mc_interface_get_configuration()->l_current_max_scale) * 100.0;
|
|
status.rpm = mc_interface_get_rpm();
|
|
status.temperature = mc_interface_temp_fet_filtered() + 273.15;
|
|
status.voltage = GET_INPUT_VOLTAGE();
|
|
|
|
uavcan_equipment_esc_Status_encode(&status, buffer);
|
|
|
|
static uint8_t transfer_id;
|
|
|
|
canardBroadcast(&canard,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_ID,
|
|
&transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
|
buffer,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE);
|
|
}
|
|
|
|
static void readUniqueID(uint8_t* out_uid) {
|
|
for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++) {
|
|
out_uid[i] = i;
|
|
}
|
|
}
|
|
|
|
static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]) {
|
|
memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE);
|
|
const uint32_t uptime_sec = ST2S(chVTGetSystemTimeX());
|
|
canardEncodeScalar(buffer, 0, 32, &uptime_sec);
|
|
canardEncodeScalar(buffer, 32, 2, &node_health);
|
|
canardEncodeScalar(buffer, 34, 3, &node_mode);
|
|
}
|
|
|
|
/**
|
|
* This callback is invoked by the library when a new message or request or response is received.
|
|
*/
|
|
static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
if (debug_level > 0) {
|
|
commands_printf("UAVCAN transfer RX: NODE: %d Type: %d ID: %d",
|
|
transfer->source_node_id, transfer->transfer_type, transfer->data_type_id);
|
|
}
|
|
|
|
if ((transfer->transfer_type == CanardTransferTypeRequest) &&
|
|
(transfer->data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) {
|
|
|
|
uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
|
|
memset(buffer, 0, sizeof(buffer));
|
|
|
|
// NodeStatus
|
|
makeNodeStatusMessage(buffer);
|
|
|
|
// SoftwareVersion
|
|
buffer[7] = FW_VERSION_MAJOR;
|
|
buffer[8] = FW_VERSION_MINOR;
|
|
buffer[9] = 1; // Optional field flags, VCS commit is set
|
|
uint32_t u32 = 0;// GIT_HASH;
|
|
canardEncodeScalar(buffer, 80, 32, &u32);
|
|
// Image CRC skipped
|
|
|
|
// HardwareVersion
|
|
// Major skipped
|
|
// Minor skipped
|
|
readUniqueID(&buffer[24]);
|
|
// Certificate of authenticity skipped
|
|
|
|
// Name
|
|
const size_t name_len = strlen(APP_NODE_NAME);
|
|
memcpy(&buffer[41], APP_NODE_NAME, name_len);
|
|
|
|
const size_t total_size = 41 + name_len;
|
|
|
|
/*
|
|
* Transmitting; in this case we don't have to release the payload because it's empty anyway.
|
|
*/
|
|
canardRequestOrRespond(ins,
|
|
transfer->source_node_id,
|
|
UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
|
|
UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
|
|
&transfer->transfer_id,
|
|
transfer->priority,
|
|
CanardResponse,
|
|
&buffer[0],
|
|
(uint16_t)total_size);
|
|
} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
|
|
(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) {
|
|
uavcan_equipment_esc_RawCommand cmd;
|
|
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
|
|
memset(buffer, 0, sizeof(buffer));
|
|
uint8_t *tmp = buffer;
|
|
|
|
if (uavcan_equipment_esc_RawCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) {
|
|
if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) {
|
|
mc_interface_set_duty(((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0);
|
|
timeout_reset();
|
|
}
|
|
}
|
|
} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
|
|
(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) {
|
|
uavcan_equipment_esc_RPMCommand cmd;
|
|
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE];
|
|
memset(buffer, 0, sizeof(buffer));
|
|
uint8_t *tmp = buffer;
|
|
|
|
if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) {
|
|
if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) {
|
|
mc_interface_set_pid_speed(cmd.rpm.data[app_get_configuration()->uavcan_esc_index]);
|
|
timeout_reset();
|
|
}
|
|
}
|
|
} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
|
|
(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) {
|
|
uavcan_equipment_esc_Status msg;
|
|
if (uavcan_equipment_esc_Status_decode_internal(transfer, transfer->payload_len, &msg, 0, 0, true) >= 0) {
|
|
for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) {
|
|
status_msg_wrapper_t *msgw = &stat_msgs[i];
|
|
if (msgw->id == -1 || msgw->id == transfer->source_node_id) {
|
|
msgw->id = transfer->source_node_id;
|
|
msgw->rx_time = chVTGetSystemTimeX();
|
|
msgw->msg = msg;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received
|
|
* by the local node.
|
|
* If the callback returns true, the library will receive the transfer.
|
|
* If the callback returns false, the library will ignore the transfer.
|
|
* All transfers that are addressed to other nodes are always ignored.
|
|
*/
|
|
static bool shouldAcceptTransfer(const CanardInstance* ins,
|
|
uint64_t* out_data_type_signature,
|
|
uint16_t data_type_id,
|
|
CanardTransferType transfer_type,
|
|
uint8_t source_node_id) {
|
|
(void)ins;
|
|
(void)source_node_id;
|
|
|
|
if (debug_level > 0) {
|
|
commands_printf("UAVCAN shouldAccept: NODE: %d Type: %d ID: %d",
|
|
source_node_id, transfer_type, data_type_id);
|
|
}
|
|
|
|
if ((transfer_type == CanardTransferTypeRequest) && (data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) {
|
|
*out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE;
|
|
return true;
|
|
}
|
|
|
|
if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) {
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
|
|
return true;
|
|
}
|
|
|
|
if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) {
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE;
|
|
return true;
|
|
}
|
|
|
|
if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) {
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static void terminal_debug_on(int argc, const char **argv) {
|
|
if (argc == 2) {
|
|
int level = -1;
|
|
sscanf(argv[1], "%d", &level);
|
|
|
|
if (level >= 0) {
|
|
debug_level = level;
|
|
commands_printf("UAVCAN debug level is now %d", debug_level);
|
|
} else {
|
|
commands_printf("Invalid argument(s).\n");
|
|
}
|
|
} else {
|
|
commands_printf("This command requires one argument.\n");
|
|
}
|
|
}
|
|
|
|
static THD_FUNCTION(canard_thread, arg) {
|
|
(void)arg;
|
|
chRegSetThreadName("UAVCAN");
|
|
|
|
canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool), onTransferReceived, shouldAcceptTransfer, NULL);
|
|
|
|
systime_t last_status_time = 0;
|
|
systime_t last_esc_status_time = 0;
|
|
|
|
for (;;) {
|
|
const app_configuration *conf = app_get_configuration();
|
|
|
|
if (!conf->uavcan_enable) {
|
|
chThdSleepMilliseconds(100);
|
|
continue;
|
|
}
|
|
|
|
canardSetLocalNodeID(&canard, conf->controller_id);
|
|
|
|
CANRxFrame *rxmsg;
|
|
while ((rxmsg = comm_can_get_rx_frame()) != 0) {
|
|
CanardCANFrame rx_frame;
|
|
|
|
if (rxmsg->IDE == CAN_IDE_EXT) {
|
|
rx_frame.id = rxmsg->EID | CANARD_CAN_FRAME_EFF;
|
|
} else {
|
|
rx_frame.id = rxmsg->SID;
|
|
}
|
|
|
|
rx_frame.data_len = rxmsg->DLC;
|
|
memcpy(rx_frame.data, rxmsg->data8, rxmsg->DLC);
|
|
|
|
canardHandleRxFrame(&canard, &rx_frame, ST2US(chVTGetSystemTimeX()));
|
|
}
|
|
|
|
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
|
|
comm_can_transmit_eid(txf->id, txf->data, txf->data_len);
|
|
canardPopTxQueue(&canard);
|
|
}
|
|
|
|
if (ST2MS(chVTTimeElapsedSinceX(last_status_time)) >= 1000) {
|
|
last_status_time = chVTGetSystemTimeX();
|
|
canardCleanupStaleTransfers(&canard, ST2US(chVTGetSystemTimeX()));
|
|
|
|
uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE];
|
|
makeNodeStatusMessage(buffer);
|
|
|
|
static uint8_t transfer_id;
|
|
canardBroadcast(&canard,
|
|
UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE,
|
|
UAVCAN_NODE_STATUS_DATA_TYPE_ID,
|
|
&transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
|
buffer,
|
|
UAVCAN_NODE_STATUS_MESSAGE_SIZE);
|
|
}
|
|
|
|
if (ST2MS(chVTTimeElapsedSinceX(last_esc_status_time)) >= 1000 / conf->send_can_status_rate_hz &&
|
|
conf->send_can_status != CAN_STATUS_DISABLED) {
|
|
last_esc_status_time = chVTGetSystemTimeX();
|
|
sendEscStatus();
|
|
}
|
|
|
|
chThdSleepMilliseconds(1);
|
|
}
|
|
}
|