Subversion Repositories HomeAutomation

Rev

Rev 2270 | Rev 2273 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1.  
  2. #include "act_PIDv1.h"
  3. #include <drivers/misc/PID_v1.h>
  4. #include <drivers/misc/PID_AutoTune.h>
  5.  
  6. #if act_PIDv1_USEEEPROM==1
  7. #include "act_PIDv1_eeprom.h"
  8. struct eeprom_act_PIDv1 EEMEM eeprom_act_PIDv1 =
  9. {
  10.     {
  11.         ///TODO: Define initialization values on the EEPROM variables here, this will generate a *.eep file that can be used to store this values to the node, can in future be done with a EEPROM module and the make-scrips. Write the values in the exact same order as the struct is defined in the *.h file.
  12.         0x00000000, // referenceValue
  13.         CAN_MODULE_TYPE_SNS_DS18X20,    //SensorModuleType
  14.         0x00,   //SensorModuleId
  15.         0x00,   //SensorId
  16.         0x00000000, //uint32_t K_P; (float)
  17.         0x00000000, //uint32_t K_I; (float)
  18.         0x00000000, //uint32_t K_D; (float)
  19.         0x00,   //uint8_t TimeMsOrS;
  20.         0x0000, //uint16_t Time;
  21.         CAN_MODULE_TYPE_ACT_SOFTPWM,    //ActuatorModuleType
  22.         0x00,   //ActuatorModuleId
  23.         0x00,   //ActuatorId
  24.     },
  25.     0   // crc, must be a correct value, but this will also be handled by the EEPROM module or make scripts
  26. };
  27. #endif
  28. #define PID_ON 1
  29. #define PID_OFF 0
  30. #define PID_AUTO 2
  31.  
  32. //! Parameters for regulator
  33. PidType pid;
  34.  
  35. //struct PID_DATA pidData;
  36. //struct PIDv1_DEBUG_DATA pidDebugData;
  37.  
  38. uint8_t sensorModuleType, sensorModuleId,sensorId;
  39. uint8_t PID_Status;
  40. uint8_t sendDebug_flag = 0;
  41. uint16_t pwmValue=0;
  42. float referenceValue, measurementValue, outputValue;
  43.  
  44. void sendPID(void)
  45. {
  46.     if (eeprom_read_byte(EEDATA.actuatorModuleType) != 0) {
  47.         StdCan_Msg_t txMsg;
  48.         StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  49.         StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_TO_OWNER);
  50.         txMsg.Header.ModuleType = eeprom_read_byte(EEDATA.actuatorModuleType); ///TODO: Change this to the actual module type
  51.         txMsg.Header.ModuleId = eeprom_read_byte(EEDATA.actuatorModuleId);
  52.         txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PWM;
  53.         txMsg.Length = 3;
  54.         txMsg.Data[0] = eeprom_read_byte(EEDATA.actuatorId);
  55.         //uint16_t tempPWM =(uint16_t) (pwmValue*10000);
  56.         txMsg.Data[1] = ( ((uint16_t)outputValue)>>8)&0xff;
  57.         txMsg.Data[2] = ( ((uint16_t)outputValue))&0xff;
  58.         while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  59.     }
  60.     StdCan_Msg_t txMsg;
  61.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  62.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  63.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
  64.     txMsg.Header.ModuleId = act_PIDv1_ID;
  65.     txMsg.Header.Command = CAN_MODULE_CMD_PID_PID_STATUS;
  66.     txMsg.Length = 8;
  67.     txMsg.Data[0] = (uint8_t)0x00ff & (((uint32_t)(measurementValue*64))>>8);
  68.     txMsg.Data[1] = (uint8_t)0x00ff & ((uint32_t)(measurementValue*64));
  69.     txMsg.Data[2] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
  70.     txMsg.Data[3] = (uint8_t)0x00ff & ((uint32_t)(referenceValue*64));
  71.     //uint16_t tempPWM =(uint16_t) (pwmValue*10000);
  72.         txMsg.Data[4] = (uint8_t)( ((uint16_t)outputValue)>>8)&0xff;
  73.         txMsg.Data[5] = (uint8_t)( ((uint16_t)outputValue))&0xff;
  74.     txMsg.Data[6] = ((int16_t) (PID_GetITerm(&pid))>>8)&0xff;
  75.     txMsg.Data[7] = ((int16_t) (PID_GetITerm(&pid)))&0xff;
  76.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  77. }
  78.  
  79. #ifdef act_PIDv1_SEND_DEBUG
  80. void sendDebug(void)
  81. {
  82.     StdCan_Msg_t txMsg;
  83.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT);
  84.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  85.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID;
  86.     txMsg.Header.ModuleId = act_PIDv1_ID;
  87.     txMsg.Header.Command = CAN_MODULE_CMD_PID_P_I_TERM;
  88.     txMsg.Length = 8;
  89.     FloatType data = PID_GetPTerm(&pid);
  90.     uint8_t *ptr;
  91.     ptr = (uint8_t*)&data;
  92.     txMsg.Data[0] = ptr[0];
  93.     txMsg.Data[1] = ptr[1];
  94.     txMsg.Data[2] = ptr[2];
  95.     txMsg.Data[3] = ptr[3];
  96.     data = PID_GetITerm(&pid);
  97.     ptr = (uint8_t*)&data;
  98.     txMsg.Data[4] = ptr[0];
  99.     txMsg.Data[5] = ptr[1];
  100.     txMsg.Data[6] = ptr[2];
  101.     txMsg.Data[7] = ptr[3];
  102.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  103.    
  104.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT);
  105.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  106.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID;
  107.     txMsg.Header.ModuleId = act_PIDv1_ID;
  108.     txMsg.Header.Command = CAN_MODULE_CMD_PID_D_TERM_OUT;
  109.     txMsg.Length = 8;
  110.     data = PID_GetDTerm(&pid);
  111.     ptr = (uint8_t*)&data;
  112.     txMsg.Data[0] = ptr[0];
  113.     txMsg.Data[1] = ptr[1];
  114.     txMsg.Data[2] = ptr[2];
  115.     txMsg.Data[3] = ptr[3];
  116.     ptr = (uint8_t*)&outputValue;
  117.     txMsg.Data[4] = ptr[0];
  118.     txMsg.Data[5] = ptr[1];
  119.     txMsg.Data[6] = ptr[2];
  120.     txMsg.Data[7] = ptr[3];
  121.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);   
  122.    
  123. //printf("PID: P:%d, I:%d, D:%d, S:%d\n",(int16_t)pidDebugData.P_term,(int16_t)pidDebugData.I_term,(int16_t)pidDebugData.D_term,(int16_t)pidDebugData.Sum);
  124. }
  125. #endif
  126. void act_PIDv1_Init(void)
  127. {
  128. #if act_PIDv1_USEEEPROM==1
  129.     if (EEDATA_OK)
  130.     {
  131.     } else
  132.     {   //The CRC of the EEPROM is not correct, store default values and update CRC
  133.       eeprom_write_dword_crc(EEDATA32.referenceValue, 20.0f, WITHOUT_CRC);
  134.       eeprom_write_byte_crc(EEDATA.sensorModuleType, PIDv1_TEMPERATURE_SENSOR_MODULE_TYPE, WITHOUT_CRC);
  135.       eeprom_write_byte_crc(EEDATA.sensorModuleId, PIDv1_TEMPERATURE_SENSOR_MODULE_ID, WITHOUT_CRC);
  136.       eeprom_write_byte_crc(EEDATA.sensorId, PIDv1_TEMPERATURE_SENSOR, WITHOUT_CRC);
  137.       eeprom_write_dword_crc(EEDATA32.K_P, 850.0f, WITHOUT_CRC);
  138.       eeprom_write_dword_crc(EEDATA32.K_I, 0.5f, WITHOUT_CRC);
  139.       eeprom_write_dword_crc(EEDATA32.K_D, 0.1f, WITHOUT_CRC);
  140.       eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);
  141.       eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);
  142.       eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
  143.       eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
  144.       eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);
  145.       EEDATA_UPDATE_CRC;
  146.     }
  147.    
  148. #else
  149. #error this driver needs EEPROM support
  150. #endif
  151.    
  152.     referenceValue = (float) eeprom_read_dword(EEDATA32.referenceValue);
  153.     sensorModuleType = eeprom_read_byte(EEDATA.sensorModuleType);
  154.     sensorModuleId = eeprom_read_byte(EEDATA.sensorModuleId);
  155.     sensorId = eeprom_read_byte(EEDATA.sensorId);
  156.     uint32_t data_P = eeprom_read_dword(EEDATA32.K_P);
  157.     uint32_t data_I = eeprom_read_dword(EEDATA32.K_I);
  158.     uint32_t data_D = eeprom_read_dword(EEDATA32.K_D);
  159.     float data_P_f = *((float*)((&data_P)));
  160.     float data_I_f = *((float*)((&data_I)));
  161.     float data_D_f = *((float*)(&data_D));
  162.     PID_init(&pid, &measurementValue, &outputValue, &referenceValue, data_P_f, data_I_f, data_D_f, PID_Direction_Direct);
  163.  
  164.     if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
  165.         PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
  166.  
  167.     } else {
  168.         PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
  169.     }
  170.  
  171.     outputValue = DEFAULT_PWM_VALUE;
  172.  
  173.     PID_SetMode(&pid, PID_Mode_Automatic);
  174. }
  175.  
  176. void act_PIDv1_Process(void)
  177. {
  178.     uint8_t newValueCalculated = PID_Compute(&pid);
  179.    
  180.     if (newValueCalculated) {
  181.         sendPID();
  182.     #ifdef act_PIDv1_SEND_DEBUG
  183.         sendDebug_flag= 1;
  184.         return;
  185.     }
  186.     if (sendDebug_flag) {
  187.         sendDebug_flag = 0;
  188.         sendDebug();
  189.     }
  190.     #else
  191.     }
  192.     #endif
  193. }
  194.  
  195. void act_PIDv1_HandleMessage(StdCan_Msg_t *rxMsg)
  196. {
  197.     FloatType data2;
  198.     uint8_t *ptr;
  199.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_ACT &&
  200.             StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_TO_OWNER &&
  201.             rxMsg->Header.ModuleType == CAN_MODULE_TYPE_ACT_PID &&
  202.             rxMsg->Header.ModuleId == act_PIDv1_ID)
  203.     {
  204.         switch (rxMsg->Header.Command)
  205.         {
  206.         case CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS:
  207.             if (rxMsg->Data[0]==0)  //sensor id shall be zero
  208.             {
  209.                 //printf("New setpoint with: %X %X\n",rxMsg->Data[1],rxMsg->Data[2]);
  210.  
  211.                 if (rxMsg->Length == 3)
  212.                 {
  213.                     if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2]) //512 degrees
  214.                     {
  215.                         //pid_Reset_Integrator(&pidData);
  216.                         PID_Status = PID_AUTO;
  217.                     }
  218.                     else
  219.                     {
  220.                         //pid_Reset_Integrator(&pidData);
  221.                         referenceValue = (((float)((rxMsg->Data[1]<<8) + rxMsg->Data[2]))/64);
  222.                         eeprom_write_dword_crc(EEDATA32.referenceValue, referenceValue, WITH_CRC);
  223.                     }
  224.                 }
  225.                 rxMsg->Data[1] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
  226.                 rxMsg->Data[2] = (uint8_t)0x00ff & ((uint32_t)referenceValue*64);
  227.                 StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  228.                 rxMsg->Length = 3;
  229.                 while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  230.             }
  231.         break;
  232.         case CAN_MODULE_CMD_PID_CONFIG_SENSOR:
  233.             if (rxMsg->Length == 3)
  234.             {
  235.                 eeprom_write_byte_crc(EEDATA.sensorModuleType, rxMsg->Data[0] , WITHOUT_CRC);
  236.                 eeprom_write_byte_crc(EEDATA.sensorModuleId, rxMsg->Data[1] , WITHOUT_CRC);
  237.                 eeprom_write_byte_crc(EEDATA.sensorId, rxMsg->Data[2] , WITH_CRC);
  238.                 sensorModuleType = eeprom_read_byte(EEDATA.sensorModuleType);
  239.                 sensorModuleId = eeprom_read_byte(EEDATA.sensorModuleId);
  240.                 sensorId = eeprom_read_byte(EEDATA.sensorId);
  241.             }
  242.             rxMsg->Data[0] = eeprom_read_byte(EEDATA.sensorModuleType);
  243.             rxMsg->Data[1] = eeprom_read_byte(EEDATA.sensorModuleId);
  244.             rxMsg->Data[2] = eeprom_read_byte(EEDATA.sensorId);
  245.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  246.             rxMsg->Length = 3;
  247.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  248.         break;
  249.         case CAN_MODULE_CMD_PID_CONFIG_ACTUATOR:
  250.             if (rxMsg->Length == 3)
  251.             {
  252.                 eeprom_write_byte_crc(EEDATA.actuatorModuleType, rxMsg->Data[0] , WITHOUT_CRC);
  253.                 eeprom_write_byte_crc(EEDATA.actuatorModuleId, rxMsg->Data[1] , WITHOUT_CRC);
  254.                 eeprom_write_byte_crc(EEDATA.actuatorId, rxMsg->Data[2] , WITH_CRC);
  255.             }
  256.             rxMsg->Data[0] = eeprom_read_byte(EEDATA.actuatorModuleType);
  257.             rxMsg->Data[1] = eeprom_read_byte(EEDATA.actuatorModuleId);
  258.             rxMsg->Data[2] = eeprom_read_byte(EEDATA.actuatorId);
  259.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  260.             rxMsg->Length = 3;
  261.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  262.         break;
  263.        
  264.         case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_D_T:
  265.             if (rxMsg->Length == 8)
  266.             {
  267.                 uint32_t* data_32;
  268.                 float* data = (float*)&rxMsg->Data[0];
  269.                 data_32 = (uint32_t*)data;
  270.                 eeprom_write_dword_crc(EEDATA32.K_D, *data_32, WITHOUT_CRC);               
  271.                 eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
  272.                 eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
  273.                 PID_SetTunings(&pid, PID_GetKp(&pid), PID_GetKi(&pid), *data);
  274.                 if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
  275.                     PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
  276.  
  277.                 } else {
  278.                     PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
  279.                 }
  280.                 PID_Status = PID_ON;
  281.                 pwmValue = DEFAULT_PWM_VALUE;
  282.             }
  283.             data2 = PID_GetKd(&pid);
  284.             ptr = (uint8_t*)&data2;
  285.             rxMsg->Data[0] = ptr[0];
  286.             rxMsg->Data[1] = ptr[1];
  287.             rxMsg->Data[2] = ptr[2];
  288.             rxMsg->Data[3] = ptr[3];
  289.             rxMsg->Data[4] = 0u;
  290.             rxMsg->Data[5] = 0u;
  291.             rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));
  292.             rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
  293.             rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
  294.            
  295.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  296.             rxMsg->Length = 8;
  297.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  298.         break;
  299.         case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_P_I:
  300.             if (rxMsg->Length == 8)
  301.             {
  302.                 uint32_t* data_32;
  303.                 float* data = (float*)&rxMsg->Data[0];
  304.                 data_32 = (uint32_t*)data;
  305.                 eeprom_write_dword_crc(EEDATA32.K_P, *data_32, WITHOUT_CRC);
  306.                 //data = (float*)&rxMsg->Data[4];
  307.                 float* data1 = (float*)&rxMsg->Data[4];
  308.                 data_32 = (uint32_t*)data1;
  309.                 eeprom_write_dword_crc(EEDATA32.K_I, *data_32, WITH_CRC);
  310.                
  311.                 PID_SetTunings(&pid, *data, *data1, PID_GetKd(&pid));
  312.                 PID_Status = PID_ON;
  313.                 pwmValue = DEFAULT_PWM_VALUE;
  314.             }
  315.             data2 = PID_GetKp(&pid);
  316.             ptr = (uint8_t*)&data2;
  317.             rxMsg->Data[0] = ptr[0];
  318.             rxMsg->Data[1] = ptr[1];
  319.             rxMsg->Data[2] = ptr[2];
  320.             rxMsg->Data[3] = ptr[3];
  321.             data2 = PID_GetKi(&pid);
  322.             ptr = (uint8_t*)&data2;
  323.             rxMsg->Data[4] = ptr[0];
  324.             rxMsg->Data[5] = ptr[1];
  325.             rxMsg->Data[6] = ptr[2];
  326.             rxMsg->Data[7] = ptr[3];
  327.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  328.             rxMsg->Length = 8;
  329.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  330.         break;
  331.         }
  332.     }
  333.  
  334.  
  335.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
  336.                 StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
  337.                 rxMsg->Header.ModuleType == sensorModuleType &&
  338.                 rxMsg->Header.ModuleId == sensorModuleId &&
  339.                 rxMsg->Header.Command == CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS &&
  340.                 rxMsg->Data[0] == sensorId)
  341.         {
  342.             if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2])
  343.             {
  344.                 //Error on the temperature signal, do something
  345.             }
  346.             else
  347.             {
  348.                 measurementValue = ((float)((rxMsg->Data[1]<<8) + rxMsg->Data[2]))/64;
  349.             }
  350.  
  351.         }
  352. }
  353.  
  354. void act_PIDv1_List(uint8_t ModuleSequenceNumber)
  355. {
  356.     StdCan_Msg_t txMsg;
  357.  
  358.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  359.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  360.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
  361.     txMsg.Header.ModuleId = act_PIDv1_ID;
  362.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_LIST;
  363.     txMsg.Length = 6;
  364.  
  365.     uint32_t HwId=BIOS_GetHwId();
  366.     txMsg.Data[0] = HwId&0xff;
  367.     txMsg.Data[1] = (HwId>>8)&0xff;
  368.     txMsg.Data[2] = (HwId>>16)&0xff;
  369.     txMsg.Data[3] = (HwId>>24)&0xff;
  370.  
  371.     txMsg.Data[4] = NUMBER_OF_MODULES;
  372.     txMsg.Data[5] = ModuleSequenceNumber;
  373.  
  374.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  375. }
  376.