forked from test34/can_wizard
355 lines
12 KiB
C
355 lines
12 KiB
C
#include "cmd_can.h"
|
|
#include "can.h"
|
|
#include "esp_err.h"
|
|
#include "esp_log.h"
|
|
#include "hal/twai_types.h"
|
|
#include "inttypes.h"
|
|
#include "freertos/projdefs.h"
|
|
#include "string.h"
|
|
#include "esp_console.h"
|
|
#include "argtable3/argtable3.h"
|
|
#include "xvprintf.h"
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
#include <ctype.h>
|
|
|
|
twai_filter_config_t my_filters = TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
|
|
|
static void register_cansend(void);
|
|
static void register_canup(void);
|
|
static void register_candown(void);
|
|
static void register_canstats(void);
|
|
static void register_canstart(void);
|
|
static void register_canrecover(void);
|
|
|
|
void register_can_commands(void) {
|
|
register_cansend();
|
|
register_canup();
|
|
register_candown();
|
|
register_canstats();
|
|
register_canstart();
|
|
register_canrecover();
|
|
}
|
|
|
|
static struct {
|
|
struct arg_str *message;
|
|
struct arg_end *end;
|
|
} cansend_args;
|
|
|
|
static int send_can_frame(int argc, char **argv) {
|
|
twai_message_t msg = {.extd = 1};
|
|
char printf_str[70];
|
|
int nerrors = arg_parse(argc, argv, (void **) &cansend_args);
|
|
if (nerrors != 0) {
|
|
arg_print_errors(stderr, cansend_args.end, argv[0]);
|
|
return 1;
|
|
}
|
|
const char *can_msg_ptr = cansend_args.message->sval[0];
|
|
char *can_msg_str_buf = strdup(can_msg_ptr);
|
|
char *id_substr = strtok(can_msg_str_buf, "#");
|
|
char *data_substr = strtok(NULL, "#");
|
|
if ((id_substr == NULL) || (strtok(NULL, "#") != NULL)) goto invalid_args;
|
|
int id_l = strlen(id_substr);
|
|
int dt_l = data_substr == NULL ? 0 : strlen(data_substr);
|
|
if ((id_l > 8) || (dt_l > 16) || (id_l % 2) || (dt_l % 2)) goto invalid_args;
|
|
for (int i = 0; i < id_l; i++) if(!isxdigit((int) id_substr[i])) goto invalid_args;
|
|
for (int i = 0; i < dt_l; i++) if(!isxdigit((int) data_substr[i])) goto invalid_args;
|
|
int msg_id;
|
|
if (sscanf(id_substr, "%X", &msg_id) < 1) goto invalid_args;
|
|
for (int i = 0; i < (dt_l / 2); i++) {
|
|
char *byte_to_parse = malloc(3);
|
|
strncpy(byte_to_parse, data_substr + i * 2, 2);
|
|
int num;
|
|
int res = sscanf(byte_to_parse, "%X", &num);
|
|
free(byte_to_parse);
|
|
if (res < 1) goto invalid_args;
|
|
msg.data[i] = num;
|
|
}
|
|
msg.data_length_code = dt_l / 2;
|
|
msg.identifier = msg_id;
|
|
esp_err_t res = twai_transmit(&msg, pdMS_TO_TICKS(1000));
|
|
switch(res) {
|
|
case ESP_OK:
|
|
can_msg_to_str(&msg, "sent ", printf_str);
|
|
print_w_clr_time(printf_str, NULL, true);
|
|
break;
|
|
case ESP_ERR_TIMEOUT:
|
|
print_w_clr_time("Timeout!", LOG_COLOR_RED, true);
|
|
break;
|
|
case ESP_ERR_NOT_SUPPORTED:
|
|
print_w_clr_time("Can't sent in Listen-Only mode!", LOG_COLOR_RED, true);
|
|
break;
|
|
default:
|
|
print_w_clr_time("Invalid state!", LOG_COLOR_RED, true);
|
|
break;
|
|
}
|
|
free(can_msg_str_buf);
|
|
return 0;
|
|
invalid_args:
|
|
printf("Invalid arguments!\n");
|
|
free(can_msg_str_buf);
|
|
return 1;
|
|
}
|
|
|
|
static int canrecover(int argc, char **argv) {
|
|
esp_err_t res = twai_initiate_recovery();
|
|
if (res == ESP_OK) print_w_clr_time("Started CAN recovery.", LOG_COLOR_GREEN, true);
|
|
else if (curr_can_state.state == CAN_NOT_INSTALLED) print_w_clr_time("CAN driver is not installed!", LOG_COLOR_RED, true);
|
|
else print_w_clr_time("Can't start recovery - not in bus-off state!", LOG_COLOR_RED, true);
|
|
return 0;
|
|
}
|
|
|
|
static const char* can_states_str[] = {
|
|
"not installed",
|
|
"stopped",
|
|
"error active",
|
|
"error passive",
|
|
"bus off",
|
|
"recovering"
|
|
};
|
|
|
|
static int canstats(int argc, char **argv) {
|
|
if (curr_can_state.state == CAN_NOT_INSTALLED) {
|
|
printf("CAN driver is not installed!\n");
|
|
return 0;
|
|
} else {
|
|
const char *state_str = can_states_str[curr_can_state.state];
|
|
printf("status: %s\n", state_str);
|
|
printf("TX Err Counter: %" PRIu32 "\n", curr_can_state.tx_error_counter);
|
|
printf("RX Err Counter: %" PRIu32 "\n", curr_can_state.rx_error_counter);
|
|
printf("Failed transmit: %" PRIu32 "\n", curr_can_state.tx_failed_count);
|
|
printf("Arbitration lost times: %" PRIu32 "\n", curr_can_state.arb_lost_count);
|
|
printf("Bus-off count: %" PRIu32 "\n", curr_can_state.bus_error_count);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static const char* can_modes[] = {
|
|
"normal",
|
|
"no_ack",
|
|
"listen_only",
|
|
};
|
|
|
|
static struct {
|
|
struct arg_int *speed;
|
|
struct arg_lit *filters;
|
|
struct arg_lit *autorecover;
|
|
struct arg_str *mode;
|
|
struct arg_end *end;
|
|
} canup_args;
|
|
|
|
static int canup(int argc, char **argv) {
|
|
esp_err_t res;
|
|
static twai_timing_config_t t_config;
|
|
twai_general_config_t gen_cfg = default_g_config;
|
|
// TODO: add CAN filtering
|
|
twai_filter_config_t f_config;
|
|
if (canup_args.filters->count) {
|
|
f_config = my_filters;
|
|
print_w_clr_time("Using predefined filters.", LOG_COLOR_GREEN, true);
|
|
} else {
|
|
f_config = (twai_filter_config_t) TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
|
print_w_clr_time("Using accept all filters.", LOG_COLOR_GREEN, true);
|
|
}
|
|
esp_log_level_t prev_gpio_lvl = esp_log_level_get("gpio");
|
|
int nerrors = arg_parse(argc, argv, (void **) &canup_args);
|
|
if (nerrors != 0) {
|
|
arg_print_errors(stderr, canup_args.end, argv[0]);
|
|
return 1;
|
|
}
|
|
int mode = 0;
|
|
if (canup_args.mode->count) {
|
|
const char* mode_str = canup_args.mode->sval[0];
|
|
while (mode < 4) {
|
|
if (mode == 3) {
|
|
print_w_clr_time("Unsupported mode!", LOG_COLOR_RED, true);
|
|
return 1;
|
|
}
|
|
if (memcmp(mode_str, can_modes[mode], strlen(mode_str)) == 0) break;
|
|
mode++;
|
|
}
|
|
}
|
|
switch(mode) {
|
|
case 1:
|
|
gen_cfg.mode = TWAI_MODE_NO_ACK;
|
|
print_w_clr_time("Starting CAN in No Ack Mode...", LOG_COLOR_BLUE, true);
|
|
break;
|
|
case 2:
|
|
gen_cfg.mode = TWAI_MODE_LISTEN_ONLY;
|
|
print_w_clr_time("Starting CAN in Listen Only Mode...", LOG_COLOR_BLUE, true);
|
|
break;
|
|
default: //0
|
|
print_w_clr_time("Starting CAN in Normal Mode...", LOG_COLOR_BLUE, true);
|
|
break;
|
|
}
|
|
switch (canup_args.speed->ival[0]) {
|
|
case 1000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_1KBITS();
|
|
break;
|
|
case 5000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_5KBITS();
|
|
break;
|
|
case 10000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_10KBITS();
|
|
break;
|
|
case 12500:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_12_5KBITS();
|
|
break;
|
|
case 16000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_16KBITS();
|
|
break;
|
|
case 20000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_20KBITS();
|
|
break;
|
|
case 25000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_25KBITS();
|
|
break;
|
|
case 50000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_50KBITS();
|
|
break;
|
|
case 100000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_100KBITS();
|
|
break;
|
|
case 125000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_125KBITS();
|
|
break;
|
|
case 250000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_250KBITS();
|
|
break;
|
|
case 500000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_500KBITS();
|
|
break;
|
|
case 800000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_800KBITS();
|
|
break;
|
|
case 1000000:
|
|
t_config = (twai_timing_config_t) TWAI_TIMING_CONFIG_1MBITS();
|
|
break;
|
|
default:
|
|
print_w_clr_time("Unsupported speed!", LOG_COLOR_RED, true);
|
|
return 1;
|
|
}
|
|
xSemaphoreTake(can_mutex, portMAX_DELAY);
|
|
esp_log_level_set("gpio", ESP_LOG_ERROR);
|
|
res = twai_driver_install(&gen_cfg, &t_config, &f_config);
|
|
if (res == ESP_OK) {
|
|
print_w_clr_time("CAN driver installed", LOG_COLOR_BLUE, true);
|
|
if (canup_args.autorecover->count) {
|
|
print_w_clr_time("Auto recovery is enabled!", LOG_COLOR_PURPLE, true);
|
|
auto_recovery = true;
|
|
} else auto_recovery = false;
|
|
} else if (res == ESP_ERR_INVALID_STATE) {
|
|
print_w_clr_time("Driver is already installed!", LOG_COLOR_BROWN, true);
|
|
goto free_exit;
|
|
} else {
|
|
print_w_clr_time("Couldn't install CAN driver! Rebooting...", LOG_COLOR_RED, true);
|
|
esp_restart();
|
|
}
|
|
ESP_ERROR_CHECK(twai_start());
|
|
is_error_passive = false;
|
|
print_w_clr_time("CAN driver started", LOG_COLOR_BLUE, true);
|
|
free_exit:
|
|
xSemaphoreGive(can_mutex);
|
|
esp_log_level_set("gpio", prev_gpio_lvl);
|
|
return 0;
|
|
}
|
|
|
|
static int canstart(int argc, char **argv) {
|
|
xSemaphoreTake(can_mutex, portMAX_DELAY);
|
|
esp_err_t res = twai_start();
|
|
if (res == ESP_OK) {
|
|
print_w_clr_time("CAN driver started", LOG_COLOR_GREEN, true);
|
|
is_error_passive = false;
|
|
} else print_w_clr_time("Driver is not in stopped state, or is not installed.", LOG_COLOR_RED, true);
|
|
xSemaphoreGive(can_mutex);
|
|
return 0;
|
|
}
|
|
|
|
static int candown(int argc, char **argv) {
|
|
xSemaphoreTake(can_mutex, portMAX_DELAY);
|
|
if (curr_can_state.state != CAN_BUF_OFF) {
|
|
esp_err_t res = twai_stop();
|
|
if (res == ESP_OK) print_w_clr_time("CAN was stopped.", LOG_COLOR_GREEN, true);
|
|
else {
|
|
print_w_clr_time("Driver is not in running state, or is not installed.", LOG_COLOR_RED, true);
|
|
xSemaphoreGive(can_mutex);
|
|
return 1;
|
|
}
|
|
}
|
|
ESP_ERROR_CHECK(twai_driver_uninstall());
|
|
xSemaphoreGive(can_mutex);
|
|
return 0;
|
|
}
|
|
|
|
static void register_cansend(void) {
|
|
|
|
cansend_args.message = arg_str1(NULL, NULL, "ID#data", "Message to send, ID and data bytes, all in hex. # is the delimiter.");
|
|
cansend_args.end = arg_end(2);
|
|
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "cansend",
|
|
.help = "Send a can message to the bus, example: cansend 00008C03#02",
|
|
.hint = NULL,
|
|
.func = &send_can_frame,
|
|
.argtable = &cansend_args,
|
|
};
|
|
ESP_ERROR_CHECK( esp_console_cmd_register(&cmd) );
|
|
}
|
|
|
|
static void register_canup(void) {
|
|
|
|
canup_args.speed = arg_int1(NULL, NULL, "<speed>", "CAN bus speed, in bps. See help for supported speeds.");
|
|
canup_args.mode = arg_str0("m", "mode", "<normal|no_ack|listen_only>", "Set CAN mode. Normal (default), No Ack (for self-testing) or Listen Only (to prevent transmitting, for monitoring).");
|
|
canup_args.filters = arg_lit0("f", NULL, "Use predefined CAN filters.");
|
|
canup_args.autorecover = arg_lit0("r", "auto-recovery", "Set to enable auto-recovery of CAN bus if case of bus-off event");
|
|
canup_args.end = arg_end(4);
|
|
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "canup",
|
|
.help = "Install can drivers and start can interface. Used right after board start or during runtime for changing CAN configuration. Supported speeds: 1mbits, 800kbits, 500kbits, 250kbits, 125kbits, 100kbits, 50kbits, 25kbits, 20kbits, 16kbits, 12.5kbits, 10kbits, 5kbits, 1kbits.",
|
|
.hint = NULL,
|
|
.func = &canup,
|
|
.argtable = &canup_args,
|
|
};
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
|
|
}
|
|
|
|
static void register_candown(void) {
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "candown",
|
|
.help = "Stop CAN interface and uninstall CAN driver, for example, to install and start with different parameters/filters.",
|
|
.hint = NULL,
|
|
.func = &candown,
|
|
};
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
|
|
}
|
|
|
|
static void register_canstats(void) {
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "canstats",
|
|
.help = "Print CAN statistics.",
|
|
.hint = NULL,
|
|
.func = &canstats,
|
|
};
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
|
|
}
|
|
|
|
static void register_canstart(void) {
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "canstart",
|
|
.help = "Start CAN interface, used after bus recovery, otherwise see canup command.",
|
|
.hint = NULL,
|
|
.func = &canstart,
|
|
};
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
|
|
}
|
|
|
|
static void register_canrecover(void) {
|
|
const esp_console_cmd_t cmd = {
|
|
.command = "canrecover",
|
|
.help = "Recover CAN after buf-off. Used when auto-recovery is turned off.",
|
|
.hint = NULL,
|
|
.func = &canrecover,
|
|
};
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
|
|
}
|