/*! \brief CAN Prepare Data to Receive * - Allocate one MOB in reception * - Start the reception */ void can_example_prepare_data_to_receive(void) { // Initialize channel 0 can_init(0, ((U32)&mob_ram_ch0[0]), CANIF_CHANNEL_MODE_NORMAL, can_out_callback_channel0); // Allocate one mob for RX pCANMOB_message2[0].handle = can_mob_alloc(0); can_rx(0, pCANMOB_message2[0].handle, pCANMOB_message2[0].req_type, pCANMOB_message2[0].can_msg); }
/** Main function to configure the CAN, and begin transfers. */ int main(void) { /* Initialize the system clocks */ sysclk_init(); /* Setup the generic clock for CAN */ scif_gc_setup(AVR32_SCIF_GCLK_CANIF, SCIF_GCCTRL_OSC0, AVR32_SCIF_GC_NO_DIV_CLOCK, 0); /* Now enable the generic clock */ scif_gc_enable(AVR32_SCIF_GCLK_CANIF); init_dbg_rs232(FPBA_HZ); /* Disable all interrupts. */ Disable_global_interrupt(); /* Initialize interrupt vectors. */ INTC_init_interrupts(); static const gpio_map_t CAN_GPIO_MAP = { {AVR32_CANIF_RXLINE_0_0_PIN, AVR32_CANIF_RXLINE_0_0_FUNCTION}, {AVR32_CANIF_TXLINE_0_0_PIN, AVR32_CANIF_TXLINE_0_0_FUNCTION} }; /* Assign GPIO to CAN. */ gpio_enable_module(CAN_GPIO_MAP, sizeof(CAN_GPIO_MAP) / sizeof(CAN_GPIO_MAP[0])); /* Initialize channel 0 */ can_init(CAN_CHANNEL_EXAMPLE, ((uint32_t)&mob_ram_ch0[0]), CANIF_CHANNEL_MODE_LISTENING, can_out_callback_channel0); /* Enable all interrupts. */ Enable_global_interrupt(); print_dbg("\r\nUC3C CAN Examples 1\r\n"); print_dbg(CAN_Wakeup); /* Initialize CAN Channel 0 */ can_init(CAN_CHANNEL_EXAMPLE, ((uint32_t)&mob_ram_ch0[0]), CANIF_CHANNEL_MODE_NORMAL, can_out_callback_channel0); /* Allocate one mob for RX */ appli_rx_msg.handle = can_mob_alloc(CAN_CHANNEL_EXAMPLE); /* Initialize RX message */ can_rx(CAN_CHANNEL_EXAMPLE, appli_rx_msg.handle, appli_rx_msg.req_type, appli_rx_msg.can_msg); /* Enable Async Wake Up Mode */ pm_asyn_wake_up_enable(CAN_WAKEUP_MASK_EXAMPLE); /* ---------SLEEP MODE PROCEDURE------------- */ /* Disable CAN Channel 0 */ CANIF_disable(CAN_CHANNEL_EXAMPLE); /* Wait CAN Channel 0 is disabled */ while(!CANIF_channel_enable_status(CAN_CHANNEL_EXAMPLE)); /* Enable Wake-Up Mode */ CANIF_enable_wakeup(CAN_CHANNEL_EXAMPLE); /* Go to sleep mode. */ SLEEP(AVR32_PM_SMODE_STATIC); /* ---------SLEEP MODE PROCEDURE------------- */ print_dbg(CAN_WakeupD); /* Initialize again CAN Channel 0 */ can_init(CAN_CHANNEL_EXAMPLE, ((uint32_t)&mob_ram_ch0[0]), CANIF_CHANNEL_MODE_NORMAL, can_out_callback_channel0); /* Allocate one mob for RX */ appli_rx_msg.handle = can_mob_alloc(CAN_CHANNEL_EXAMPLE); /* Initialize RX message */ can_rx(CAN_CHANNEL_EXAMPLE, appli_rx_msg.handle, appli_rx_msg.req_type, appli_rx_msg.can_msg); for (;;) { /* Do nothing; interrupts handle the DAC conversions */ } }
static void __attribute__((noreturn)) node_run( uint8_t node_id, Configuration& configuration, const struct motor_params_t& motor_params, const struct control_params_t& control_params ) { size_t length; uint32_t message_id, current_time, node_status_time, esc_status_time, foc_status_time, node_status_interval, esc_status_interval, foc_status_interval, last_setpoint_update, enumeration_deadline; uint8_t filter_id, foc_status_transfer_id, node_status_transfer_id, esc_status_transfer_id, enumeration_indication_transfer_id, esc_index, message[8], broadcast_filter_id, service_filter_id; enum controller_mode_t mode; float setpoint, value, power_w, v_dq_v[2], to_rpm; bool got_setpoint, param_valid, wants_bootloader_restart; struct param_t param; struct motor_state_t motor_state; uint16_t foc_status_dtid; UAVCANTransferManager broadcast_manager(node_id); UAVCANTransferManager service_manager(node_id); uavcan::equipment::esc::RawCommand raw_cmd; uavcan::equipment::indication::BeepCommand beep_cmd; uavcan::protocol::param::ExecuteOpcode::Request xo_req; uavcan::protocol::param::GetSet::Request gs_req; uavcan::protocol::RestartNode::Request rn_req; uavcan::protocol::enumeration::Begin::Request enum_req; foc_status_transfer_id = node_status_transfer_id = esc_status_transfer_id = enumeration_indication_transfer_id = 0u; broadcast_filter_id = service_filter_id = 0xFFu; foc_status_time = node_status_time = esc_status_time = last_setpoint_update = enumeration_deadline = 0u; wants_bootloader_restart = false; node_status_interval = 900u; to_rpm = float(60.0 / M_PI) / float(motor_params.num_poles); while (true) { current_time = g_controller_state.time; got_setpoint = false; setpoint = 0.0f; /* Allow UAVCAN parameters to be changed without restarting */ esc_index = (uint8_t) configuration.get_param_value_by_index(PARAM_UAVCAN_ESC_INDEX); foc_status_dtid = (uint16_t) configuration.get_param_value_by_index(PARAM_THIEMAR_STATUS_ID); esc_status_interval = (uint32_t)( configuration.get_param_value_by_index( PARAM_UAVCAN_ESCSTATUS_INTERVAL) * 0.001f); foc_status_interval = (uint32_t)( configuration.get_param_value_by_index( PARAM_THIEMAR_STATUS_INTERVAL) * 0.001f); /* Check for UAVCAN commands (FIFO 0) -- these are all broadcasts */ while (can_rx(0u, &filter_id, &message_id, &length, message)) { uavcan::TransferCRC crc; /* Filter IDs are per-FIFO, so convert this back to the bank index */ filter_id = (uint8_t)(filter_id + UAVCAN_EQUIPMENT_ESC_RAWCOMMAND); switch (filter_id) { case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND: crc = uavcan::equipment::esc::RawCommand::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND: crc = uavcan::equipment::indication::BeepCommand::getDataTypeSignature().toTransferCRC(); break; default: break; } broadcast_manager.receive_frame(current_time, message_id, crc, length, message); if (broadcast_manager.is_rx_done()) { broadcast_filter_id = filter_id; break; } } if (broadcast_manager.is_rx_done()) { if (broadcast_filter_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND && broadcast_manager.decode(raw_cmd) && esc_index < raw_cmd.cmd.size() && raw_cmd.cmd[esc_index] != 0) { got_setpoint = true; mode = CONTROLLER_VOLTAGE; /* Scale 0-8191 to represent 0 to maximum voltage. */ value = g_vbus_v; setpoint = /* std::min(motor_params.max_voltage_v, value) */ motor_params.max_voltage_v * float(raw_cmd.cmd[esc_index]) * float(1.0 / 8192.0); if (setpoint > 0.0f && setpoint < motor_params.accel_voltage_v) { setpoint = motor_params.accel_voltage_v; } if (setpoint < 0.0f && setpoint > -motor_params.accel_voltage_v) { setpoint = -motor_params.accel_voltage_v; } } else if (broadcast_filter_id == UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND && broadcast_manager.decode(beep_cmd)) { /* Set up audio generation */ g_audio_state.off_time = uint32_t(beep_cmd.duration * (1.0f / hal_control_t_s)); g_audio_state.angular_velocity_rad_per_u = float(2.0 * M_PI) * hal_control_t_s * std::max(100.0f, beep_cmd.frequency); g_audio_state.volume_v = motor_params.accel_voltage_v; } broadcast_manager.receive_acknowledge(); } /* Check for UAVCAN service requests (FIFO 1) -- only process if the first byte of the data is the local node ID */ while (can_rx(1u, &filter_id, &message_id, &length, message)) { uavcan::TransferCRC crc; switch (filter_id) { case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE: crc = uavcan::protocol::param::ExecuteOpcode::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_PROTOCOL_PARAM_GETSET: crc = uavcan::protocol::param::GetSet::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE: crc = uavcan::protocol::file::BeginFirmwareUpdate::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_PROTOCOL_GETNODEINFO: crc = uavcan::protocol::GetNodeInfo::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_PROTOCOL_RESTARTNODE: crc = uavcan::protocol::RestartNode::getDataTypeSignature().toTransferCRC(); break; case UAVCAN_PROTOCOL_ENUMERATION_BEGIN: crc = uavcan::protocol::enumeration::Begin::getDataTypeSignature().toTransferCRC(); break; default: break; } service_manager.receive_frame(current_time, message_id, crc, length, message); if (service_manager.is_rx_done()) { service_filter_id = filter_id; break; } } /* Don't process service requests until the last service response is completely sent, to avoid overwriting the TX buffer. */ if (service_manager.is_rx_done() && service_manager.is_tx_done()) { if (service_filter_id == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE && service_manager.decode(xo_req)) { /* Return OK if the opcode is understood and the controller is stopped, otherwise reject. */ uavcan::protocol::param::ExecuteOpcode::Response xo_resp; xo_resp.ok = false; if (g_controller_state.mode == CONTROLLER_STOPPED && xo_req.opcode == xo_req.OPCODE_SAVE) { configuration.write_params(); xo_resp.ok = true; } else if (g_controller_state.mode == CONTROLLER_STOPPED && xo_req.opcode == xo_req.OPCODE_ERASE) { /* Set all parameters to default values, then erase the flash */ configuration.reset_params(); configuration.write_params(); xo_resp.ok = true; } service_manager.encode_response<uavcan::protocol::param::ExecuteOpcode>(xo_resp); } else if (service_filter_id == UAVCAN_PROTOCOL_PARAM_GETSET && service_manager.decode(gs_req)) { uavcan::protocol::param::GetSet::Response resp; if (!gs_req.name.empty()) { param_valid = configuration.get_param_by_name( param, gs_req.name.c_str()); } else { param_valid = configuration.get_param_by_index( param, (uint8_t)gs_req.index); } if (param_valid) { if (param.public_type == PARAM_TYPE_FLOAT && !gs_req.name.empty() && gs_req.value.is(uavcan::protocol::param::Value::Tag::real_value)) { value = gs_req.value.to<uavcan::protocol::param::Value::Tag::real_value>(); configuration.set_param_value_by_index(param.index, value); } else if (param.public_type == PARAM_TYPE_INT && !gs_req.name.empty() && gs_req.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { value = (float)((int32_t)gs_req.value.to<uavcan::protocol::param::Value::Tag::integer_value>()); configuration.set_param_value_by_index(param.index, value); } value = configuration.get_param_value_by_index( param.index); resp.name = (const char*)param.name; if (param.public_type == PARAM_TYPE_FLOAT) { resp.value.to<uavcan::protocol::param::Value::Tag::real_value>() = value; resp.default_value.to<uavcan::protocol::param::Value::Tag::real_value>() = param.default_value; resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = param.min_value; resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = param.max_value; } else if (param.public_type == PARAM_TYPE_INT) { resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = (int32_t)value; resp.default_value.to<uavcan::protocol::param::Value::Tag::integer_value>() = (int32_t)param.default_value; resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = (int32_t)param.min_value; resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = (int32_t)param.max_value; } } service_manager.encode_response<uavcan::protocol::param::GetSet>(resp); } else if (service_filter_id == UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE) { uavcan::protocol::file::BeginFirmwareUpdate::Response resp; /* Don't actually need to decode since we don't care about the request data */ if (g_controller_state.mode == CONTROLLER_STOPPED) { resp.error = resp.ERROR_OK; wants_bootloader_restart = true; } else { resp.error = resp.ERROR_INVALID_MODE; } service_manager.encode_response<uavcan::protocol::file::BeginFirmwareUpdate>(resp); } else if (service_filter_id == UAVCAN_PROTOCOL_GETNODEINFO) { uavcan::protocol::GetNodeInfo::Response resp; /* Empty request so don't need to decode */ resp.status.uptime_sec = current_time / 1000u; resp.status.health = g_controller_state.fault ? resp.status.HEALTH_CRITICAL : resp.status.HEALTH_OK; resp.status.mode = resp.status.MODE_OPERATIONAL; resp.status.sub_mode = 0u; resp.status.vendor_specific_status_code = (uint16_t)g_controller_state.mode; resp.software_version.major = flash_app_descriptor.major_version; resp.software_version.minor = flash_app_descriptor.minor_version; resp.software_version.optional_field_flags = resp.software_version.OPTIONAL_FIELD_FLAG_VCS_COMMIT | resp.software_version.OPTIONAL_FIELD_FLAG_IMAGE_CRC; resp.software_version.vcs_commit = flash_app_descriptor.vcs_commit; resp.software_version.image_crc = flash_app_descriptor.image_crc; resp.hardware_version.major = HW_VERSION_MAJOR; resp.hardware_version.minor = HW_VERSION_MINOR; /* Set the unique ID */ memset(resp.hardware_version.unique_id.begin(), 0u, resp.hardware_version.unique_id.size()); memcpy(resp.hardware_version.unique_id.begin(), (uint8_t*)0x1ffff7ac, 12u); /* Set the hardware name */ resp.name = HW_UAVCAN_NAME; service_manager.encode_response<uavcan::protocol::GetNodeInfo>(resp); } else if (service_filter_id == UAVCAN_PROTOCOL_RESTARTNODE && service_manager.decode(rn_req)) { uavcan::protocol::RestartNode::Response resp; /* Restart if the magic number is correct and the controller is currently stopped, otherwise reject. */ if (g_controller_state.mode == CONTROLLER_STOPPED && rn_req.magic_number == rn_req.MAGIC_NUMBER) { resp.ok = true; wants_bootloader_restart = true; } else { resp.ok = false; } service_manager.encode_response<uavcan::protocol::RestartNode>(resp); } else if (service_filter_id == UAVCAN_PROTOCOL_ENUMERATION_BEGIN && service_manager.decode(enum_req)) { uavcan::protocol::enumeration::Begin::Response resp; /* We only support enumeration for the esc_index property, and only while the controller is stopped. */ if (g_controller_state.mode != CONTROLLER_STOPPED) { resp.error = resp.ERROR_INVALID_MODE; } else if (enum_req.parameter_name != "esc_index") { resp.error = resp.ERROR_INVALID_PARAMETER; } else { /* All fine, start enumeration */ resp.error = resp.ERROR_OK; enumeration_deadline = current_time + 1000 * enum_req.timeout_sec; g_enumeration_active = true; } service_manager.encode_response<uavcan::protocol::enumeration::Begin>(resp); } service_manager.receive_acknowledge(); } /* Transmit service responses if available */ if (can_is_ready(1u) && service_manager.transmit_frame(message_id, length, message)) { can_tx(1u, message_id, length, message); } if (broadcast_manager.is_tx_done() && service_manager.is_tx_done() && !service_manager.is_rx_in_progress(current_time)) { motor_state = const_cast<struct motor_state_t&>(g_motor_state); v_dq_v[0] = g_v_dq_v[0]; v_dq_v[1] = g_v_dq_v[1]; if (foc_status_interval && current_time - foc_status_time >= foc_status_interval) { thiemar::equipment::esc::Status msg; msg.i_dq[0] = motor_state.i_dq_a[0]; msg.i_dq[1] = motor_state.i_dq_a[1]; msg.i_setpoint = g_controller_state.current_setpoint; msg.v_dq[0] = v_dq_v[0]; msg.v_dq[1] = v_dq_v[1]; msg.esc_index = esc_index; broadcast_manager.encode_message( foc_status_transfer_id++, foc_status_dtid, msg); foc_status_time = current_time; } else if (esc_status_interval && current_time - esc_status_time >= esc_status_interval) { uavcan::equipment::esc::Status msg; power_w = __VSQRTF( (motor_state.i_dq_a[0] * motor_state.i_dq_a[0] + motor_state.i_dq_a[1] * motor_state.i_dq_a[1]) * (v_dq_v[0] * v_dq_v[0] + v_dq_v[1] * v_dq_v[1]) ); msg.voltage = g_vbus_v; msg.current = power_w / g_vbus_v; /* If Q current has opposite sign to Q voltage, the flow is reversed due to regenerative braking. */ if (motor_state.i_dq_a[1] * v_dq_v[1] < 0.0f) { msg.current = -msg.current; } msg.temperature = 273.15f + hal_get_temperature_degc(); msg.rpm = int32_t(g_motor_state.angular_velocity_rad_per_s * to_rpm); msg.power_rating_pct = uint8_t(100.0f * power_w / (motor_params.max_current_a * motor_params.max_voltage_v)); msg.esc_index = esc_index; broadcast_manager.encode_message( esc_status_transfer_id++, msg); esc_status_time = current_time; } else if (current_time - node_status_time >= node_status_interval) { uavcan::protocol::NodeStatus msg; msg.uptime_sec = current_time / 1000u; msg.health = g_controller_state.fault ? msg.HEALTH_CRITICAL : msg.HEALTH_OK; msg.mode = msg.MODE_OPERATIONAL; msg.sub_mode = 0u; msg.vendor_specific_status_code = (uint16_t)g_controller_state.mode; broadcast_manager.encode_message( node_status_transfer_id++, msg); node_status_time = current_time; } else if (g_enumeration_active && std::abs(g_enumeration_velocity_rad_per_s) > ENUMERATION_VELOCITY_THRESHOLD_RAD_PER_S) { /* Change rotation direction if the enumerated direction was negative */ if (g_enumeration_velocity_rad_per_s < 0.0f) { value = configuration.get_param_value_by_index( PARAM_CONTROL_DIRECTION); configuration.set_param_value_by_index( PARAM_CONTROL_DIRECTION, value == 0.0f ? 1.0f : 0.0f); } uavcan::protocol::enumeration::Indication msg; msg.parameter_name = "esc_index"; broadcast_manager.encode_message( enumeration_indication_transfer_id++, msg); /* Reset enumeration velocity to avoid sending another message straight away */ g_enumeration_active = false; g_enumeration_velocity_rad_per_s = 0.0f; } } /* Transmit broadcast CAN frames if available */ if (can_is_ready(0u) && broadcast_manager.transmit_frame(message_id, length, message)) { can_tx(0u, message_id, length, message); } /* Update the controller mode and setpoint, unless the motor is currently stopping or enumeration is active. */ if (!g_controller_state.fault && got_setpoint && g_controller_state.mode != CONTROLLER_ABORTING && !g_enumeration_active) { if (mode == CONTROLLER_VOLTAGE) { g_controller_state.voltage_setpoint = setpoint; g_controller_state.mode = CONTROLLER_VOLTAGE; last_setpoint_update = current_time; } else if ((g_controller_state.mode == CONTROLLER_STOPPED && motor_params.idle_speed_rad_per_s > 0.0f) || g_controller_state.mode == CONTROLLER_IDLING) { /* Setpoint was non-zero, but was not high enough to start spinning and we're not already running -- so start idling, or keep on idling. */ g_controller_state.mode = CONTROLLER_IDLING; g_controller_state.voltage_setpoint = 0.0f; last_setpoint_update = current_time; } } else if (g_controller_state.mode != CONTROLLER_STOPPED && g_controller_state.mode != CONTROLLER_STOPPING && g_controller_state.mode != CONTROLLER_ABORTING && current_time - last_setpoint_update > THROTTLE_TIMEOUT_MS) { /* Stop gracefully if the present command mode doesn't provide an updated setpoint in the required period. */ g_controller_state.mode = CONTROLLER_STOPPING; g_controller_state.voltage_setpoint = 0.0f; } /* Update the Kv parameter with the latest estimate of phi -- we don't really need to do this in the main loop, but it can't hurt. */ if (g_phi_v_s_per_rad > 0.0f && g_controller_state.mode == CONTROLLER_VOLTAGE) { configuration.set_param_value_by_index( PARAM_MOTOR_KV, to_rpm / g_phi_v_s_per_rad); } /* Stop enumeration if the deadline has expired */ if (enumeration_deadline < current_time) { g_enumeration_active = false; } /* Only restart into the bootloader if the acknowledgement message has been sent, and we're otherwise unoccupied. */ if (g_controller_state.mode == CONTROLLER_STOPPED && broadcast_manager.is_tx_done() && can_is_ready(0u) && service_manager.is_tx_done() && can_is_ready(1u) && wants_bootloader_restart) { g_controller_state.fault = true; hal_restart(); } } }
/** * @brief Thread principal : Traite les messages reçus pour et depuis le CAN. * * Attend sur le socket CAN et sur la fifo d'envois de message. Dès qu'un message * se présente il le traite. */ static void * can_thread_fct(void* args) { fd_set readfds; struct timeval timeout; int ndfs; struct can_frame msg; args = args; /* Initializing select() */ while(continu) { FD_ZERO(&readfds); FD_SET(socket_can, &readfds); FD_SET(fifofd_rd, &readfds); timeout.tv_sec = 0; timeout.tv_usec = 100000; ndfs = MAX(socket_can, fifofd_rd) + 1; while(select(ndfs, &readfds, NULL, NULL, &timeout) > 0) /* Boucle de 100 ms */ { if(FD_ISSET(socket_can, &readfds)) { /* Données reçues sur le CAN */ int err; /* Lecture des donneés */ if(!(err = recv(socket_can, &msg, sizeof(msg), 0)) == sizeof(msg)) { if(err == -1) perror("Recv from socket can"); else fprintf(stderr, "Incomplete read from socket can"); } /* Traitement du message reçu */ can_rx(msg); } if(FD_ISSET(fifofd_rd, &readfds)) { /* Données reçues dans la FIFO d'envoi */ int err; /* Lecture des données dans la Fifo*/ if ((err=read(fifofd_rd, &msg, sizeof(msg))) == sizeof(msg)) { /* Envoi de la donnée par le socket CAN */ if(!(err = write(socket_can, &msg, sizeof(msg))) == sizeof(msg)) { if(err== -1) perror("Writing on socket can"); else fprintf(stderr, "Incomplete Write on socket can"); } } else { if(err == -1) { if(errno != EAGAIN) /* EAGAIN si la fifo est vide */ perror("Read from fifo"); } else { if(!err == 0) fprintf(stderr, "Incomplete read from fifo"); } } } /* Réinitialisation de select() */ FD_ZERO(&readfds); FD_SET(socket_can, &readfds); FD_SET(fifofd_rd, &readfds); ndfs = MAX(socket_can, fifofd_rd) + 1; } } pthread_exit(NULL); }
void ecu_can_init(void) { /* Setup the generic clock for CAN output */ scif_gc_setup( AVR32_SCIF_GCLK_CANIF, SCIF_GCCTRL_OSC0, AVR32_SCIF_GC_NO_DIV_CLOCK, 0 ); /* Now enable the generic clock input for the CAN module */ scif_gc_enable(AVR32_SCIF_GCLK_CANIF); static const gpio_map_t CAN_GPIO_MAP = { {CAN0_RX_PIN, CAN0_RX_FUNCTION}, {CAN0_TX_PIN, CAN0_TX_FUNCTION}, {CAN1_RX_PIN, CAN1_RX_FUNCTION}, {CAN1_TX_PIN, CAN1_TX_FUNCTION} }; /* Assign GPIO to CAN. */ gpio_enable_module(CAN_GPIO_MAP, sizeof(CAN_GPIO_MAP) / sizeof(CAN_GPIO_MAP[0])); /* Initialize interrupt vectors. */ INTC_init_interrupts(); /* Allocate channel message box */ mob_rx_speed_sens_fl.handle = 0; mob_rx_speed_sens_fr.handle = 1; mob_rx_speed_sens_rl.handle = 2; mob_rx_speed_sens_rr.handle = 3; mob_rx_dash_pri.handle = 4; mob_tx_dash.handle = 5; mob_rx_trq_sens0.handle = 6; mob_rx_trq_sens1.handle = 7; mob_ecu_slow_data.handle = 8; mob_rx_bms_precharge.handle = 9; mob_brk.handle = 10; mob_ecu_fast_data.handle = 11; mob_rx_bms_battvolt.handle = 12; mob_rx_bspd.handle = 13; mob_rx_dash_data.handle = 14; mob_slip_current.handle = 15; /* Initialize CAN channels */ can_init(CAN_BUS_0, ((uint32_t)&mob_ram_ch0[0]), CANIF_CHANNEL_MODE_NORMAL, can_out_callback_channel0); can_init(CAN_BUS_1, ((uint32_t)&mob_ram_ch1[0]), CANIF_CHANNEL_MODE_NORMAL, can_out_callback_channel1); /* Prepare for message reception */ can_rx( CAN_BUS_1 , mob_rx_speed_sens_fl.handle , mob_rx_speed_sens_fl.req_type , mob_rx_speed_sens_fl.can_msg ); can_rx( CAN_BUS_1 , mob_rx_speed_sens_fr.handle , mob_rx_speed_sens_fr.req_type , mob_rx_speed_sens_fr.can_msg ); can_rx( CAN_BUS_1 , mob_rx_speed_sens_rl.handle , mob_rx_speed_sens_rl.req_type , mob_rx_speed_sens_rl.can_msg ); can_rx( CAN_BUS_1 , mob_rx_speed_sens_rr.handle , mob_rx_speed_sens_rr.req_type , mob_rx_speed_sens_rr.can_msg ); can_rx( CAN_BUS_0 , mob_rx_dash_pri.handle , mob_rx_dash_pri.req_type , mob_rx_dash_pri.can_msg ); can_rx( CAN_BUS_0 , mob_rx_dash_data.handle , mob_rx_dash_data.req_type , mob_rx_dash_data.can_msg ); can_rx( CAN_BUS_0 , mob_rx_trq_sens0.handle , mob_rx_trq_sens0.req_type , mob_rx_trq_sens0.can_msg ); can_rx( CAN_BUS_1 , mob_rx_trq_sens1.handle , mob_rx_trq_sens1.req_type , mob_rx_trq_sens1.can_msg ); can_rx( CAN_BUS_1 , mob_rx_bms_precharge.handle , mob_rx_bms_precharge.req_type , mob_rx_bms_precharge.can_msg ); can_rx( CAN_BUS_1 , mob_rx_bms_battvolt.handle , mob_rx_bms_battvolt.req_type , mob_rx_bms_battvolt.can_msg ); can_rx( CAN_BUS_1 , mob_brk.handle , mob_brk.req_type , mob_brk.can_msg ); can_rx( CAN_BUS_0 , mob_rx_bspd.handle , mob_rx_bspd.req_type , mob_rx_bspd.can_msg ); asm("nop"); }
/* Call Back called by can_drv, channel 1 */ void can_out_callback_channel1(U8 handle, U8 event){ if (handle == mob_rx_speed_sens_fl.handle) { mob_rx_speed_sens_fl.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_speed_sens_fl.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_speed_sens_fl.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_speed_sens_fl.status = event; xQueueOverwriteFromISR(queue_wheel_fl, &mob_rx_speed_sens_fl.can_msg->data.u16[0], NULL); /* Empty message field */ mob_rx_speed_sens_fl.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_speed_sens_fl.handle, mob_rx_speed_sens_fl.req_type, mob_rx_speed_sens_fl.can_msg); } else if (handle == mob_rx_speed_sens_fr.handle) { mob_rx_speed_sens_fr.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_speed_sens_fr.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_speed_sens_fr.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_speed_sens_fr.status = event; xQueueOverwriteFromISR(queue_wheel_fr, &mob_rx_speed_sens_fr.can_msg->data.u16[0], NULL); /* Empty message field */ mob_rx_speed_sens_fr.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_speed_sens_fr.handle, mob_rx_speed_sens_fr.req_type, mob_rx_speed_sens_fr.can_msg); } else if (handle == mob_rx_speed_sens_rl.handle) { mob_rx_speed_sens_rl.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_speed_sens_rl.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_speed_sens_rl.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_speed_sens_rl.status = event; xQueueOverwriteFromISR(queue_wheel_rl, &mob_rx_speed_sens_rl.can_msg->data.u16[0], NULL); /* Empty message field */ mob_rx_speed_sens_rl.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_speed_sens_rl.handle, mob_rx_speed_sens_rl.req_type, mob_rx_speed_sens_rl.can_msg); } else if (handle == mob_rx_speed_sens_rr.handle) { mob_rx_speed_sens_rr.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_speed_sens_rr.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_speed_sens_rr.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_speed_sens_rr.status = event; xQueueOverwriteFromISR(queue_wheel_rr, &mob_rx_speed_sens_rr.can_msg->data.u16[0], NULL); /* Empty message field */ mob_rx_speed_sens_rr.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_speed_sens_rr.handle, mob_rx_speed_sens_rr.req_type, mob_rx_speed_sens_rr.can_msg); } else if (handle == mob_rx_trq_sens1.handle) { mob_rx_trq_sens1.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_trq_sens1.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_trq_sens1.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_trq_sens1.status = event; xQueueOverwriteFromISR(queue_trq_sens1, &mob_rx_trq_sens1.can_msg->data.s16[0], NULL); xQueueOverwriteFromISR(queue_trq_sens1_err, &mob_rx_trq_sens1.can_msg->data.u8[2], NULL); asm("nop"); /* Empty message field */ mob_rx_trq_sens1.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_trq_sens1.handle, mob_rx_trq_sens1.req_type, mob_rx_trq_sens1.can_msg); } else if (handle == mob_rx_bms_precharge.handle) { mob_rx_bms_precharge.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_bms_precharge.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_bms_precharge.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_bms_precharge.status = event; bms_can_msg_t bms_can_msg; bms_can_msg.data.u64 = mob_rx_bms_precharge.can_msg->data.u64; bms_can_msg.id = mob_rx_bms_precharge.can_msg->id; xQueueSendToBackFromISR(queue_bms_rx, &bms_can_msg, NULL); /* Empty message field */ mob_rx_bms_precharge.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_bms_precharge.handle, mob_rx_bms_precharge.req_type, mob_rx_bms_precharge.can_msg); } else if (handle == mob_rx_bms_battvolt.handle) { mob_rx_bms_battvolt.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_rx_bms_battvolt.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_rx_bms_battvolt.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_rx_bms_battvolt.status = event; bms_can_msg_t bms_can_msg; bms_can_msg.data.u64 = mob_rx_bms_battvolt.can_msg->data.u64; bms_can_msg.id = mob_rx_bms_battvolt.can_msg->id; xQueueSendToBackFromISR(queue_bms_rx, &bms_can_msg, NULL); /* Empty message field */ mob_rx_bms_battvolt.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_rx_bms_battvolt.handle, mob_rx_bms_battvolt.req_type, mob_rx_bms_battvolt.can_msg); } else if (handle == mob_brk.handle) { mob_brk.can_msg->data.u64 = can_get_mob_data(CAN_BUS_1, handle).u64; mob_brk.can_msg->id = can_get_mob_id(CAN_BUS_1, handle); mob_brk.dlc = can_get_mob_dlc(CAN_BUS_1, handle); mob_brk.status = event; if (mob_brk.can_msg->id == (CANR_FCN_DATA_ID | CANR_GRP_SENS_BRK_ID | CANR_MODULE_ID0_ID)) { xQueueSendToBackFromISR( queue_brake_front, &mob_brk.can_msg->data.u16[0], NULL ); } else { xQueueSendToBackFromISR( queue_brake_rear, &mob_brk.can_msg->data.u16[0], NULL ); } /* Empty message field */ mob_brk.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_1, mob_brk.handle, mob_brk.req_type, mob_brk.can_msg); } }
void can_out_callback_channel0(U8 handle, U8 event){ if (handle == mob_rx_dash_pri.handle) { mob_rx_dash_pri.can_msg->data.u64 = can_get_mob_data(CAN_BUS_0, handle).u64; mob_rx_dash_pri.can_msg->id = can_get_mob_id(CAN_BUS_0, handle); mob_rx_dash_pri.dlc = can_get_mob_dlc(CAN_BUS_0, handle); mob_rx_dash_pri.status = event; dash_can_msg_t dash_can_msg; dash_can_msg.data.u64 = mob_rx_dash_pri.can_msg->data.u64; dash_can_msg.id = mob_rx_dash_pri.can_msg->id; xQueueSendToBackFromISR(queue_dash_msg, &dash_can_msg, NULL); /* Empty message field */ mob_rx_dash_pri.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_0, mob_rx_dash_pri.handle, mob_rx_dash_pri.req_type, mob_rx_dash_pri.can_msg); } else if (handle == mob_rx_dash_data.handle) { mob_rx_dash_data.can_msg->data.u64 = can_get_mob_data(CAN_BUS_0, handle).u64; mob_rx_dash_data.can_msg->id = can_get_mob_id(CAN_BUS_0, handle); mob_rx_dash_data.dlc = can_get_mob_dlc(CAN_BUS_0, handle); mob_rx_dash_data.status = event; dash_can_msg_t dash_can_msg; dash_can_msg.data.u64 = mob_rx_dash_data.can_msg->data.u64; dash_can_msg.id = mob_rx_dash_data.can_msg->id; xQueueSendToBackFromISR(queue_dash_msg, &dash_can_msg, NULL); /* Empty message field */ mob_rx_dash_data.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_0, mob_rx_dash_data.handle, mob_rx_dash_data.req_type, mob_rx_dash_data.can_msg); } else if (handle == mob_rx_trq_sens0.handle) { mob_rx_trq_sens0.can_msg->data.u64 = can_get_mob_data(CAN_BUS_0, handle).u64; mob_rx_trq_sens0.can_msg->id = can_get_mob_id(CAN_BUS_0, handle); mob_rx_trq_sens0.dlc = can_get_mob_dlc(CAN_BUS_0, handle); mob_rx_trq_sens0.status = event; xQueueOverwriteFromISR(queue_trq_sens0, &mob_rx_trq_sens0.can_msg->data.s16[0], NULL); xQueueOverwriteFromISR(queue_trq_sens0_err, &mob_rx_trq_sens0.can_msg->data.u8[2], NULL); asm("nop"); /* Empty message field */ mob_rx_trq_sens0.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_0, mob_rx_trq_sens0.handle, mob_rx_trq_sens0.req_type, mob_rx_trq_sens0.can_msg); } else if (handle == mob_rx_bspd.handle) { mob_rx_bspd.can_msg->data.u64 = can_get_mob_data(CAN_BUS_0, handle).u64; mob_rx_bspd.can_msg->id = can_get_mob_id(CAN_BUS_0, handle); mob_rx_bspd.dlc = can_get_mob_dlc(CAN_BUS_0, handle); mob_rx_bspd.status = event; xQueueOverwriteFromISR( queue_bspd, &mob_rx_bspd.can_msg->data.u8[0], NULL ); /* Empty message field */ mob_rx_bspd.can_msg->data.u64 = 0x0LL; /* Prepare message reception */ can_rx(CAN_BUS_0, mob_rx_bspd.handle, mob_rx_bspd.req_type, mob_rx_bspd.can_msg); } }