/*
* We received a message, check if a generic action can be done or else forward
* it to the right implementation
*/
void OnMsgVUE32(NETV_MESSAGE *msg)
{
if ( msg->msg_remote && msg->msg_type == VUE32_TYPE_GETVALUE )
{
// If it's a request, let's see if we can answer it
switch( msg->msg_cmd )
{
case E_ID_VERSION:
msg->msg_dest = msg->msg_source;
msg->msg_remote = 0;
msg->msg_data_length = 2;
msg->msg_source = GetMyAddr();
msg->msg_data[0] = (unsigned char)(GetFirmVersion() >> 8);
msg->msg_data[1] = (unsigned char)(GetFirmVersion() & 0x00FF);
netv_send_message(msg);
return;
break;
case E_ID_ADDRESS:
msg->msg_dest = msg->msg_source;
msg->msg_remote = 0;
msg->msg_data_length = 1;
msg->msg_source = GetMyAddr();
msg->msg_data[0] = GetMyAddr();
netv_send_message(msg);
return;
break;
default:
break;
}
}
//Start Emetting (Long polling)
ON_MSG_TYPE( VUE32_TYPE_STARTEMETTING)
ActionStartEmettings(msg);
END_OF_MSG_TYPE
ON_MSG_TYPE( VUE32_TYPE_STOPEMETTING)
DesactivateLongPolling(msg->msg_cmd);
END_OF_MSG_TYPE
#ifndef __32MX575F512H__
// Call the ID specific message parser
gOnMsgFunc[GetBoardID()](msg);
//Call Emergency Instruction Here
ON_MSG_TYPE( NETV_TYPE_EMERGENCY )
gOnEmergencyMsgVUE32[GetBoardID()]();
END_OF_MSG_TYPE
#else
// Call the BMS message parser
OnMsgBMS(msg);
#endif
}
void RunLongPolling(void)
{
//For each subcribed sensor to long polling event
unsigned int i;
for(i = 0; i<g_unLpSize; i++)
{
//Check if it's time to send the sensor's value
if(g_sLpParams[i].unEndWait <= uiTimeStamp)
{
NETV_MESSAGE msg = {0};
msg.msg_priority = NETV_PRIORITY_EVENTS;
msg.msg_type = NETV_TYPE_EVENT;
msg.msg_cmd = g_sLpParams[i].ucResourceId;
msg.msg_source = GetMyAddr();
msg.msg_dest = 0x00;
msg.msg_comm_iface = 0xFF;
//TODO Link with the application who's interfaced with the hardware layer
//msg.msg_data_length = g_sLpParams[i]->NbrByte;
//memcpy(&msg.msg_data, TODO, g_sLpParams[i]->NbrByte);
//Determine the next time which the board will emit value
g_sLpParams[i].unEndWait = uiTimeStamp+g_sLpParams[i].unDelay;
netv_send_message(&msg);
}
//Check for the sensor's life time
if(g_sLpParams[i].unEndPolling <= uiTimeStamp)
{
//Desactivating the long polling
DesactivateLongPolling(g_sLpParams[i].ucResourceId);
}
}
}
//////////////////////////////////////////////////////////////////////
// netv_send_im_alive
//////////////////////////////////////////////////////////////////////
//
// Description: Send a I'M Alive can message
//
// Input: netv_addr (Address read in the EEPROM at 0XFE)
// Output: NONE
// Input/Output: NONE
// Returned value: NONE
//
//////////////////////////////////////////////////////////////////////
void netv_send_im_alive(unsigned char netv_addr)
{
NETV_MESSAGE msg;
int i = 0;
msg.msg_priority = 0x00;
msg.msg_type = NETV_TYPE_EVENTS;
msg.msg_cmd = NETV_EVENTS_CMD_ALIVE;
msg.msg_dest = netv_addr;
msg.msg_eeprom_ram = NETV_REQUEST_EEPROM;
msg.msg_read_write = NETV_REQUEST_READ;
msg.msg_data_length = 8;
//Protocol version 2
for (i = 0; i < 8; i++)
{
msg.msg_data[i] = netv_read_eeprom(i);
}
//make sure we are returning the right netv_addr
msg.msg_data[2] = netv_addr;
msg.msg_remote = 0;
netv_send_message(&msg);
}
void netv_send_im_alive(unsigned char netv_addr) {
NETV_MESSAGE msg;
int i = 0;
msg.msg_priority = 0x00;
msg.msg_type = NETV_TYPE_EVENTS;
msg.msg_cmd = NETV_EVENTS_CMD_ALIVE;
msg.msg_dest = netv_addr;
msg.msg_eeprom_ram = NETV_REQUEST_EEPROM;
msg.msg_read_write = NETV_REQUEST_READ;
msg.msg_data_length = 8;
//Copy boot config
memcpy(msg.msg_data, (char*) netv_get_boot_config(), 8);
//make sure we are returning the right netv_addr
msg.msg_data[2] = netv_addr;
msg.msg_remote = 0;
netv_send_message(&msg);
}
/*
* State Machine Processing
*/
void ImplVUE32_3(void)
{
int i = 0;
int sum = 0;
if(flag_1ms_b)
{
flag_1ms_b = 0;
asm volatile ("di"); //Disable int
filter_wheel();
asm volatile ("ei"); //Enable int
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FR] = wheel_period_to_kph(spdo1_mean, spd1_moving);
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FL] = wheel_period_to_kph(spdo2_mean, spd2_moving);
}
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && gResourceMemory[E_ID_DPR] == PARK)
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveDisable(gDrivesVUE32_3, LeftDrive);
DriveDisable(gDrivesVUE32_3, RightDrive);
fDirectionMode = 0.0;
}
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && (gResourceMemory[E_ID_DPR] == REVERSE || gResourceMemory[E_ID_DPR] == DRIVE))
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveEnable(gDrivesVUE32_3, LeftDrive);
DriveEnable(gDrivesVUE32_3, RightDrive);
if(gResourceMemory[E_ID_DPR] == REVERSE)
fDirectionMode = -1.0;
else if(gResourceMemory[E_ID_DPR] == DRIVE)
fDirectionMode = 1.0;
}
EVERY_X_MS(20)
DriveStateMachine(gDrivesVUE32_3, LeftDrive, (float)gResourceMemory[E_ID_ACCELERATOR]*0.13*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
DriveStateMachine(gDrivesVUE32_3, RightDrive, (float)gResourceMemory[E_ID_ACCELERATOR]*0.13*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
END_OF_EVERY
EVERY_X_MS(250)
ReturnDriveInformation(gDrivesVUE32_3, LeftDrive, &gResourceMemory[E_ID_LEFT_MOTOR_SPEED], &gResourceMemory[E_ID_LEFT_MOTOR_CURRENT], &gResourceMemory[E_ID_LEFT_MOTOR_TEMP], &gResourceMemory[E_ID_LEFT_CONTROLLER_TEMP], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_LEFT_DRIVE_STATUS]);
ReturnDriveInformation(gDrivesVUE32_3, RightDrive, &gResourceMemory[E_ID_RIGHT_MOTOR_SPEED], &gResourceMemory[E_ID_RIGHT_MOTOR_CURRENT], &gResourceMemory[E_ID_RIGHT_MOTOR_TEMP], &gResourceMemory[E_ID_RIGHT_CONTROLLER_TEMP], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_RIGHT_DRIVE_STATUS]);
//Compute mean between both speed motor
// TODO : Fix sign error
gResourceMemory[E_ID_GLOBAL_CAR_SPEED] = (gDrivesVUE32_3[RightDrive].nMotorSpeed-gDrivesVUE32_3[LeftDrive].nMotorSpeed)/2;
SetResourceValue(E_ID_GLOBAL_CAR_SPEED, VUE32_1, sizeof(unsigned short), gResourceMemory[E_ID_GLOBAL_CAR_SPEED]);
END_OF_EVERY
EVERY_X_MS(50)
NETV_MESSAGE msg = {{0}};
msg.msg_priority = NETV_PRIORITY_HIGHEST;
msg.msg_type = NETV_TYPE_SYNCHRONIZE;
msg.msg_cmd = E_ID_VERSION;
msg.msg_source = GetMyAddr(); //GetMyAddr();
msg.msg_dest = 0x05;
msg.msg_comm_iface = NETV_COMM_IFACE_ALL;
msg.msg_data_length = 0;
msg.msg_remote = 1;
netv_send_message(&msg);
END_OF_EVERY
EVERY_X_MS(50)
// Drive mode
//if (fDirectionMode == 1)
//{
userCommand = (float)gResourceMemory[E_ID_ACCELERATOR]*0.13;
carState.w3 = (((float)abs(gResourceMemory[E_ID_RIGHT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
carState.w4 = (((float)abs(gResourceMemory[E_ID_LEFT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
FloatToInt conv;
conv.val = carState.w3;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED1] = conv.raw;
conv.val = carState.w4;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED2] = conv.raw;
command = comp(carState, userCommand, gainCorrection, threshold);
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_1] = command.tmWh3;
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_2] = command.tmWh4;
//***********Uncomment the two following lines to activate the torque vectoring********
//DriveStateMachine(gDrivesVUE32_3, LeftDrive, command.tmWh3*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
//DriveStateMachine(gDrivesVUE32_3, RightDrive, command.tmWh4*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
//*************************************************************
/*}
// Reverse mode
else if (fDirectionMode == -1)
{
command.tmWh3 = userCommand;
command.tmWh4 = userCommand;
}
// Park mode
else if (fDirectionMode == 0)
{
command.tmWh3 = 0;
command.tmWh4 = 0;
}*/
END_OF_EVERY
}
void netv_transceiver(unsigned char netv_addr) {
NETV_MESSAGE g_rMessage;
// unsigned char buffer_size = 0;
unsigned int offset = 0;
while (netv_recv_message(&g_rMessage)) {
if (g_rMessage.msg_dest == netv_addr || g_rMessage.msg_dest == 0xFF) {
//Analyse for boot mode and I'm alive signal
switch (g_rMessage.msg_type) {
case NETV_TYPE_EMERGENCY:
switch (g_rMessage.msg_cmd) {
case NETV_EMERGENCY_CMD_PROGRAM:
//indicate to the bootloader that we are gonna program
//TODO Update Boot Config
//netv_write_eeprom(0xFF,NETV_BOOT_MODE_ID);
//TODO FIX THIS
//software reset, from reset documentation!
//mSYSTEMUnlock();
//Set arm reset
//RSWRSTSET = 1;
//Read register to trigger reset
//volatile int *p = &RSWRST;
//Wait until the reset happens
//while(1);
// Reset();
break;
}//end switch msg_cmd
break;
case NETV_TYPE_EVENTS:
switch (g_rMessage.msg_cmd) {
case NETV_EVENTS_CMD_ALIVE:
//Send i'm alive
netv_send_im_alive(netv_addr);
break;
}//end switch msg_cmd
break;
case NETV_TYPE_REQUEST_DATA:
//get the memory offset
offset = g_rMessage.msg_cmd;
switch (g_rMessage.msg_read_write) {
case NETV_REQUEST_READ:
//WE MUST RECEIVE A REMOTE REQUEST...
if (g_rMessage.msg_remote == 1) {
netv_read_data_flow_table_v2(offset,
g_rMessage.msg_eeprom_ram,
&g_rMessage.msg_data[0],
MIN(g_rMessage.msg_data_length, 8));
g_rMessage.msg_remote = 0;
while (netv_send_message(&g_rMessage)) {
;
}
}
break;
case NETV_REQUEST_WRITE:
// We don't want to overwrite EEPROM table information
// The message must not be a remote request
if (g_rMessage.msg_remote == 0) {
netv_write_data_flow_table_v2(offset,
g_rMessage.msg_eeprom_ram,
&g_rMessage.msg_data[0],
MIN(g_rMessage.msg_data_length, 8));
}
break;
}//end sub-switch read_write
break;
}
} //if dest
// processe the received message
netv_proc_message(&g_rMessage);
}//while
}
/*
* State Machine Processing
*/
void ImplVUE32_3(void)
{
if(flag_1ms_b)
{
flag_1ms_b = 0;
asm volatile ("di"); //Disable int
filter_wheel();
asm volatile ("ei"); //Enable int
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FR] = wheel_period_to_kph(spdo1_mean, spd1_moving);
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FL] = wheel_period_to_kph(spdo2_mean, spd2_moving);
}
/* Manage motor command sign and drive enabler acconding to DPR switch state*/
//Car state is on Park, then drives have to be disabled
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && gResourceMemory[E_ID_DPR] == PARK)
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveDisable(gDrivesVUE32_3, LeftDrive);
DriveDisable(gDrivesVUE32_3, RightDrive);
fDirectionMode = 0.0;
}
//Car state is on Drive or Reverse, then the drives have to be enabled
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && (gResourceMemory[E_ID_DPR] == REVERSE || gResourceMemory[E_ID_DPR] == DRIVE))
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveEnable(gDrivesVUE32_3, LeftDrive);
DriveEnable(gDrivesVUE32_3, RightDrive);
//Negative sign
if(gResourceMemory[E_ID_DPR] == REVERSE)
fDirectionMode = -1.0;
//Positive sign
else if(gResourceMemory[E_ID_DPR] == DRIVE)
fDirectionMode = 1.0;
}
EVERY_X_MS(250)
//Copy drives informations to the VUE32 application layer
ReturnDriveInformation(gDrivesVUE32_3, LeftDrive, &gResourceMemory[E_ID_LEFT_MOTOR_SPEED], &gResourceMemory[E_ID_LEFT_MOTOR_CURRENT], &gResourceMemory[E_ID_LEFT_MOTOR_TEMP], &gResourceMemory[E_ID_LEFT_CONTROLLER_TEMP], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_LEFT_DRIVE_STATUS]);
ReturnDriveInformation(gDrivesVUE32_3, RightDrive, &gResourceMemory[E_ID_RIGHT_MOTOR_SPEED], &gResourceMemory[E_ID_RIGHT_MOTOR_CURRENT], &gResourceMemory[E_ID_RIGHT_MOTOR_TEMP], &gResourceMemory[E_ID_RIGHT_CONTROLLER_TEMP], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_RIGHT_DRIVE_STATUS]);
//TODO Add the transfert function for to get the exact speed car in Kph
gResourceMemory[E_ID_GLOBAL_CAR_SPEED] = (gDrivesVUE32_3[RightDrive].nMotorSpeed+gDrivesVUE32_3[LeftDrive].nMotorSpeed)/2;
SetResourceValue(E_ID_GLOBAL_CAR_SPEED, VUE32_1, sizeof(unsigned short), gResourceMemory[E_ID_GLOBAL_CAR_SPEED]);
END_OF_EVERY
// Sending synchronization messages for simplified compensation
EVERY_X_MS(50)
NETV_MESSAGE msg = {{0}};
msg.msg_priority = NETV_PRIORITY_HIGHEST;
msg.msg_type = NETV_TYPE_SYNCHRONIZE;
msg.msg_cmd = E_ID_VERSION;
msg.msg_source = GetMyAddr(); //GetMyAddr();
msg.msg_dest = 0x05;
msg.msg_comm_iface = NETV_COMM_IFACE_ALL;
msg.msg_data_length = 0;
msg.msg_remote = 1;
netv_send_message(&msg);
END_OF_EVERY
// Calculating compensated torque commands and sending them to the drives
EVERY_X_MS(50)
//Getting accelerator pedal command
userCommand = (float)gResourceMemory[E_ID_ACCELERATOR]*0.31;
//Getting rear wheel speeds (rpm to km/h)
carState.w3 = (((float)abs(gResourceMemory[E_ID_RIGHT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
carState.w4 = (((float)abs(gResourceMemory[E_ID_LEFT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
//Sending rear wheels speed to the user interface
FloatToInt conv;
conv.val = carState.w3;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED1] = conv.raw;
conv.val = carState.w4;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED2] = conv.raw;
//Calculating the compensated values
command = comp(carState, userCommand, gainCorrection, threshold);
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_1] = command.tmWh3;
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_2] = command.tmWh4;
// Sending the calculated commands to the drives
DriveStateMachine(gDrivesVUE32_3, LeftDrive, command.tmWh3*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
DriveStateMachine(gDrivesVUE32_3, RightDrive, command.tmWh4*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
END_OF_EVERY
}
//////////////////////////////////////////////////////////////////////
// netv_transceiver()
//////////////////////////////////////////////////////////////////////
//
// Description: MCP2510 unsigned charerrupt sub-routine
// Handles all MCP2510 unsigned charerruptions until none are left
//
// Input: NONE
// Output: NONE
// Input/Output: NONE
// Returned value: NONE
//
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void netv_transceiver(unsigned char netv_addr)
{
NETV_MESSAGE g_rMessage;
// unsigned char buffer_size = 0;
unsigned int offset = 0;
while(netv_recv_message(&g_rMessage)) {
//Analyse for boot mode and I'm alive signal
switch(g_rMessage.msg_type){
case NETV_TYPE_EMERGENCY:
switch(g_rMessage.msg_cmd){
case NETV_EMERGENCY_CMD_PROGRAM:
//indicate to the bootloader that we are gonna program
//TODO Update Boot Config
//netv_write_eeprom(0xFF,NETV_BOOT_MODE_ID);
//wait 10ms
//FCY must be defined
__delay_ms(10);
//reset!
asm("RESET"); //Reset();
break;
}//end switch msg_cmd
break;
case NETV_TYPE_EVENTS:
switch(g_rMessage.msg_cmd){
case NETV_EVENTS_CMD_ALIVE:
//Send i'm alive
netv_send_im_alive(netv_addr);
break;
}//end switch msg_cmd
break;
case NETV_TYPE_REQUEST_DATA:
//get the memory offset
offset = g_rMessage.msg_cmd;
switch(g_rMessage.msg_read_write){
case NETV_REQUEST_READ:
//WE MUST RECEIVE A REMOTE REQUEST...
if (g_rMessage.msg_remote == 1)
{
netv_read_data_flow_table_v2(offset,
g_rMessage.msg_eeprom_ram,
&g_rMessage.msg_data[0],
MIN(g_rMessage.msg_data_length,8));
g_rMessage.msg_remote = 0;
while(netv_send_message(&g_rMessage)){;}
}
break;
case NETV_REQUEST_WRITE:
// We don't want to overwrite EEPROM table information
// The message must not be a remote request
if(g_rMessage.msg_remote == 0)
{
netv_write_data_flow_table_v2(offset,
g_rMessage.msg_eeprom_ram,
&g_rMessage.msg_data[0],
MIN(g_rMessage.msg_data_length,8));
}
break;
}//end sub-switch read_write
break;
}
// processe the received message
netv_proc_message(&g_rMessage);
}
}
