WebSVN – HomeAutomation – Diff – /trunk/EmbeddedSoftware/AVR/module/act_PIDv1/act_PIDv1.c



        CAN_MODULE_TYPE_ACT_SOFTPWM,    //ActuatorModuleType
        0x00,   //ActuatorModuleId
        0x00,   //ActuatorId
        0x00000000, //uint32_t MAX; (float)
        0x00000000, //uint32_t MIN; (float)
        0x00,   //uint8_t ControllerDirection;
    },
    0   // crc, must be a correct value, but this will also be handled by the EEPROM module or make scripts
};
#endif
#define PID_ON 1

      eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);
      eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);
      eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
      eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
      eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);
      eeprom_write_byte_crc(EEDATA.ControllerDirection, 0, WITHOUT_CRC);
      EEDATA_UPDATE_CRC;
    }
   
#else
#error this driver needs EEPROM support 

    memcpy(&data_P_f, &data_P, sizeof(data_P));
    memcpy(&data_I_f, &data_I, sizeof(data_I));
    memcpy(&data_D_f, &data_D, sizeof(data_D));
    memcpy(&Min_out_f, &Min_D, sizeof(Min_D));
    memcpy(&Max_out_f, &Max_D, sizeof(Max_D));
    PID_init(&pid, &measurementValue, &outputValue, &referenceValue, data_P_f, data_I_f, data_D_f, eeprom_read_byte(EEDATA.ControllerDirection);
 
    if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
        PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
 
    } else {

                uint32_t* data_32;
                float* data = (float*)&rxMsg->Data[0];
                data_32 = (uint32_t*)data;
                eeprom_write_dword_crc(EEDATA32.K_D, *data_32, WITHOUT_CRC);               
                eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
                eeprom_write_byte_crc(EEDATA.ControllerDirection, ((rxMsg->Data[5]&0x80)>>7), WITHOUT_CRC);
                eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
                PID_SetTunings(&pid, PID_GetKp(&pid), PID_GetKi(&pid), *data);
                if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
                    PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
 
                } else {
                    PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
                }
               
                PID_SetControllerDirection(&pid, eeprom_read_byte(EEDATA.ControllerDirection);
   
                PID_Status = PID_ON;
                pwmValue = DEFAULT_PWM_VALUE;
            }
            data2 = PID_GetKd(&pid);
            ptr = (uint8_t*)&data2;
            rxMsg->Data[0] = ptr[0];
            rxMsg->Data[1] = ptr[1];
            rxMsg->Data[2] = ptr[2];
            rxMsg->Data[3] = ptr[3];
            rxMsg->Data[4] = 0u;
            rxMsg->Data[5] = (0x80&(eeprom_read_byte(EEDATA.ControllerDirection))<<7);
            rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));
            rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
            rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
           
            StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
            rxMsg->Length = 8;
            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
        break;
        case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_P_I:
            if (rxMsg->Length == 8)
            {
                uint32_t* data_32;
                float* data = (float*)&rxMsg->Data[0];
                data_32 = (uint32_t*)data;
                eeprom_write_dword_crc(EEDATA32.K_P, *data_32, WITHOUT_CRC);
                //data = (float*)&rxMsg->Data[4];

            rxMsg->Data[0] = ptr[0];
            rxMsg->Data[1] = ptr[1];
            rxMsg->Data[2] = ptr[2];
            rxMsg->Data[3] = ptr[3];
            data2 = PID_GetKi(&pid);
            ptr = (uint8_t*)&data2;
            rxMsg->Data[4] = ptr[0];
            rxMsg->Data[5] = ptr[1];
            rxMsg->Data[6] = ptr[2];
            rxMsg->Data[7] = ptr[3];
            StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
            rxMsg->Length = 8;
            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
        break;
        case CAN_MODULE_CMD_PID_OUTMINMAX:
            //printf("New value with: %X %X %X %X %X %X %X %X\n",rxMsg->Data[0],rxMsg->Data[1],rxMsg->Data[2],rxMsg->Data[3],rxMsg->Data[4],rxMsg->Data[5],rxMsg->Data[6],rxMsg->Data[7]);
            //printf("Len: %X\n",rxMsg->Length);
            if (rxMsg->Length > 1)
            {
                uint32_t* data_32;
                float* data = (float*)&rxMsg->Data[0];
                data_32 = (uint32_t*)data;

                //data = (float*)&rxMsg->Data[4];
                float* data1 = (float*)&rxMsg->Data[4];
                data_32 = (uint32_t*)data1;
                eeprom_write_dword_crc(EEDATA32.MAX, *data_32, WITH_CRC);
                PID_SetOutputLimits(&pid, *data, *data1);
               
            }
            data2 = PID_GetMin(&pid);
            ptr = (uint8_t*)&data2;
            rxMsg->Data[0] = ptr[0];
            rxMsg->Data[1] = ptr[1];

            while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
        break;
       
        }
    }
 
 
    if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
                StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
                rxMsg->Header.ModuleType == sensorModuleType &&
                rxMsg->Header.ModuleId == sensorModuleId &&

