Subversion Repositories HomeAutomation

Rev

Rev 2264 | Rev 2270 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | SVN | RSS feed

Rev 2264 Rev 2269
Line 2... Line 2...
2 
#include "act_PIDv1.h"
 2 
#include "act_PIDv1.h"
3 
#include <drivers/misc/PID_v1.h>
 3 
#include <drivers/misc/PID_v1.h>
- 
 
 4 
#include <drivers/misc/PID_AutoTune.h>
4 
 
 5 
 
5 
#if act_PIDv1_USEEEPROM==1
 6 
#if act_PIDv1_USEEEPROM==1
6 
#include "act_PIDv1_eeprom.h"
 7 
#include "act_PIDv1_eeprom.h"
7 
struct eeprom_act_PIDv1 EEMEM eeprom_act_PIDv1 =
 8 
struct eeprom_act_PIDv1 EEMEM eeprom_act_PIDv1 =
8 
{
 9 
{
Line 81... Line 82...
81 
}
 82 
}
82 
 
 83 
 
83 
#ifdef act_PIDv1_SEND_DEBUG_TIMER
 84 
#ifdef act_PIDv1_SEND_DEBUG_TIMER
84 
void sendPID_debug_callback(uint8_t timer)
 85 
void sendPID_debug_callback(uint8_t timer)
85 
{
 86 
{
86 
    /*
 87 
   
87 
    StdCan_Msg_t txMsg;
 88 
    StdCan_Msg_t txMsg;
88 
    StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
 89 
    StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
89 
    StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
 90 
    StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
90 
    txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
 91 
    txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
91 
    txMsg.Header.ModuleId = act_PIDv1_ID;
 92 
    txMsg.Header.ModuleId = act_PIDv1_ID;
92 
    txMsg.Header.Command = CAN_MODULE_CMD_PID_DEBUG;
 93 
    txMsg.Header.Command = CAN_MODULE_CMD_PID_DEBUG;
93 
    txMsg.Length = 8;
 94 
    txMsg.Length = 8;
94 
 
 95 
 
95 
    txMsg.Data[0] = ((int16_t) (pidDebugData.P_term)>>8)&0xff;
 96 
    txMsg.Data[0] = ((int16_t) (PID_GetPTerm(&pid))>>8)&0xff;
96 
    txMsg.Data[1] = ((int16_t) (pidDebugData.P_term))&0xff;
 97 
    txMsg.Data[1] = ((int16_t) (PID_GetPTerm(&pid)))&0xff;
97 
    txMsg.Data[2] = ((int16_t) (pidDebugData.I_term)>>8)&0xff;
 98 
    txMsg.Data[2] = ((int16_t) (PID_GetITerm(&pid))>>8)&0xff;
98 
    txMsg.Data[3] = ((int16_t) (pidDebugData.I_term))&0xff;
 99 
    txMsg.Data[3] = ((int16_t) (PID_GetITerm(&pid)))&0xff;
99 
    txMsg.Data[4] = ((int16_t) (pidDebugData.D_term)>>8)&0xff;
 100 
    txMsg.Data[4] = ((int16_t) (PID_GetDTerm(&pid))>>8)&0xff;
100 
    txMsg.Data[5] = ((int16_t) (pidDebugData.D_term))&0xff;
 101 
    txMsg.Data[5] = ((int16_t) (PID_GetDTerm(&pid)))&0xff;
101 
    txMsg.Data[6] = ((int16_t) (pidDebugData.Sum)>>8)&0xff;
 102 
    //txMsg.Data[6] = ((int16_t) (pidDebugData.Sum)>>8)&0xff;
102 
    txMsg.Data[7] = ((int16_t) (pidDebugData.Sum))&0xff;
 103 
    //txMsg.Data[7] = ((int16_t) (pidDebugData.Sum))&0xff;
- 
 
 104 
    txMsg.Data[6] = 0;
- 
 
 105 
    txMsg.Data[7] = 0;
- 
 
 106 
    while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
- 
 
 107 
   
- 
 
 108 
    StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT);
- 
 
 109 
    StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
- 
 
 110 
    txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID;
- 
 
 111 
    txMsg.Header.ModuleId = act_PIDv1_ID;
- 
 
 112 
    txMsg.Header.Command = CAN_MODULE_CMD_PID_P_I_TERM;
- 
 
 113 
    txMsg.Length = 8;
- 
 
 114 
    FloatType data = PID_GetPTerm(&pid);
- 
 
 115 
    data = 1.0f;
- 
 
 116 
    uint8_t *ptr;
- 
 
 117 
    ptr = (uint8_t*)&data;
- 
 
 118 
    txMsg.Data[0] = ptr[0];
- 
 
 119 
    txMsg.Data[1] = ptr[1];
- 
 
 120 
    txMsg.Data[2] = ptr[2];
- 
 
 121 
    txMsg.Data[3] = ptr[3];
- 
 
 122 
    data = PID_GetITerm(&pid);
- 
 
 123 
    data = -2.0f;
- 
 
 124 
    ptr = (uint8_t*)&data;
- 
 
 125 
    txMsg.Data[4] = ptr[0];
- 
 
 126 
    txMsg.Data[5] = ptr[1];
- 
 
 127 
    txMsg.Data[6] = ptr[2];
- 
 
 128 
    txMsg.Data[7] = ptr[3];
- 
 
 129 
    while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
- 
 
 130 
   
- 
 
 131 
    StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT);
- 
 
 132 
    StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
- 
 
 133 
    txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID;
- 
 
 134 
    txMsg.Header.ModuleId = act_PIDv1_ID;
- 
 
 135 
    txMsg.Header.Command = CAN_MODULE_CMD_PID_D_TERM_OUT;
- 
 
 136 
    txMsg.Length = 8;
- 
 
 137 
    data = PID_GetDTerm(&pid);
- 
 
 138 
    ptr = (uint8_t*)&data;
- 
 
 139 
    txMsg.Data[0] = ptr[0];
- 
 
 140 
    txMsg.Data[1] = ptr[1];
- 
 
 141 
    txMsg.Data[2] = ptr[2];
- 
 
 142 
    txMsg.Data[3] = ptr[3];
- 
 
 143 
    ptr = (uint8_t*)&outputValue;
- 
 
 144 
    txMsg.Data[4] = ptr[0];
- 
 
 145 
    txMsg.Data[5] = ptr[1];
- 
 
 146 
    txMsg.Data[6] = ptr[2];
- 
 
 147 
    txMsg.Data[7] = ptr[3];
103 
    while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
 148 
    while (StdCan_Put(&txMsg) != StdCan_Ret_OK);   
104 
    */
 149 
   
105 
//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);
 150 
//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);
106 
}
 151 
}
107 
#endif
 152 
