/*! \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);
}
}
