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