#endif
108 
void act_PIDv1_Init(void)
 153 
void act_PIDv1_Init(void)
109 
{
 154 
{
110 
#if act_PIDv1_USEEEPROM==1
 155 
#if act_PIDv1_USEEEPROM==1
111 
    if (EEDATA_OK)
 156 
    if (EEDATA_OK)
112 
    {
 157 
    {
113 
    } else
 158 
    } else
114 
    {   //The CRC of the EEPROM is not correct, store default values and update CRC
 159 
    {   //The CRC of the EEPROM is not correct, store default values and update CRC
115 
      eeprom_write_dword_crc(EEDATA32.referenceValue, 20.0, WITHOUT_CRC);
 160 
      eeprom_write_dword_crc(EEDATA32.referenceValue, 20.0f, WITHOUT_CRC);
116 
      eeprom_write_byte_crc(EEDATA.sensorModuleType, PIDv1_TEMPERATURE_SENSOR_MODULE_TYPE, WITHOUT_CRC);
 161 
      eeprom_write_byte_crc(EEDATA.sensorModuleType, PIDv1_TEMPERATURE_SENSOR_MODULE_TYPE, WITHOUT_CRC);
117 
      eeprom_write_byte_crc(EEDATA.sensorModuleId, PIDv1_TEMPERATURE_SENSOR_MODULE_ID, WITHOUT_CRC);
 162 
      eeprom_write_byte_crc(EEDATA.sensorModuleId, PIDv1_TEMPERATURE_SENSOR_MODULE_ID, WITHOUT_CRC);
118 
      eeprom_write_byte_crc(EEDATA.sensorId, PIDv1_TEMPERATURE_SENSOR, WITHOUT_CRC);
 163 
      eeprom_write_byte_crc(EEDATA.sensorId, PIDv1_TEMPERATURE_SENSOR, WITHOUT_CRC);
119 
      eeprom_write_dword_crc(EEDATA32.K_P, 850.0, WITHOUT_CRC);
 164 
      eeprom_write_dword_crc(EEDATA32.K_P, 850.0f, WITHOUT_CRC);
120 
      eeprom_write_dword_crc(EEDATA32.K_I, 0.5, WITHOUT_CRC);
 165 
      eeprom_write_dword_crc(EEDATA32.K_I, 0.5f, WITHOUT_CRC);
121 
      eeprom_write_dword_crc(EEDATA32.K_D, 0.1, WITHOUT_CRC);
 166 
      eeprom_write_dword_crc(EEDATA32.K_D, 0.1f, WITHOUT_CRC);
122 
      eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);
 167 
      eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);
123 
      eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);
 168 
      eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);
