openinverter forum

Code: Select all

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Dilbert's code here
//////////////////////////////////////////////////////////////////////////////////////////////////////


uint8_t CalcMTHChecksum(void){

	uint16_t mth_checksum=0;

	for(int i=0;i<98;i++)mth_checksum+=mth_data[i];


  if(mth_checksum==(mth_data[98]|(mth_data[99]<<8))) return 1;
  else return 0;

}

void CalcHTMChecksum(void){

uint16_t htm_checksum=0;

  for(int i=0;i<78;i++)htm_checksum+=htm_data[i];
  htm_data[78]=htm_checksum&0xFF;
  htm_data[79]=htm_checksum>>8;

}



void GS450H::UpdateHTMState1Ms(int8_t gear, int16_t torque)
{


dma_read(mth_data,100);//read in mth data via dma. Probably need some kind of check dma complete flag here

switch(htm_state){

case 0:{
rx_buffer_count=0;
 DigIo::req_out.Clear(); //HAL_GPIO_WritePin(HTM_SYNC_GPIO_Port, HTM_SYNC_Pin, 0);
htm_state++;
}break;

case 1:{
 DigIo::req_out.Set();  //HAL_GPIO_WritePin(HTM_SYNC_GPIO_Port, HTM_SYNC_Pin, 1);

if(inv_status==0){
dma_write(htm_data,80); //HAL_UART_Transmit_IT(&huart2, htm_data, 80);
}
else {
dma_write(htm_data_setup,80);   //HAL_UART_Transmit_IT(&huart2, htm_data_setup, 80);
if(mth_data[1]!=0)
inv_status--;
}
htm_state++;
break;

case 2:
htm_state++;
}break;

case 3:{
if(CalcMTHChecksum()==0 || rx_buffer_count!=100){
//HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, 1 );
}
else{
//HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, 0 );
//exchange data and prepare next HTM frame
dc_bus_voltage=(((mth_data[82]|mth_data[83]<<8)-5)/2);
temp_inv_water=(mth_data[42]|mth_data[43]<<8);
temp_inv_inductor=(mth_data[86]|mth_data[87]<<8);
mg1_speed=mth_data[6]|mth_data[7]<<8;
mg2_speed=mth_data[31]|mth_data[32]<<8;
}

mth_data[98]=0;
mth_data[99]=0;

htm_state++;
}break;

case 4:{

    // -3500 (reverse) to 3500 (forward)
    if(gear==0) mg2_torque=0;//Neutral
    if(gear==32) mg2_torque=torque;//Drive
    if(gear==-32) mg2_torque=torque*-1;//Reverse

  mg1_torque=((mg2_torque*5)/4);
  if(gear=-32) mg1_torque=0; //no mg1 torque in reverse.
  Param::SetInt(Param::torque,mg2_torque);//post processed final torue value sent to inv to web interface

	//speed feedback
	speedSum=mg2_speed+mg1_speed;
	speedSum/=113;
    uint8_t speedSum2=speedSum;
    htm_data[0]=speedSum2;
	htm_data[75]=(mg1_torque*4)&0xFF;
	htm_data[76]=((mg1_torque*4)>>8);

	//mg1
	htm_data[5]=(mg1_torque*-1)&0xFF;  //negative is forward
	htm_data[6]=((mg1_torque*-1)>>8);
	htm_data[11]=htm_data[5];
	htm_data[12]=htm_data[6];

	//mg2
	htm_data[26]=(mg2_torque)&0xFF; //positive is forward
	htm_data[27]=((mg2_torque)>>8);
	htm_data[32]=htm_data[26];
	htm_data[33]=htm_data[27];

	//checksum
	htm_checksum=0;
	for(int i=0;i<78;i++)htm_checksum+=htm_data[i];
	htm_data[78]=htm_checksum&0xFF;
	htm_data[79]=htm_checksum>>8;

if(counter>100){
//HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin );
counter = 0;
}
else{
counter++;
}

htm_state=0;
}break;

case 5:{

}break;


}
}
//////////////////////////////////////////////////////////////