static int init(t_hydra_console *con, t_tokenline_parsed *p)
{
mode_config_proto_t* proto = &con->mode->proto;
bsp_status_t bsp_status;
int tokens_used;
/* Defaults */
init_proto_default(con);
/* Process cmdline arguments, skipping "can". */
tokens_used = 1 + exec(con, p, 1);
bsp_status = bsp_can_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
}
/* By default, get all packets */
if (proto->config.can.filter_id_low != 0 || proto->config.can.filter_id_high != 0) {
bsp_status = bsp_can_set_filter(proto->dev_num, proto,
proto->config.can.filter_id_low,
proto->config.can.filter_id_high);
} else {
bsp_status = bsp_can_init_filter(proto->dev_num, proto);
}
if( bsp_status != BSP_OK) {
cprintf(con, "bsp_can_init_filter() error %d\r\n", bsp_status);
}
show_params(con);
return tokens_used;
}
result_t can_aerospace_init(const neutron_parameters_t *params, bool init_mode)
{
handle_t task_id;
result_t result;
hardware_revision = params->hardware_revision;
software_revision = params->software_revision;
node_id = params->node_id;
if (failed(result = deque_create(sizeof(canmsg_t),
params->tx_length == 0
? DEFAULT_TX_QUEUE_SIZE
: params->tx_length,
&can_tx_queue)))
{
trace_error("Cannot create can_txt_queue");
return result;
}
if (failed(result = deque_create(sizeof(canmsg_t),
params->rx_length == 0
? DEFAULT_RX_QUEUE_SIZE
: params->rx_length,
&can_rx_queue)))
{
trace_error("Cannot create can_rx_queue");
return result;
}
if (failed(result = task_create("CAN_TX",
params->tx_stack_length,
can_tx_task, 0,
NORMAL_PRIORITY, &task_id)))
{
trace_error("Cannot create the can_tx task");
return result;
}
if (failed(result = task_create("CAN_RX",
params->rx_stack_length,
can_rx_task, 0,
NORMAL_PRIORITY + 1, &task_id)))
{
trace_error("Cannot create the can_rx task");
return result;
}
if(failed(result = neutron_init(params, init_mode)))
return result;
// start the can driver running.
return bsp_can_init(can_rx_queue);
}
static int exec(t_hydra_console *con, t_tokenline_parsed *p, int token_pos)
{
mode_config_proto_t* proto = &con->mode->proto;
int arg_int, t;
bsp_status_t bsp_status;
for (t = token_pos; p->tokens[t]; t++) {
switch (p->tokens[t]) {
case T_SHOW:
t += show(con, p);
break;
case T_DEVICE:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 1 || arg_int > 2) {
cprintf(con, "CAN device must be 1 or 2.\r\n");
return t;
}
proto->dev_num = arg_int - 1;
bsp_status = bsp_can_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
tl_set_prompt(con->tl, (char *)con->mode->exec->get_prompt(con));
cprintf(con, "Note: CAN parameters have been reset to default values.\r\n");
break;
case T_SPEED:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
bsp_status = bsp_can_set_speed(proto->dev_num, arg_int);
proto->config.can.dev_speed = bsp_can_get_speed(proto->dev_num);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
cprintf(con, "Speed: %d bps\r\n", proto->config.can.dev_speed);
break;
case T_TS1:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 16) {
bsp_status = bsp_can_set_ts1(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_TS2:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 8) {
bsp_status = bsp_can_set_ts2(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_SJW:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 4) {
bsp_status = bsp_can_set_sjw(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_FILTER:
/* Integer parameter. */
switch(p->tokens[t+1]) {
case T_OFF:
bsp_status = bsp_can_init_filter(proto->dev_num,
proto);
if(bsp_status != BSP_OK) {
cprintf(con, "Reset filter error %02X", bsp_status);
}
break;
case T_LOW:
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.filter_id_low = arg_int;
bsp_status = bsp_can_set_filter(proto->dev_num,
proto,
proto->config.can.filter_id_low,
proto->config.can.filter_id_high);
break;
case T_HIGH:
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.filter_id_high = arg_int;
bsp_status = bsp_can_set_filter(proto->dev_num,
proto,
proto->config.can.filter_id_low,
proto->config.can.filter_id_high);
break;
}
t+=3;
break;
case T_ID:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.can_id = arg_int;
cprintf(con, "ID set to %d\r\n", proto->config.can.can_id);
break;
case T_CONTINUOUS:
while(!USER_BUTTON) {
read(con, NULL, 0);
}
break;
case T_SLCAN:
if(proto->config.can.dev_mode == BSP_CAN_MODE_RO) {
bsp_can_mode_rw(proto->dev_num, proto);
}
slcan(con);
break;
default:
return t - token_pos;
}
}
return t - token_pos;
}