124 
      eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
 169 
      eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
125 
      eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
 170 
      eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
126 
      eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);
 171 
      eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);
Line 139... Line 184...
139 
   
 184 
   
140 
    PID_init(&pid, &measurementValue, &outputValue, &referenceValue, (float) eeprom_read_dword(EEDATA32.K_P), (float) eeprom_read_dword(EEDATA32.K_I), (float) eeprom_read_dword(EEDATA32.K_D), PID_Direction_Direct);
 185 
    PID_init(&pid, &measurementValue, &outputValue, &referenceValue, (float) eeprom_read_dword(EEDATA32.K_P), (float) eeprom_read_dword(EEDATA32.K_I), (float) eeprom_read_dword(EEDATA32.K_D), PID_Direction_Direct);
141 
 
 186 
 
142 
    if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
 187 
    if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
143 
        PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
 188 
        PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
144 
    } else {
 189 
    } else {
145 
        PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
 190 
        PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
146 
    }
 191 
    }
147 
 
 192 
 
148 
    outputValue = DEFAULT_PWM_VALUE;
 193 
    outputValue = DEFAULT_PWM_VALUE;
149 
 
 194 
 
150 
    PID_SetMode(&pid, PID_Mode_Automatic);
 195 
    PID_SetMode(&pid, PID_Mode_Automatic);
151 
   
 196 
   
152 
    Timer_SetTimeout(act_PIDv1_SEND_TIMER, eeprom_read_byte(EEDATA.sendPeriod)*1000, TimerTypeFreeRunning, &sendPID_callback);
 197 
    Timer_SetTimeout(act_PIDv1_SEND_TIMER, eeprom_read_byte(EEDATA.sendPeriod)*1000, TimerTypeFreeRunning, &sendPID_callback);
153 
    #ifdef act_PIDv1_SEND_DEBUG_TIMER
 198 
    #ifdef act_PIDv1_SEND_DEBUG_TIMER
154 
      Timer_SetTimeout(act_PIDv1_SEND_DEBUG_TIMER, PIDv1_SEND_DEBUG_PERIOD, TimerTypeFreeRunning, &sendPID_debug_callback);
 199 
      Timer_SetTimeout(act_PIDv1_SEND_DEBUG_TIMER, PIDv1_SEND_DEBUG_PERIOD, TimerTypeFreeRunning, &sendPID_debug_callback);
155 
    #endif
 200 
    #endif
156 
}
 201 
}
157 
 
 202 
 
158 
void act_PIDv1_Process(void)
 203 
void act_PIDv1_Process(void)
159 
{
 204 
{
160 
    PID_Compute(&pid);
 205 
    PID_Compute(&pid);
161 
   
 206 
   
162 
    if (sendPID_flag) {
 207 
    if (sendPID_flag) {
163 
        sendPID();
 208 
        sendPID();
164 
        sendPID_flag= 0;
 209 
        sendPID_flag= 0;
165 
    }
 210 
    }
166 
}
 211 
}
167 
 
 212 
 
168 
void act_PIDv1_HandleMessage(StdCan_Msg_t *rxMsg)
 213 
void act_PIDv1_HandleMessage(StdCan_Msg_t *rxMsg)
169 
{
 214 
{
- 
 
 215 
    FloatType data2;
- 
 
 216 
    uint8_t *ptr;
170 
    if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_ACT &&
 217 
    if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_ACT &&
171 
            StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_TO_OWNER &&
 218 
            StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_TO_OWNER &&
172 
            rxMsg->Header.ModuleType == CAN_MODULE_TYPE_ACT_PID &&
 219 
            rxMsg->Header.ModuleType == CAN_MODULE_TYPE_ACT_PID &&
173 
            rxMsg->Header.ModuleId == act_PIDv1_ID)
 220 
            rxMsg->Header.ModuleId == act_PIDv1_ID)
