mirror of https://github.com/rusefi/bldc.git
1245 lines
40 KiB
C
Executable File
1245 lines
40 KiB
C
Executable File
/*
|
|
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 "uavcan/protocol/param/GetSet.h"
|
|
#include "uavcan/protocol/GetNodeInfo.h"
|
|
#include "uavcan/protocol/RestartNode.h"
|
|
#include <uavcan/protocol/file/BeginFirmwareUpdate.h>
|
|
#include <uavcan/protocol/file/Read.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"
|
|
#include "mempools.h"
|
|
#include "flash_helper.h"
|
|
#include "crc.h"
|
|
#include "nrf_driver.h"
|
|
#include "buffer.h"
|
|
#include "utils.h"
|
|
|
|
// Constants
|
|
#define CAN_APP_NODE_NAME "org.vesc." HW_NAME
|
|
#define UNIQUE_ID_LENGTH_BYTES 12
|
|
#define STATUS_MSGS_TO_STORE 10
|
|
#define AP_MAX_NAME_SIZE 16
|
|
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
|
|
|
|
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH 16
|
|
|
|
#define CURRENT_CALC_FREQ_HZ 10
|
|
#define PARAM_REFRESH_RATE_HZ 10
|
|
#define ESC_STATUS_TIMEOUT 500 //ms?
|
|
#define RESERVED_FLASH_SPACE_SIZE 393216
|
|
|
|
/*
|
|
* Node status variables
|
|
*/
|
|
static uavcan_protocol_NodeStatus node_status;
|
|
|
|
// Private datatypes
|
|
typedef struct {
|
|
int id;
|
|
systime_t rx_time;
|
|
bool timeout_flag;
|
|
uavcan_equipment_esc_Status msg;
|
|
} status_msg_wrapper_t;
|
|
|
|
// Private variables
|
|
static CanardInstance canard_ins;
|
|
static uint8_t canard_memory_pool[1024];
|
|
#ifdef HW_CAN2_DEV
|
|
static CanardInstance canard_ins_if2;
|
|
static uint8_t canard_memory_pool_if2[1024];
|
|
#endif
|
|
static uint8_t msg_buffer[512];
|
|
static uint8_t node_health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
|
|
static uint8_t node_mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
|
|
static int debug_level;
|
|
static status_msg_wrapper_t stat_msgs[STATUS_MSGS_TO_STORE];
|
|
static uint8_t my_node_id = 0;
|
|
static bool refresh_parameters_enabled = true;
|
|
|
|
// Threads
|
|
static THD_WORKING_AREA(canard_thread_wa, 1024);
|
|
static THD_FUNCTION(canard_thread, arg);
|
|
|
|
// Private functions
|
|
static void calculateTotalCurrent(void);
|
|
static void sendEscStatus(CanardInstance *ins);
|
|
static void readUniqueID(uint8_t* out_uid);
|
|
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);
|
|
|
|
/*
|
|
* Firmware Update Stuff
|
|
*/
|
|
static struct {
|
|
uint64_t ofs;
|
|
uint32_t last_ms;
|
|
uint8_t node_id;
|
|
uint8_t transfer_id;
|
|
uint8_t path[UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1];
|
|
uint8_t sector;
|
|
uint32_t sector_ofs;
|
|
CanardInstance *ins;
|
|
} fw_update;
|
|
|
|
systime_t last_read_file_req = 0;
|
|
systime_t jump_delay_start = 0;
|
|
bool jump_to_bootloader = false;
|
|
|
|
#define FLASH_SECTORS 12
|
|
#define BOOTLOADER_BASE 11
|
|
#define APP_BASE 0
|
|
#define APP_SECTORS 7
|
|
#define NEW_APP_BASE 8
|
|
#define NEW_APP_SECTORS 3
|
|
#define NEW_APP_MAX_SIZE (3 * (1 << 17))
|
|
|
|
/*
|
|
* Base address of the Flash sectors
|
|
* May be able to remove these and only use the address for NEW_APP_BASE and APP_BASE instead
|
|
*/
|
|
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) // Base @ of Sector 0, 16 Kbytes
|
|
#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000) // Base @ of Sector 1, 16 Kbytes
|
|
#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000) // Base @ of Sector 2, 16 Kbytes
|
|
#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000) // Base @ of Sector 3, 16 Kbytes
|
|
#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000) // Base @ of Sector 4, 64 Kbytes
|
|
#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000) // Base @ of Sector 5, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000) // Base @ of Sector 6, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000) // Base @ of Sector 7, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_8 ((uint32_t)0x08080000) // Base @ of Sector 8, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_9 ((uint32_t)0x080A0000) // Base @ of Sector 9, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_10 ((uint32_t)0x080C0000) // Base @ of Sector 10, 128 Kbytes
|
|
#define ADDR_FLASH_SECTOR_11 ((uint32_t)0x080E0000) // Base @ of Sector 11, 128 Kbytes
|
|
|
|
static const uint32_t flash_addr[FLASH_SECTORS] = {
|
|
ADDR_FLASH_SECTOR_0,
|
|
ADDR_FLASH_SECTOR_1,
|
|
ADDR_FLASH_SECTOR_2,
|
|
ADDR_FLASH_SECTOR_3,
|
|
ADDR_FLASH_SECTOR_4,
|
|
ADDR_FLASH_SECTOR_5,
|
|
ADDR_FLASH_SECTOR_6,
|
|
ADDR_FLASH_SECTOR_7,
|
|
ADDR_FLASH_SECTOR_8,
|
|
ADDR_FLASH_SECTOR_9,
|
|
ADDR_FLASH_SECTOR_10,
|
|
ADDR_FLASH_SECTOR_11
|
|
};
|
|
|
|
/*
|
|
* Parameter types and enums
|
|
*/
|
|
typedef struct
|
|
{
|
|
char* name;
|
|
uint8_t type;
|
|
int64_t val;
|
|
int64_t min;
|
|
int64_t max;
|
|
int64_t defval;
|
|
} param_t;
|
|
|
|
enum ap_var_type {
|
|
AP_PARAM_NONE = 0,
|
|
AP_PARAM_INT8,
|
|
AP_PARAM_INT16,
|
|
AP_PARAM_INT32,
|
|
AP_PARAM_FLOAT,
|
|
AP_PARAM_VECTOR3F,
|
|
AP_PARAM_GROUP
|
|
};
|
|
|
|
/*
|
|
* Private Parameter function declarations
|
|
*/
|
|
static void updateParamByName(uint8_t * name, float value);
|
|
static void refresh_parameters(void);
|
|
static param_t* getParamByIndex(uint16_t index);
|
|
static param_t* getParamByName(char * name);
|
|
static void write_app_config(void);
|
|
|
|
/*
|
|
* Local parameter table
|
|
* This table contains the parameters we want to be able to change via UAVCAN. It is a copy of the
|
|
* appconf parameters with more information, the format of each parameter is as follows:
|
|
* { parameter name, parameter type, current value, min value, max value, default value}
|
|
*/
|
|
static param_t parameters[] =
|
|
{
|
|
{"can_baud_rate", AP_PARAM_INT8, 0, 0, 8, CAN_BAUD_500K},
|
|
{"can_status_rate", AP_PARAM_INT32, 0, 0, 1000, 50},
|
|
{"can_esc_index", AP_PARAM_INT16, 0, 0, 255, 0},
|
|
{"controller_id", AP_PARAM_INT16, 0, 0, 253, 0}
|
|
};
|
|
|
|
/*
|
|
* This function updates the local parameter value. It is called after reading the current appconf
|
|
* data to update the local copy of the parameter.
|
|
*/
|
|
static void updateParamByName(uint8_t * name, float value)
|
|
{
|
|
param_t* p = NULL;
|
|
p = getParamByName((char *)name);
|
|
if (p != NULL) {
|
|
if (p->val != value) {
|
|
if (debug_level > 0) {
|
|
commands_printf("%s, %s p->val %0.02f, value %0.02f", p->name, name, (double)p->val, (double)value);
|
|
}
|
|
p->val = value;
|
|
}
|
|
} else {
|
|
if (debug_level > 0) {
|
|
commands_printf("UAVCAN updateParamByName(): Parameter name not found");
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* This function updates the app config from the local app config. It is used to
|
|
* write new parameters after a set parameter request.
|
|
*/
|
|
static void write_app_config(void) {
|
|
app_configuration *appconf = mempools_alloc_appconf();
|
|
*appconf = *app_get_configuration();
|
|
|
|
appconf->can_baud_rate = (uint8_t)getParamByName("can_baud_rate")->val;
|
|
appconf->can_status_rate_1 = (uint32_t)getParamByName("can_status_rate")->val;
|
|
appconf->uavcan_esc_index = (uint16_t)getParamByName("can_esc_index")->val;
|
|
appconf->controller_id = (uint16_t)getParamByName("controller_id")->val;
|
|
|
|
conf_general_store_app_configuration(appconf);
|
|
app_set_configuration(appconf);
|
|
|
|
mempools_free_appconf(appconf);
|
|
|
|
refresh_parameters_enabled = true;
|
|
}
|
|
|
|
/*
|
|
* This function updates the local copy of the parameters from the appconf data
|
|
* This is called periodically so that the paremters are kept in sync. Care has
|
|
* to be taken when a set parameter request is recieved so that the parameter is
|
|
* not overwritten before it is actually writen to the emulated EEPROM.
|
|
*/
|
|
static void refresh_parameters(void){
|
|
const app_configuration *appconf = app_get_configuration();
|
|
|
|
updateParamByName((uint8_t *)"can_baud_rate", appconf->can_baud_rate );
|
|
updateParamByName((uint8_t *)"can_status_rate", appconf->can_status_rate_1 );
|
|
updateParamByName((uint8_t *)"can_esc_index", appconf->uavcan_esc_index );
|
|
updateParamByName((uint8_t *)"controller_id", appconf->controller_id );
|
|
}
|
|
|
|
/*
|
|
* Get parameter by index
|
|
* Retrieves the parameter with the given index if found
|
|
* if not found it returns null
|
|
*/
|
|
static param_t* getParamByIndex(uint16_t index)
|
|
{
|
|
if (index >= (sizeof(parameters) / sizeof(parameters[0])))
|
|
{
|
|
if (debug_level == 2) {
|
|
commands_printf("Index is out of range");
|
|
}
|
|
return NULL;
|
|
}
|
|
if (debug_level == 2) {
|
|
commands_printf("Size of parameter array is: %d", (sizeof(parameters) / sizeof(parameters[0])));
|
|
}
|
|
return ¶meters[index];
|
|
}
|
|
|
|
/*
|
|
* Get parameter by name
|
|
* Searches the parameter table for the given name and returns the parameter information
|
|
* if no parameter is found it returns null.
|
|
*/
|
|
static param_t* getParamByName(char * name)
|
|
{
|
|
for (uint16_t i = 0; i < (sizeof(parameters) / sizeof(parameters[0])); i++)
|
|
{
|
|
if (debug_level == 2) {
|
|
commands_printf("name: %s paramname: %s", name, parameters[i].name);
|
|
}
|
|
|
|
if (strcmp(name, parameters[i].name) == 0)
|
|
{
|
|
if (debug_level == 2) {
|
|
commands_printf("found match!");
|
|
}
|
|
return ¶meters[i];
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/*
|
|
* UAVCAN Driver Init
|
|
*/
|
|
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 1: errors 2: param getset 3: current calc 4: comms stuff)",
|
|
"[level]",
|
|
terminal_debug_on);
|
|
}
|
|
|
|
/*
|
|
* Calculate the total system current consumption based on the reported consumption by other
|
|
* ESCs transmitting data on the bus.
|
|
*/
|
|
static void calculateTotalCurrent(void) {
|
|
// Calculate total current being consumed by the ESCs on the system
|
|
float totalCurrent = mc_interface_get_tot_current();
|
|
float avgVoltage = mc_interface_get_input_voltage_filtered();
|
|
float totalSysCurrent = 0;
|
|
|
|
uint8_t escTotal = 1;
|
|
|
|
for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) {
|
|
status_msg_wrapper_t *msgw = &stat_msgs[i];
|
|
if (msgw->id != -1) {
|
|
systime_t elapsedTime = chVTGetSystemTimeX() - msgw->rx_time;
|
|
if(ST2MS(elapsedTime) > ESC_STATUS_TIMEOUT) {
|
|
if (debug_level > 0) {
|
|
commands_printf("ESC timeout for NodeID: %d",msgw->id);
|
|
}
|
|
msgw->id = -1;
|
|
} else {
|
|
totalCurrent += msgw->msg.current;
|
|
totalSysCurrent += msgw->msg.current;
|
|
escTotal++;
|
|
avgVoltage += msgw->msg.voltage;
|
|
}
|
|
}
|
|
}
|
|
|
|
avgVoltage = avgVoltage / escTotal;
|
|
if(debug_level == 3) {
|
|
commands_printf("Total number of nodes: %d", escTotal);
|
|
commands_printf("Total Current: %0.02f", (double)totalCurrent);
|
|
commands_printf("Total Sys Curr: %0.02f", (double)totalSysCurrent);
|
|
commands_printf("Average Voltage: %0.02f", (double)avgVoltage);
|
|
}
|
|
|
|
// ToDo: Add a way to limit the ESC current based on the total system current consumed
|
|
}
|
|
|
|
/*
|
|
* Send Node Status Message
|
|
*/
|
|
static void sendNodeStatus(CanardInstance *ins) {
|
|
node_mode = fw_update.node_id?UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
|
|
|
|
uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
|
|
|
|
node_status.health = node_health;
|
|
node_status.mode = node_mode;
|
|
node_status.uptime_sec = (uint32_t)ST2S(chVTGetSystemTimeX());
|
|
// status.sub_mode =
|
|
// status.vendor_specific_status_code is filled in the firmware update loop
|
|
uavcan_protocol_NodeStatus_encode(&node_status, buffer);
|
|
static uint8_t transfer_id;
|
|
canardBroadcast(ins,
|
|
UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
|
|
UAVCAN_PROTOCOL_NODESTATUS_ID,
|
|
&transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
|
buffer,
|
|
UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE);
|
|
}
|
|
|
|
/*
|
|
* Send ESC Status Message
|
|
*/
|
|
static void sendEscStatus(CanardInstance *ins) {
|
|
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 = fmaxf(mc_interface_temp_fet_filtered(), mc_interface_temp_motor_filtered()) + 273.15;
|
|
status.voltage = mc_interface_get_input_voltage_filtered();
|
|
|
|
uavcan_equipment_esc_Status_encode(&status, msg_buffer);
|
|
|
|
static uint8_t transfer_id;
|
|
|
|
if (debug_level > 11) {
|
|
commands_printf("UAVCAN sendESCStatus");
|
|
}
|
|
|
|
canardBroadcast(ins,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_ID,
|
|
&transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
|
msg_buffer,
|
|
UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE);
|
|
}
|
|
|
|
/*
|
|
* Reads the STM32 Unique ID
|
|
*/
|
|
static void readUniqueID(uint8_t* out_uid) {
|
|
uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH;
|
|
memset(out_uid, 0, len);
|
|
memcpy(out_uid, (const void *)STM32_UUID_8, UNIQUE_ID_LENGTH_BYTES);
|
|
}
|
|
|
|
/*
|
|
* Handle a GET_NODE_INFO request
|
|
* FW_VERSION is from conf_general.h
|
|
* HW_VERSION is from HW_HEADER if HW_MAJOR/HW_MINOR are defined, else it sends 0
|
|
* This needs a change to all HW_HEADER files if desired.
|
|
* Unique ID is now being read from the STM32 UUID field
|
|
* Node status is syncronize with the data from the node status message.
|
|
*/
|
|
static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
uavcan_protocol_GetNodeInfoResponse pkt;
|
|
|
|
node_status.uptime_sec = ST2S(chVTGetSystemTimeX());
|
|
|
|
pkt.status = node_status;
|
|
pkt.software_version.major = FW_VERSION_MAJOR;
|
|
pkt.software_version.minor = FW_VERSION_MINOR;
|
|
pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC;
|
|
pkt.software_version.vcs_commit = FW_TEST_VERSION_NUMBER;
|
|
uint32_t *crc = (uint32_t *)&pkt.software_version.image_crc;
|
|
crc[0] = 0;
|
|
crc[1] = 0;
|
|
|
|
readUniqueID(pkt.hardware_version.unique_id);
|
|
|
|
// use hw major/minor for APJ_BOARD_ID so we know what fw is
|
|
// compatible with this hardware
|
|
#ifdef HW_MAJOR
|
|
pkt.hardware_version.major = HW_MAJOR;
|
|
pkt.hardware_version.minor = HW_MINOR;
|
|
#else
|
|
pkt.hardware_version.major = 0;
|
|
pkt.hardware_version.minor = 0;
|
|
#endif
|
|
|
|
char name[strlen(CAN_APP_NODE_NAME)+1];
|
|
strcpy(name, CAN_APP_NODE_NAME);
|
|
pkt.name.len = strlen(CAN_APP_NODE_NAME);
|
|
pkt.name.data = (uint8_t *)name;
|
|
|
|
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, msg_buffer);
|
|
|
|
const int16_t resp_res = canardRequestOrRespond(ins,
|
|
transfer->source_node_id,
|
|
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
|
|
UAVCAN_PROTOCOL_GETNODEINFO_ID,
|
|
&transfer->transfer_id,
|
|
transfer->priority,
|
|
CanardResponse,
|
|
&msg_buffer[0],
|
|
total_size);
|
|
if (resp_res <= 0) {
|
|
if (debug_level > 1) {
|
|
commands_printf("Could not respond to GetNodeInfo: %d\n", resp_res);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Handle ESC Raw command
|
|
*/
|
|
static void handle_esc_raw_command(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
(void)ins;
|
|
|
|
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) >= 0) {
|
|
if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) {
|
|
float raw_val = ((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0;
|
|
|
|
volatile const app_configuration *conf = app_get_configuration();
|
|
|
|
switch (conf->uavcan_raw_mode) {
|
|
case UAVCAN_RAW_MODE_CURRENT:
|
|
mc_interface_set_current_rel(raw_val);
|
|
break;
|
|
|
|
case UAVCAN_RAW_MODE_CURRENT_NO_REV_BRAKE:
|
|
if (raw_val >= 0.0) {
|
|
mc_interface_set_current_rel(raw_val);
|
|
} else {
|
|
mc_interface_set_brake_current_rel(-raw_val);
|
|
}
|
|
break;
|
|
|
|
case UAVCAN_RAW_MODE_DUTY:
|
|
mc_interface_set_duty(raw_val);
|
|
break;
|
|
|
|
case UAVCAN_RAW_MODE_RPM:
|
|
mc_interface_set_pid_speed(raw_val * conf->uavcan_raw_rpm_max);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
timeout_reset();
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Handle ESC RPM command
|
|
*/
|
|
static void handle_esc_rpm_command(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
(void)ins;
|
|
|
|
uavcan_equipment_esc_RPMCommand cmd;
|
|
uint8_t *tmp = msg_buffer;
|
|
|
|
if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0) >= 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();
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Handle Equipment ESC Status Request
|
|
* Reads status messages from other ESCs on the system and stores the information in memory
|
|
* this data can be used to respond to total system current consumption.
|
|
*/
|
|
static void handle_esc_status(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
(void)ins;
|
|
|
|
uavcan_equipment_esc_Status msg;
|
|
if (uavcan_equipment_esc_Status_decode_internal(transfer, transfer->payload_len, &msg, 0, 0) >= 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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Handle parameter GetSet request
|
|
* In UAVCAN parameters are requested individually either by index or by name
|
|
* VESC currently does not have a way to write parameters individually or a way to
|
|
* access the parameters by name. So I needed to create a bridge between the parameters
|
|
* and the UAVCAN driver which was not very clean and creates too much overhead if we
|
|
* want to access a large number of parameters. For that reason only a few paremeters
|
|
* were added here to allow changing of the most basic features.
|
|
* For now any tunning that needs to be done could be done using the USB interface
|
|
* parameters can be changed and if it needs to be deployed on the field to other
|
|
* ESCs that are using a UAVCAN interface then a special firmware with the defaults
|
|
* modified can be create and deployed by a UAVCAN firmware update
|
|
*/
|
|
static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer)
|
|
{
|
|
uavcan_protocol_param_GetSetRequest req;
|
|
uint8_t *arraybuf_ptr = msg_buffer;
|
|
if (uavcan_protocol_param_GetSetRequest_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) {
|
|
return;
|
|
}
|
|
|
|
uavcan_protocol_param_GetSetResponse pkt;
|
|
|
|
memset(&pkt, 0, sizeof(uavcan_protocol_param_GetSetResponse));
|
|
|
|
char name[AP_MAX_NAME_SIZE+1] = "";
|
|
|
|
param_t* p = NULL;
|
|
|
|
// Verify the request is valid and try to get the parameter if it is.
|
|
if (req.name.len != 0 && req.name.len > AP_MAX_NAME_SIZE) {
|
|
if (debug_level > 1) {
|
|
commands_printf("UAVCAN param_getset: Parameter Name is too long!");
|
|
}
|
|
p = NULL;
|
|
} else if (req.name.len != 0 && req.name.len <= AP_MAX_NAME_SIZE) {
|
|
strncpy((char *)name, (char *)req.name.data, req.name.len);
|
|
if (debug_level > 1) {
|
|
commands_printf("UAVCAN param_getset param name: %s", name);
|
|
}
|
|
p = getParamByName(name);
|
|
} else {
|
|
if (debug_level > 1) {
|
|
commands_printf("UAVCAN param_getset param index: %d", req.index);
|
|
}
|
|
p = getParamByIndex(req.index);
|
|
}
|
|
|
|
// If a valid parameter was retrived...
|
|
if ((p != NULL) && (debug_level > 1)) {
|
|
uint8_t arrSize = strlen(p->name);
|
|
commands_printf("UAVCAN param_getset got\nname: %s\nsize: %d\ntype: %d\nvalue: %d", (char *)p->name, arrSize, p->type, p->val);
|
|
}
|
|
|
|
// If a valid parameter was retrived and this is a set parameter request...
|
|
if (p != NULL && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) {
|
|
// Set request and valid parameter found
|
|
// Prevent overwrite of local parameter copy before its written to eeprom
|
|
refresh_parameters_enabled = false;
|
|
|
|
switch (req.value.union_tag) {
|
|
case UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY:
|
|
return;
|
|
break;
|
|
|
|
case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE:
|
|
switch(p->type) {
|
|
case AP_PARAM_INT8:
|
|
p->val = (uint8_t)req.value.integer_value;
|
|
break;
|
|
|
|
case AP_PARAM_INT16:
|
|
p->val = (uint16_t)req.value.integer_value;
|
|
break;
|
|
|
|
case AP_PARAM_INT32:
|
|
p->val = (uint32_t)req.value.integer_value;
|
|
break;
|
|
|
|
case AP_PARAM_FLOAT:
|
|
p->val = (float)req.value.integer_value;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
write_app_config();
|
|
}
|
|
|
|
// If a valid parameter was retrived send back the value
|
|
if(p != NULL) {
|
|
switch(p->type) {
|
|
case AP_PARAM_INT8:
|
|
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.value.integer_value = (int8_t)p->val;
|
|
pkt.default_value.integer_value = (int8_t)p->defval;
|
|
pkt.min_value.integer_value = (int8_t)p->min;
|
|
pkt.max_value.integer_value = (int8_t)p->max;
|
|
break;
|
|
|
|
case AP_PARAM_INT16:
|
|
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.value.integer_value = (int16_t)p->val;
|
|
pkt.default_value.integer_value = (int16_t)p->defval;
|
|
pkt.min_value.integer_value = (int16_t)p->min;
|
|
pkt.max_value.integer_value = (int16_t)p->max;
|
|
break;
|
|
|
|
case AP_PARAM_INT32:
|
|
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
|
|
pkt.value.integer_value = (int32_t)p->val;
|
|
pkt.default_value.integer_value = (int32_t)p->defval;
|
|
pkt.min_value.integer_value = (int32_t)p->min;
|
|
pkt.max_value.integer_value = (int32_t)p->max;
|
|
break;
|
|
|
|
case AP_PARAM_FLOAT:
|
|
pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
|
|
pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
|
|
pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
|
|
pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
|
|
pkt.value.real_value = (float)p->val;
|
|
pkt.default_value.real_value = (float)p->defval;
|
|
pkt.min_value.real_value = (float)p->min;
|
|
pkt.max_value.real_value = (float)p->max;
|
|
break;
|
|
}
|
|
|
|
pkt.name.len = strlen((char *)p->name);
|
|
pkt.name.data = (uint8_t *)p->name;
|
|
}
|
|
|
|
if (debug_level == 2) {
|
|
commands_printf("UAVCAN param_getset: Sending %s", pkt.name.data);
|
|
}
|
|
uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, msg_buffer);
|
|
|
|
const int16_t resp_res = canardRequestOrRespond(ins,
|
|
transfer->source_node_id,
|
|
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
|
|
UAVCAN_PROTOCOL_PARAM_GETSET_ID,
|
|
&transfer->transfer_id,
|
|
transfer->priority,
|
|
CanardResponse,
|
|
&msg_buffer[0],
|
|
total_size);
|
|
|
|
if ((resp_res <= 0) && (debug_level > 1)) {
|
|
commands_printf("Could not respond to param_getset_req: %d\n", resp_res);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Create a Watchdog reset in order to restart the node if a restart command is recieved
|
|
*/
|
|
static void handle_restart_node(void) {
|
|
// Lock the system and enter an infinite loop. The watchdog will reboot.
|
|
__disable_irq();
|
|
for(;;){};
|
|
}
|
|
|
|
/*
|
|
* Send a read for a fw update file
|
|
* This request is sent after we recieve a begin firmware udpate request, and then
|
|
* everytime we finish processing one chunk of the firmware file. It uses the
|
|
* fw_update.ofs value to keep track of what chunk of data needs to be requested.
|
|
* We use fw_update.last_ms to give the server enough time to respond to the previous
|
|
* request before we send a new one. The value is cleared after procesing a response
|
|
* so that we dont have to wait 250ms before sending the next request.
|
|
*/
|
|
static void send_fw_read(CanardInstance *ins)
|
|
{
|
|
fw_update.ins = ins;
|
|
|
|
uint32_t now = ST2MS(chVTGetSystemTimeX());
|
|
if (now - fw_update.last_ms < 250) {
|
|
// the server may still be responding
|
|
return;
|
|
}
|
|
fw_update.last_ms = now;
|
|
|
|
canardEncodeScalar(msg_buffer, 0, 40, &fw_update.ofs);
|
|
uint32_t offset = 40;
|
|
uint8_t len = strlen((const char *)fw_update.path);
|
|
for (uint8_t i=0; i<len; i++) {
|
|
canardEncodeScalar(msg_buffer, offset, 8, &fw_update.path[i]);
|
|
offset += 8;
|
|
}
|
|
uint32_t total_size = (offset+7)/8;
|
|
|
|
canardRequestOrRespond(ins,
|
|
fw_update.node_id,
|
|
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
|
|
UAVCAN_PROTOCOL_FILE_READ_ID,
|
|
&fw_update.transfer_id,
|
|
CANARD_TRANSFER_PRIORITY_HIGH,
|
|
CanardRequest,
|
|
&msg_buffer[0],
|
|
total_size);
|
|
}
|
|
|
|
/*
|
|
* Handle response to file read request. This is called when we recieve a response to
|
|
* send_fw_read() above. the packet contains a 16 bit value at the begining called
|
|
* error that needs to be removed before reading the file chunk
|
|
*/
|
|
static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer) {
|
|
(void)ins;
|
|
|
|
if ((transfer->transfer_id+1)%256 != fw_update.transfer_id ||
|
|
transfer->source_node_id != fw_update.node_id) {
|
|
return;
|
|
}
|
|
int16_t error = 0;
|
|
canardDecodeScalar(transfer, 0, 16, true, (void*)&error);
|
|
uint16_t len = transfer->payload_len - 2;
|
|
|
|
uint32_t offset = 16;
|
|
uint32_t buf32[(len+3)/4];
|
|
uint8_t *buf = (uint8_t *)&buf32[0];
|
|
for (uint16_t i=0; i<len; i++) {
|
|
canardDecodeScalar(transfer, offset, 8, false, (void*)&buf[i]);
|
|
offset += 8;
|
|
}
|
|
|
|
if (debug_level == 9) {
|
|
commands_printf("UAVCAN read_response\nlen: %d\noffset: %d",len, fw_update.ofs);
|
|
}
|
|
|
|
if (nrf_driver_ext_nrf_running()) {
|
|
nrf_driver_pause(2000);
|
|
}
|
|
|
|
// Write to flash, skip the first 6 bytes for Size and CRC so need to add 6 always
|
|
uint16_t flash_res = flash_helper_write_new_app_data(fw_update.ofs+6, buf, len);
|
|
fw_update.ofs += len;
|
|
|
|
// TODO: Check result and abort on failure.
|
|
(void)flash_res;
|
|
|
|
// If the packet is incomplete that means that was the last packet (might be an issue if the last packet is exactly full)
|
|
// however Ardupilot seems to be handling this same way, and no issues have been reported so far.
|
|
if (len < UAVCAN_PROTOCOL_FILE_READ_RESPONSE_DATA_MAX_LENGTH) {
|
|
fw_update.node_id = 0;
|
|
const uint32_t app_size = (uint32_t)fw_update.ofs-6;
|
|
uint16_t app_crc = crc16((uint8_t *)ADDR_FLASH_SECTOR_8+6,app_size);
|
|
uint8_t sizecrc[6];
|
|
int32_t ind = 0;
|
|
|
|
uint32_t sizefromflash = 0;
|
|
uint16_t crc_app = 0;
|
|
uint32_t nextData = 0;
|
|
|
|
// This is debug stuff used to valiate the file transfer.
|
|
if (debug_level == 8) {
|
|
commands_printf("UAVCAN read_response transfer finished %d kB", fw_update.ofs / 1024U);
|
|
commands_printf("new app address: 0x%lx", flash_addr[NEW_APP_BASE]);
|
|
|
|
// Print reserved space contents for size and crc
|
|
sizefromflash = buffer_get_uint32((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
crc_app = buffer_get_uint16((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
nextData = buffer_get_uint32((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
commands_printf("orig size from flash: 0x%lx", (long)sizefromflash);
|
|
commands_printf("orig crc from flash: 0x%02hhX", crc_app);
|
|
commands_printf("orig nextData: 0x%08lx", (long)nextData);
|
|
}
|
|
|
|
// Calculate and write size and crc to start of reserved space
|
|
ind = 0;
|
|
buffer_append_uint32(sizecrc, app_size, &ind);
|
|
buffer_append_uint16(sizecrc, app_crc, &ind);
|
|
|
|
flash_res = flash_helper_write_new_app_data(0, sizecrc, sizeof(sizecrc));
|
|
|
|
if (debug_level == 8) {
|
|
// Print data for debuging
|
|
commands_printf("ofs: %ld", (long)fw_update.ofs);
|
|
commands_printf("len: %d", len);
|
|
commands_printf("Size: 0x%lx", (long)app_size);
|
|
commands_printf("crc16: 0x%02hhX", app_crc);
|
|
uint16_t app_crc1 = crc16((uint8_t *)flash_addr[APP_BASE],app_size);
|
|
commands_printf("app crc16: 0x%02hhX", app_crc1);
|
|
|
|
// Print size and crc data read from flash after calculation and write
|
|
ind = 0;
|
|
sizefromflash = buffer_get_uint32((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
crc_app = buffer_get_uint16((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
commands_printf("size from flash: 0x%lx", (long)sizefromflash);
|
|
commands_printf("crc from flash: 0x%02hhX", crc_app);
|
|
nextData = buffer_get_uint32((uint8_t *)flash_addr[NEW_APP_BASE], &ind);
|
|
commands_printf("nextData: 0x%lx", (long)nextData);
|
|
ind = 0;
|
|
uint32_t appstartdata = buffer_get_uint32((uint8_t *)flash_addr[APP_BASE], &ind);
|
|
commands_printf("appStartData: 0x%08lx", appstartdata);
|
|
commands_printf("Jumping to Bootloader in 500ms!");
|
|
jump_delay_start = chVTGetSystemTimeX();
|
|
}
|
|
|
|
// Do not jump directly to the bootloader after finising the transfer in case we need time
|
|
// to allow other things to finish. Currently it only delays if it needs to print the debug
|
|
// data.
|
|
jump_to_bootloader = true;
|
|
}
|
|
|
|
// show offset number we are flashing in kbyte as crude progress indicator
|
|
node_status.vendor_specific_status_code = 1 + (fw_update.ofs / 1024U);
|
|
|
|
// Clear the counter so we dont delay the next request for data unecesarily
|
|
fw_update.last_ms = 0;
|
|
}
|
|
|
|
/**
|
|
* Handle a begin firmware update request.
|
|
* UAVCAN uses the file system requests to pull the firmware file from the host.
|
|
* A begin firmware update call is made to tell the client node to ask the host
|
|
* for the firmware file. From this point on the client basically becomes the host
|
|
* for the file transfer until the file is received by sending requests for the
|
|
* next chunk of data every so often.
|
|
*/
|
|
static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer)
|
|
{
|
|
// manual decoding due to TAO bug in libcanard generated code
|
|
if (transfer->payload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) {
|
|
return;
|
|
}
|
|
|
|
if (fw_update.node_id == 0) {
|
|
uint32_t offset = 0;
|
|
canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id);
|
|
offset += 8;
|
|
for (uint8_t i=0; i<transfer->payload_len-1; i++) {
|
|
canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]);
|
|
offset += 8;
|
|
}
|
|
|
|
fw_update.ofs = 0;
|
|
fw_update.last_ms = 0;
|
|
fw_update.sector = 0;
|
|
fw_update.sector_ofs = 0;
|
|
if (fw_update.node_id == 0) {
|
|
last_read_file_req = chVTGetSystemTimeX();
|
|
fw_update.node_id = transfer->source_node_id;
|
|
}
|
|
}
|
|
|
|
uavcan_protocol_file_BeginFirmwareUpdateResponse reply;
|
|
reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK;
|
|
|
|
uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, msg_buffer);
|
|
canardRequestOrRespond(ins,
|
|
transfer->source_node_id,
|
|
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE,
|
|
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID,
|
|
&transfer->transfer_id,
|
|
transfer->priority,
|
|
CanardResponse,
|
|
&msg_buffer[0],
|
|
total_size);
|
|
|
|
// Erase the reserved flash for new app
|
|
flash_helper_erase_new_app(RESERVED_FLASH_SPACE_SIZE);
|
|
|
|
last_read_file_req = chVTGetSystemTimeX();
|
|
|
|
if (debug_level > 0) {
|
|
commands_printf("UAVCAN Begin firmware update from node_id: %d",fw_update.node_id);
|
|
}
|
|
|
|
send_fw_read(ins);
|
|
}
|
|
|
|
/**
|
|
* 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 == 4) {
|
|
commands_printf("UAVCAN transfer RX: NODE: %d Type: %d ID: %d",
|
|
transfer->source_node_id, transfer->transfer_type, transfer->data_type_id);
|
|
}
|
|
|
|
/*
|
|
* Dynamic node ID allocation protocol.
|
|
* Taking this branch only if we don't have a node ID, ignoring otherwise.
|
|
*/
|
|
// if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) {
|
|
// if (transfer->transfer_type == CanardTransferTypeBroadcast &&
|
|
// transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
|
|
// handle_allocation_response(ins, transfer);
|
|
// }
|
|
// return;
|
|
// }
|
|
|
|
switch (transfer->data_type_id) {
|
|
case UAVCAN_PROTOCOL_GETNODEINFO_ID:
|
|
handle_get_node_info(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
|
|
handle_esc_raw_command(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID:
|
|
handle_esc_rpm_command(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_STATUS_ID:
|
|
handle_esc_status(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_PROTOCOL_RESTARTNODE_ID:
|
|
if (debug_level > 0) {
|
|
commands_printf("RestartNode\n");
|
|
}
|
|
handle_restart_node();
|
|
break;
|
|
|
|
case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
|
|
handle_param_getset(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
|
|
handle_begin_firmware_update(ins, transfer);
|
|
break;
|
|
|
|
case UAVCAN_PROTOCOL_FILE_READ_ID:
|
|
handle_file_read_response(ins, transfer);
|
|
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 == 4) {
|
|
commands_printf("UAVCAN shouldAccept: NODE: %d Type: %d ID: %d",
|
|
source_node_id, transfer_type, data_type_id);
|
|
}
|
|
|
|
// This is for future use if Dynamic node ID allocation is used.
|
|
// if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID)
|
|
// {
|
|
// /*
|
|
// * If we're in the process of allocation of dynamic node ID, accept only relevant transfers.
|
|
// */
|
|
// if ((transfer_type == CanardTransferTypeBroadcast) &&
|
|
// (data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID))
|
|
// {
|
|
// *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
|
|
// return true;
|
|
// }
|
|
// return false;
|
|
// }
|
|
|
|
switch (data_type_id) {
|
|
case UAVCAN_PROTOCOL_GETNODEINFO_ID:
|
|
*out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID:
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_EQUIPMENT_ESC_STATUS_ID:
|
|
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_PROTOCOL_RESTARTNODE_ID:
|
|
*out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
|
|
*out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_PROTOCOL_FILE_READ_ID:
|
|
*out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE;
|
|
return true;
|
|
|
|
case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
|
|
*out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE;
|
|
return true;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
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");
|
|
|
|
getParamByName("controller_id")->defval = HW_DEFAULT_ID;
|
|
|
|
canardInit(&canard_ins, canard_memory_pool, sizeof(canard_memory_pool),
|
|
onTransferReceived, shouldAcceptTransfer, NULL);
|
|
|
|
#ifdef HW_CAN2_DEV
|
|
canardInit(&canard_ins_if2, canard_memory_pool_if2, sizeof(canard_memory_pool_if2),
|
|
onTransferReceived, shouldAcceptTransfer, NULL);
|
|
#endif
|
|
|
|
systime_t last_status_time = 0;
|
|
systime_t last_esc_status_time = 0;
|
|
systime_t last_tot_current_calc_time = 0;
|
|
systime_t last_param_refresh = 0;
|
|
|
|
for (;;) {
|
|
const app_configuration *conf = app_get_configuration();
|
|
|
|
if (conf->can_mode != CAN_MODE_UAVCAN) {
|
|
chThdSleepMilliseconds(100);
|
|
continue;
|
|
}
|
|
|
|
my_node_id = conf->controller_id;
|
|
canardSetLocalNodeID(&canard_ins, conf->controller_id);
|
|
|
|
#ifdef HW_CAN2_DEV
|
|
canardSetLocalNodeID(&canard_ins_if2, conf->controller_id);
|
|
#endif
|
|
|
|
CANRxFrame *rxmsg;
|
|
while ((rxmsg = comm_can_get_rx_frame(1)) != 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_ins, &rx_frame, ST2US(chVTGetSystemTimeX()));
|
|
}
|
|
|
|
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard_ins)) != NULL;) {
|
|
comm_can_transmit_eid_if(txf->id, txf->data, txf->data_len, 1);
|
|
canardPopTxQueue(&canard_ins);
|
|
}
|
|
|
|
#ifdef HW_CAN2_DEV
|
|
while ((rxmsg = comm_can_get_rx_frame(2)) != 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_ins_if2, &rx_frame, ST2US(chVTGetSystemTimeX()));
|
|
}
|
|
|
|
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard_ins_if2)) != NULL;) {
|
|
comm_can_transmit_eid_if(txf->id, txf->data, txf->data_len, 2);
|
|
canardPopTxQueue(&canard_ins_if2);
|
|
}
|
|
#endif
|
|
|
|
if (ST2MS(chVTTimeElapsedSinceX(last_status_time)) >= 1000) {
|
|
last_status_time = chVTGetSystemTimeX();
|
|
canardCleanupStaleTransfers(&canard_ins, ST2US(chVTGetSystemTimeX()));
|
|
sendNodeStatus(&canard_ins);
|
|
#ifdef HW_CAN2_DEV
|
|
canardCleanupStaleTransfers(&canard_ins_if2, ST2US(chVTGetSystemTimeX()));
|
|
sendNodeStatus(&canard_ins_if2);
|
|
#endif
|
|
}
|
|
|
|
if (conf->can_status_rate_1 > 0 &&
|
|
UTILS_AGE_S(last_esc_status_time) >= (1.0 / (float)conf->can_status_rate_1)) {
|
|
last_esc_status_time = chVTGetSystemTimeX();
|
|
sendEscStatus(&canard_ins);
|
|
#ifdef HW_CAN2_DEV
|
|
sendEscStatus(&canard_ins_if2);
|
|
#endif
|
|
}
|
|
|
|
if (ST2MS(chVTTimeElapsedSinceX(last_tot_current_calc_time)) >= 1000 / CURRENT_CALC_FREQ_HZ) {
|
|
last_tot_current_calc_time = chVTGetSystemTimeX();
|
|
calculateTotalCurrent();
|
|
if (debug_level == 3) {
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
|
commands_printf("Max Current: %0.02f", (double)mcconf->lo_current_motor_max_now);
|
|
}
|
|
}
|
|
|
|
if (ST2MS(chVTTimeElapsedSinceX(last_param_refresh)) >= 1000 / PARAM_REFRESH_RATE_HZ) {
|
|
last_param_refresh = chVTGetSystemTimeX();
|
|
if(refresh_parameters_enabled) {
|
|
refresh_parameters();
|
|
}
|
|
|
|
if(debug_level == 7) {
|
|
commands_printf("Refreshing Parameters Time Delta: %d", ST2MS(chVTGetSystemTimeX()-last_param_refresh));
|
|
}
|
|
}
|
|
|
|
if ((ST2MS(chVTTimeElapsedSinceX(last_read_file_req)) >= 10) && (fw_update.node_id != 0)) {
|
|
last_read_file_req = chVTGetSystemTimeX();
|
|
send_fw_read(fw_update.ins);
|
|
}
|
|
|
|
// delay jump to bootloader after receiving data for 0.5 sec
|
|
if ((ST2MS(chVTTimeElapsedSinceX(jump_delay_start)) >= 500) && (jump_to_bootloader == true)) {
|
|
flash_helper_jump_to_bootloader();
|
|
}
|
|
|
|
chThdSleepMilliseconds(1);
|
|
}
|
|
}
|