Rev 2278	Rev 2281
Line 22...	Line 22...
22	CAN_MODULE_TYPE_ACT_SOFTPWM, //ActuatorModuleType	22	CAN_MODULE_TYPE_ACT_SOFTPWM, //ActuatorModuleType
23	0x00, //ActuatorModuleId	23	0x00, //ActuatorModuleId
24	0x00, //ActuatorId	24	0x00, //ActuatorId
25	0x00000000, //uint32_t MAX; (float)	25	0x00000000, //uint32_t MAX; (float)
26	0x00000000, //uint32_t MIN; (float)	26	0x00000000, //uint32_t MIN; (float)
-		27	0x00, //uint8_t ControllerDirection;
27	},	28	},
28	0 // crc, must be a correct value, but this will also be handled by the EEPROM module or make scripts	29	0 // crc, must be a correct value, but this will also be handled by the EEPROM module or make scripts
29	};	30	};
30	#endif	31	#endif
31	#define PID_ON 1	32	#define PID_ON 1
Line 145...	Line 146...
145	eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);	146	eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PIDv1_CALC_PERIOD_UNIT, WITHOUT_CRC);
146	eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);	147	eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PIDv1_CALC_PERIOD, WITHOUT_CRC);
147	eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);	148	eeprom_write_byte_crc(EEDATA.actuatorModuleType, PIDv1_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
148	eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);	149	eeprom_write_byte_crc(EEDATA.actuatorModuleId, PIDv1_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
149	eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);	150	eeprom_write_byte_crc(EEDATA.actuatorId, PIDv1_PWM_ACTUATOR, WITHOUT_CRC);
-		151	eeprom_write_byte_crc(EEDATA.ControllerDirection, 0, WITHOUT_CRC);
150	EEDATA_UPDATE_CRC;	152	EEDATA_UPDATE_CRC;
151	}	153	}
152		154
153	#else	155	#else
154	#error this driver needs EEPROM support	156	#error this driver needs EEPROM support
Line 171...	Line 173...
171	memcpy(&data_P_f, &data_P, sizeof(data_P));	173	memcpy(&data_P_f, &data_P, sizeof(data_P));
172	memcpy(&data_I_f, &data_I, sizeof(data_I));	174	memcpy(&data_I_f, &data_I, sizeof(data_I));
173	memcpy(&data_D_f, &data_D, sizeof(data_D));	175	memcpy(&data_D_f, &data_D, sizeof(data_D));
174	memcpy(&Min_out_f, &Min_D, sizeof(Min_D));	176	memcpy(&Min_out_f, &Min_D, sizeof(Min_D));
175	memcpy(&Max_out_f, &Max_D, sizeof(Max_D));	177	memcpy(&Max_out_f, &Max_D, sizeof(Max_D));
176	PID_init(&pid, &measurementValue, &outputValue, &referenceValue, data_P_f, data_I_f, data_D_f, ~~PID_Direction_Direct~~);	178	PID_init(&pid, &measurementValue, &outputValue, &referenceValue, data_P_f, data_I_f, data_D_f, eeprom_read_byte(EEDATA.ControllerDirection);
177		179
178	if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {	180	if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
179	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);	181	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
180		182
181	} else {	183	} else {
Line 283...	Line 285...
283	uint32_t* data_32;	285	uint32_t* data_32;
284	float* data = (float*)&rxMsg->Data[0];	286	float* data = (float*)&rxMsg->Data[0];
285	data_32 = (uint32_t*)data;	287	data_32 = (uint32_t*)data;
286	eeprom_write_dword_crc(EEDATA32.K_D, *data_32, WITHOUT_CRC);	288	eeprom_write_dword_crc(EEDATA32.K_D, *data_32, WITHOUT_CRC);
287	eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);	289	eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
-		290	eeprom_write_byte_crc(EEDATA.ControllerDirection, ((rxMsg->Data[5]&0x80)>>7), WITHOUT_CRC);
288	eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);	291	eeprom_write_word_crc(EEDATA16.Time, (uint16_t)rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
289	PID_SetTunings(&pid, PID_GetKp(&pid), PID_GetKi(&pid), *data);	292	PID_SetTunings(&pid, PID_GetKp(&pid), PID_GetKi(&pid), *data);
290	if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {	293	if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_S) {
291	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);	294	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time))*1000);
292		295
293	} else {	296	} else {
294	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));	297	PID_SetSampleTime(&pid, (uint32_t)(eeprom_read_word(EEDATA16.Time)));
295	}	298	}
-		299
-		300	PID_SetControllerDirection(&pid, eeprom_read_byte(EEDATA.ControllerDirection);
-		301
296	PID_Status = PID_ON;	302	PID_Status = PID_ON;
297	pwmValue = DEFAULT_PWM_VALUE;	303	pwmValue = DEFAULT_PWM_VALUE;
298	}	304	}
299	data2 = PID_GetKd(&pid);	305	data2 = PID_GetKd(&pid);
300	ptr = (uint8_t*)&data2;	306	ptr = (uint8_t*)&data2;
301	rxMsg->Data[0] = ptr[0];	307	rxMsg->Data[0] = ptr[0];
302	rxMsg->Data[1] = ptr[1];	308	rxMsg->Data[1] = ptr[1];
303	rxMsg->Data[2] = ptr[2];	309	rxMsg->Data[2] = ptr[2];
304	rxMsg->Data[3] = ptr[3];	310	rxMsg->Data[3] = ptr[3];
305	rxMsg->Data[4] = 0u;	311	rxMsg->Data[4] = 0u;
306	rxMsg->Data[5] = 0u;	312	rxMsg->Data[5] = (0x80&(eeprom_read_byte(EEDATA.ControllerDirection))<<7);
307	rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));	313	rxMsg->Data[6] = (0x7f&(eeprom_read_word(EEDATA16.Time)>>8));
308	rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));	314	rxMsg->Data[7] = (0xff&(eeprom_read_word(EEDATA16.Time)));
309	rxMsg->Data[6] \|= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);	315	rxMsg->Data[6] \|= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
310		316
311	StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);	317	StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
312	rxMsg->Length = 8;	318	rxMsg->Length = 8;
313	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);	319	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
314	break;	320	break;
315	case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_P_I:	321	case CAN_MODULE_CMD_PID_CONFIG_PARAMETER_P_I:
316	if (rxMsg->Length == 8)	322	if (rxMsg->Length == 8)
317	{	323	{
318	uint32_t* data_32;	324	uint32_t* data_32;
319	float* data = (float*)&rxMsg->Data[0];	325	float* data = (float*)&rxMsg->Data[0];
320	data_32 = (uint32_t*)data;	326	data_32 = (uint32_t*)data;
321	eeprom_write_dword_crc(EEDATA32.K_P, *data_32, WITHOUT_CRC);	327	eeprom_write_dword_crc(EEDATA32.K_P, *data_32, WITHOUT_CRC);
322	//data = (float*)&rxMsg->Data[4];	328	//data = (float*)&rxMsg->Data[4];
Line 333...	Line 339...
333	rxMsg->Data[0] = ptr[0];	339	rxMsg->Data[0] = ptr[0];
334	rxMsg->Data[1] = ptr[1];	340	rxMsg->Data[1] = ptr[1];
335	rxMsg->Data[2] = ptr[2];	341	rxMsg->Data[2] = ptr[2];
336	rxMsg->Data[3] = ptr[3];	342	rxMsg->Data[3] = ptr[3];
337	data2 = PID_GetKi(&pid);	343	data2 = PID_GetKi(&pid);
338	ptr = (uint8_t*)&data2;	344	ptr = (uint8_t*)&data2;
339	rxMsg->Data[4] = ptr[0];	345	rxMsg->Data[4] = ptr[0];
340	rxMsg->Data[5] = ptr[1];	346	rxMsg->Data[5] = ptr[1];
341	rxMsg->Data[6] = ptr[2];	347	rxMsg->Data[6] = ptr[2];
342	rxMsg->Data[7] = ptr[3];	348	rxMsg->Data[7] = ptr[3];
343	StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);	349	StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
344	rxMsg->Length = 8;	350	rxMsg->Length = 8;
345	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);	351	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
346	break;	352	break;
347	case CAN_MODULE_CMD_PID_OUTMINMAX:	353	case CAN_MODULE_CMD_PID_OUTMINMAX:
-		354	//printf("New value with: %X %X %X %X %X %X %X %X\n",rxMsg->Data[0],rxMsg->Data[1],rxMsg->Data[2],rxMsg->Data[3],rxMsg->Data[4],rxMsg->Data[5],rxMsg->Data[6],rxMsg->Data[7]);
-		355	//printf("Len: %X\n",rxMsg->Length);
348	if (rxMsg->Length > 1)	356	if (rxMsg->Length > 1)
349	{	357	{
350	uint32_t* data_32;	358	uint32_t* data_32;
351	float* data = (float*)&rxMsg->Data[0];	359	float* data = (float*)&rxMsg->Data[0];
352	data_32 = (uint32_t*)data;	360	data_32 = (uint32_t*)data;
Line 354...	Line 362...
354	//data = (float*)&rxMsg->Data[4];	362	//data = (float*)&rxMsg->Data[4];
355	float* data1 = (float*)&rxMsg->Data[4];	363	float* data1 = (float*)&rxMsg->Data[4];
356	data_32 = (uint32_t*)data1;	364	data_32 = (uint32_t*)data1;
357	eeprom_write_dword_crc(EEDATA32.MAX, *data_32, WITH_CRC);	365	eeprom_write_dword_crc(EEDATA32.MAX, *data_32, WITH_CRC);
358	PID_SetOutputLimits(&pid, data, data1);	366	PID_SetOutputLimits(&pid, data, data1);
359		367
360	}	368	}
361	data2 = PID_GetMin(&pid);	369	data2 = PID_GetMin(&pid);
362	ptr = (uint8_t*)&data2;	370	ptr = (uint8_t*)&data2;
363	rxMsg->Data[0] = ptr[0];	371	rxMsg->Data[0] = ptr[0];
364	rxMsg->Data[1] = ptr[1];	372	rxMsg->Data[1] = ptr[1];
Line 375...	Line 383...
375	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);	383	while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
376	break;	384	break;
377		385
378	}	386	}
379	}	387	}
380		-
381		388
382	if ( StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&	389	if ( StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
383	StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&	390	StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
384	rxMsg->Header.ModuleType == sensorModuleType &&	391	rxMsg->Header.ModuleType == sensorModuleType &&
385	rxMsg->Header.ModuleId == sensorModuleId &&	392	rxMsg->Header.ModuleId == sensorModuleId &&

Subversion Repositories HomeAutomation

(root)/trunk/EmbeddedSoftware/AVR/module/act_PIDv1/act_PIDv1.c – Rev 2278 → 2281