174 
    {
 221 
    {
175 
        switch (rxMsg->Header.Command)
 222 
        switch (rxMsg->Header.Command)
176 
        {
 223 
        {
177 
        case CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS:
 224 
        case CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS:
178 
            if (rxMsg->Data[0]==0)  //sensor id shall be zero
 225 
            if (rxMsg->Data[0]==0)  //sensor id shall be zero
179 
            {
 226 
            {
180 
              printf("Hej: data1: %x Data2: %x\n",rxMsg->Data[1],rxMsg->Data[2]);
 227 
                printf("New setpoint with: %X %X\n",rxMsg->Data[1],rxMsg->Data[2]);
181 
 
 228 
 
182 
                if (rxMsg->Length == 3)
 229 
                if (rxMsg->Length == 3)
183 
                {
 230 
                {
184 
                    if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2])
 231 
                    if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2]) //512 degrees
185 
                    {
 232 
                    {
186 
                        //pid_Reset_Integrator(&pidData);
 233 
                        //pid_Reset_Integrator(&pidData);
187 
                        PID_Status = PID_AUTO;
 234 
                        PID_Status = PID_AUTO;
188 
                    }
 235 
                    }
189 
                    else
 236 
                    else
190 
                    {
 237 
                    {
191 
                        //pid_Reset_Integrator(&pidData);
 238 
                        //pid_Reset_Integrator(&pidData);
192 
                        eeprom_write_word_crc(EEDATA16.referenceValue, ((rxMsg->Data[1]<<8) + rxMsg->Data[2]), WITH_CRC);
 - 
 
193 
                        referenceValue = ((rxMsg->Data[1]<<8) + rxMsg->Data[2])/64;
 239 
                        referenceValue = (((float)((rxMsg->Data[1]<<8) + rxMsg->Data[2]))/64);
194 
                       
 - 
 
195 
                        rxMsg->Data[1] = 0x10;
 - 
 
196 
                rxMsg->Data[2] = 0x20;
 - 
 
197 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
 240 
                        eeprom_write_dword_crc(EEDATA32.referenceValue, referenceValue, WITH_CRC);
198 
                rxMsg->Length = 3;
 - 
 
199 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 - 
 
200 
                    }
 241 
                    }
201 
                rxMsg->Data[1] = 0x05;
 - 
 
202 
                rxMsg->Data[2] = 0x05;
 - 
 
203 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
 - 
 
204 
                rxMsg->Length = 3;
 - 
 
205 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 - 
 
206 
                }
 242 
                }
207 
                rxMsg->Data[1] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
 243 
                rxMsg->Data[1] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
208 
                rxMsg->Data[2] = (uint8_t)0x00ff & ((uint32_t)referenceValue*64);
 244 
                rxMsg->Data[2] = (uint8_t)0x00ff & ((uint32_t)referenceValue*64);
209 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
 245 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
210 
                rxMsg->Length = 3;
 246 
                rxMsg->Length = 3;
211 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 247 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
212 
               
 - 
 
213 
               
 - 
 
214 
 
 - 
 
215 
            }
 248 
            }
216 
        break;
 249 
        break;
217 
        case CAN_MODULE_CMD_PID_CONFIG_SENSOR:
 250 
        case CAN_MODULE_CMD_PID_CONFIG_SENSOR:
218 
            if (rxMsg->Length == 3)
 251 
            if (rxMsg->Length == 3)
219 
            {
 252 
            {
Line 257... Line 290...
257 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 290 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
258 
            }
 291 
            }
259 
        break;
 292 
        break;
260 
 
 293 
 
261 
        case CAN_MODULE_CMD_PID_CONFIG_PARAMETER:
 294 
        case CAN_MODULE_CMD_PID_CONFIG_PARAMETER:
262 
            if (rxMsg->Length == 8)
 295 
            if (rxMsg->Length == 8)
263 
            {
 296 
            {
264 
                eeprom_write_word_crc(EEDATA16.K_P, (uint16_t)rxMsg->Data[1]+(rxMsg->Data[0]<<8), WITHOUT_CRC);
 297 
                eeprom_write_dword_crc(EEDATA32.K_P, (((float)((rxMsg->Data[0]<<8) + rxMsg->Data[1]))/64), WITHOUT_CRC);
265 
                eeprom_write_word_crc(EEDATA16.K_I, (uint16_t)rxMsg->Data[3]+(rxMsg->Data[2]<<8), WITHOUT_CRC);
 298 
                eeprom_write_dword_crc(EEDATA32.K_I, (((float)((rxMsg->Data[2]<<8) + rxMsg->Data[3]))/64), WITHOUT_CRC);
266 
                eeprom_write_word_crc(EEDATA16.K_D, (uint16_t)rxMsg->Data[5]+(rxMsg->Data[4]<<8), WITHOUT_CRC);
 299 
                eeprom_write_dword_crc(EEDATA32.K_D, (((float)((rxMsg->Data[4]<<8) + rxMsg->Data[5]))/64), WITHOUT_CRC);
267 
                eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
 300 
                eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
268 
                eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
 301 
                eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
269 
                if (eeprom_read_byte(EEDATA.TimeMsOrS) == 0) {
 302 
                if (eeprom_read_byte(EEDATA.TimeMsOrS) == 0) {
270 
                    //Timer_SetTimeout(act_PIDv1_TIMER, 1000, TimerTypeFreeRunning, &calculatePID_callback);
 303 
                    //Timer_SetTimeout(act_PIDv1_TIMER, 1000, TimerTypeFreeRunning, &calculatePID_callback);
271 
                } else {
 304 
                } else {
272 
                    //Timer_SetTimeout(act_PIDv1_TIMER, eeprom_read_word(EEDATA16.Time), TimerTypeFreeRunning, &calculatePID_callback);
 305 
                    //Timer_SetTimeout(act_PIDv1_TIMER, eeprom_read_word(EEDATA16.Time), TimerTypeFreeRunning, &calculatePID_callback);
273 
                }
 306 
                }
- 
 
 307 
                PID_SetTunings(&pid, (float) eeprom_read_dword(EEDATA32.K_P), (float) eeprom_read_dword(EEDATA32.K_I), (float) eeprom_read_dword(EEDATA32.K_D));
- 
 
 308 
                PID_Status = PID_ON;
- 
 
 309 
                pwmValue = DEFAULT_PWM_VALUE;
- 
 
 310 
            }
- 
 
 311 
            rxMsg->Data[0] = ((int16_t) (PID_GetKp(&pid))>>8)&0xff;
- 
 
 312 
            rxMsg->Data[1] = ((int16_t) (PID_GetKp(&pid)))&0xff;
- 
 
 313 
            rxMsg->Data[2] = ((int16_t) (PID_GetKi(&pid))>>8)&0xff;
- 
 
 314 
            rxMsg->Data[3] = ((int16_t) (PID_GetKi(&pid)))&0xff;
- 
 
 315 
            rxMsg->Data[4] = ((int16_t) (PID_GetKd(&pid))>>8)&0xff;
- 
 
 316 
            rxMsg->Data[5] = ((int16_t) (PID_GetKd(&pid)))&0xff;
- 
 
 317 
            rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));
- 
 
 318 
            rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
- 
 
 319 
            rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
- 
 
 320 
            StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
- 
 
 321 
            rxMsg->Length = 8;
- 
 
 322 
            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
- 
 
 323 
        break;
- 
 
 324 
       
- 
 
 325 
        case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_D_T:
- 
 
 326 
            if (rxMsg->Length == 8)
- 
 
 327 
            {
- 
 
 328 
                float *data = (float*)&rxMsg->Data[0];
- 
 
 329 
               
- 
 
 330 
                eeprom_write_dword_crc(EEDATA32.K_D, *data, WITHOUT_CRC);
- 
 
 331 
                eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
- 
 
 332 
                eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
- 
 
 333 
                PID_SetTunings(&pid, PID_GetKp(&pid), PID_GetKi(&pid), (float) eeprom_read_dword(EEDATA32.K_D));
- 
 
 334 
                PID_Status = PID_ON;
- 
 
 335 
                pwmValue = DEFAULT_PWM_VALUE;
- 
 
 336 
            }
- 
 
 337 
            data2 = PID_GetKd(&pid);
- 
 
 338 
            ptr = (uint8_t*)&data2;
- 
 
 339 
            rxMsg->Data[0] = ptr[0];
- 
 
 340 
            rxMsg->Data[1] = ptr[1];
- 
 
 341 
            rxMsg->Data[2] = ptr[2];
- 
 
 342 
            rxMsg->Data[3] = ptr[3];
- 
 
 343 
            rxMsg->Data[4] = 0u;
- 
 
 344 
            rxMsg->Data[5] = 0u;
- 
 
 345 
            rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));
- 
 
 346 
            rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
- 
 
 347 
            rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
- 
 
 348 
           
- 
 
 349 
            StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
- 
 
 350 
            rxMsg->Length = 8;
- 
 
 351 
            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
- 
 
 352 
        break;
- 
 
 353 
        case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_P_I:
- 
 
 354 
            if (rxMsg->Length == 8)
- 
 
 355 
            {
- 
 
 356 
                float *data = (float*)&rxMsg->Data[0];
- 
 
 357 
               
- 
 
 358 
                eeprom_write_dword_crc(EEDATA32.K_P, *data, WITHOUT_CRC);
- 
 
 359 
                data = (float*)&rxMsg->Data[4];
- 
 
 360 
                eeprom_write_dword_crc(EEDATA32.K_I, *data, WITH_CRC);
274 
 
 361 
               
275 
                //pid_Init((float) eeprom_read_word(EEDATA16.K_P)/64 * SCALING_FACTOR, (float) eeprom_read_word(EEDATA16.K_I)/64 * SCALING_FACTOR , (float) eeprom_read_word(EEDATA16.K_D)/64 * SCALING_FACTOR , &pidData);
 362 
                PID_SetTunings(&pid, (float) eeprom_read_dword(EEDATA32.K_P), (float) eeprom_read_dword(EEDATA32.K_I), PID_GetKd(&pid));
276 
                //pwmValue += (int16_t) pid_Controller(referenceValue, measurementValue, &pidData, &pidDebugData);
 - 
 
277 
                PID_Status = PID_ON;
 363 
                PID_Status = PID_ON;
278 
                pwmValue = DEFAULT_PWM_VALUE;
 364 
                pwmValue = DEFAULT_PWM_VALUE;
279 
 
 365 
            }
- 
 
 366 
            data2 = PID_GetKp(&pid);
- 
 
 367 
            ptr = (uint8_t*)&data2;
280 
                rxMsg->Data[0] = (0xff&(eeprom_read_word(EEDATA16.K_P)>>8));
 368 
            rxMsg->Data[0] = ptr[0];
281 
                rxMsg->Data[1] = (0xff&(eeprom_read_word(EEDATA16.K_P)));
 369 
            rxMsg->Data[1] = ptr[1];
282 
                rxMsg->Data[2] = (0xff&(eeprom_read_word(EEDATA16.K_I)>>8));
 370 
            rxMsg->Data[2] = ptr[2];
283 
                rxMsg->Data[3] = (0xff&(eeprom_read_word(EEDATA16.K_I)));
 371 
            rxMsg->Data[3] = ptr[3];
284 
                rxMsg->Data[4] = (0xff&(eeprom_read_word(EEDATA16.K_D)>>8));
 372 
            data2 = PID_GetKi(&pid);
- 
 
 373 
            ptr = (uint8_t*)&data2;
285 
                rxMsg->Data[5] = (0xff&(eeprom_read_word(EEDATA16.K_D)));
 374 
            rxMsg->Data[4] = ptr[0];
286 
                rxMsg->Data[6] = (0xff&(eeprom_read_word(EEDATA16.Time)>>8));
 375 
            rxMsg->Data[5] = ptr[1];
287 
                rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
 376 
            rxMsg->Data[6] = ptr[2];
288 
                rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
 377 
            rxMsg->Data[7] = ptr[3];
289 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
 378 
            StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
290 
                rxMsg->Length = 8;
 379 
            rxMsg->Length = 8;
291 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 380 
            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
292 
            } else
 - 
 
293 
            {
 - 
 
294 
                rxMsg->Data[0] = (0xff&(eeprom_read_word(EEDATA16.K_P)>>8));
 - 
 
295 
                rxMsg->Data[1] = (0xff&(eeprom_read_word(EEDATA16.K_P)));
 - 
 
296 
                rxMsg->Data[2] = (0xff&(eeprom_read_word(EEDATA16.K_I)>>8));
 - 
 
297 
                rxMsg->Data[3] = (0xff&(eeprom_read_word(EEDATA16.K_I)));
 - 
 
298 
                rxMsg->Data[4] = (0xff&(eeprom_read_word(EEDATA16.K_D)>>8));
 - 
 
299 
                rxMsg->Data[5] = (0xff&(eeprom_read_word(EEDATA16.K_D)));
 - 
 
300 
                rxMsg->Data[6] = (0xff&(eeprom_read_word(EEDATA16.Time)>>8));
 - 
 
301 
                rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
 - 
 
302 
                rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
 - 
 
303 
                StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
 - 
 
304 
                rxMsg->Length = 8;
 - 
 
305 
                while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
 - 
 
306 
            }
 - 
 
307 
        break;
 381 
        break;
308 
        }
 382 
        }
309 
    }
 383 
    }
310 
 
 384 
 
311 
 
 385