diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Control')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c | 82 | ||||
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Control/control.c | 91 |
2 files changed, 160 insertions, 13 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 68bd680a8..64b6ac4e0 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -90,6 +90,7 @@ MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0}; MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0}; /******************** GLOBAL PARAMETERS ********************************************/ Mailbox_Handle MillisecMsgQ = NULL; +Mailbox_Handle TenMillisecMsgQ = NULL; Mailbox_Handle MotorsMsgQ[NUM_OF_MOTORS] = {NULL}; bool MillisecRestart; static GateMutex_Handle gateMillisecDB; @@ -114,6 +115,7 @@ void MillisecInit(void) Error_init(&eb); MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb); + TenMillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb); for (i=0;i<NUM_OF_MOTORS;i++) { MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb); @@ -158,6 +160,7 @@ void MillisecStart(void) void OneMilliSecondMillisecInterrupt(UArg arg0) { MillisecMessageStruc Message; + uint32_t Tenmsec_millisecondCounter = 0; ROM_IntMasterDisable(); ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT); // Clear the timer interrupt if (MillisecRestart == true) @@ -170,13 +173,26 @@ void OneMilliSecondMillisecInterrupt(UArg arg0) ROM_IntMasterEnable(); return; } + if (msec_millisecondCounter%10 == 0) + { + Tenmsec_millisecondCounter = msec_millisecondCounter; + } //send message to the Millisec task Message.messageId = OneMillisec; - Message.tick = msec_millisecondCounter++;; + Message.tick = msec_millisecondCounter++; Message.msglen = sizeof(MillisecMessageStruc); if (MillisecMsgQ != NULL) Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT); + if (Tenmsec_millisecondCounter) + { + Message.messageId = OneMillisec; + Message.tick = Tenmsec_millisecondCounter; + Message.msglen = sizeof(MillisecMessageStruc); + if (TenMillisecMsgQ != NULL) + Mailbox_post(TenMillisecMsgQ , &Message, BIOS_NO_WAIT); + } + // // Enable all interrupts. // @@ -341,17 +357,16 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len else return false; } -//TEMPERATURE_SENSOR_ID_ENUM Sensor_Read = 0; uint32_t MillisecLoop(uint32_t tick) { - uint8_t Motor_i,Disp_i; + uint8_t Motor_i; TEMPERATURE_SENSOR_ID_ENUM Sensor_i; unsigned int MotorInfo = 0; static int temp=0; //call all modules Millisec functions //test dancers and speed encoders //check all callback units (state machine waiting for completion of a change) - bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick; +/* bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick; Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; O900Millisecond_Tick = (tick%eOneSecond == 900) ?true:false; @@ -360,7 +375,7 @@ uint32_t MillisecLoop(uint32_t tick) Tick98 = (tick%eHundredMillisecond == 99) ?true:false; //gather Motor data from FPGA //ROM_IntMasterDisable(); - +*/ #ifndef EVALUATION_BOARD FPGA_GetBusy(); //load the busy motor information to all motors FPGA_Read_limit_Switches_Registers(); @@ -473,6 +488,26 @@ uint32_t MillisecLoop(uint32_t tick) Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER); Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER); Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER); + return OK; +} +uint32_t MillisecLowLoop(uint32_t tick) +{ + uint8_t Disp_i; + TEMPERATURE_SENSOR_ID_ENUM Sensor_i; + + static int temp=0; + //call all modules Millisec functions + //test dancers and speed encoders + //check all callback units (state machine waiting for completion of a change) + bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick; + Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; + Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; + O900Millisecond_Tick = (tick%eOneSecond == 900) ?true:false; + Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; + OneMinute_Tick = (tick%eOneMinute == 0) ?true:false; + //gather Motor data from FPGA + //ROM_IntMasterDisable(); + if (Ten_msTick) { //Speed_Data = Calculate_Speed_Sensor_Velocity(); @@ -501,7 +536,7 @@ uint32_t MillisecLoop(uint32_t tick) for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++) { CalculateDispenserPressure(Disp_i); - Read_MidTank_Pressure_Sensor(Disp_i); + //Read_MidTank_Pressure_Sensor(Disp_i); } FPGA_GetAllDispensersValveBusyOCD(); DrawerFansStatus = Read_Fans_Tacho(); @@ -509,10 +544,10 @@ uint32_t MillisecLoop(uint32_t tick) } if (OneMinute_Tick) { - for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++) - { - Read_MidTank_Pressure_Sensor(Disp_i); - } +// for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++) +// { +// Read_MidTank_Pressure_Sensor(Disp_i); +// } /* for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++) { if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW) @@ -553,6 +588,33 @@ void MillisecTask(UArg arg0, UArg arg1) } } } +/****************************************************************************** + * ======== messageTsk ======== + * Task for this function is created statically. See the project's .cfg file. + * this message task is created statically in system initialization, + ******************************************************************************/ +void MillisecLowTask(UArg arg0, UArg arg1) +{ + MillisecMessageStruc Message; + //char str[60]; + //uint16_t length; + //Clock_setTimeout(HostKAClock, 1000); + //Clock_start(HostKAClock); + //MillisecInit(); + //Millisecond_Task_Handle = Task_self(); + while(1) + { + Mailbox_pend(TenMillisecMsgQ , &Message, BIOS_WAIT_FOREVER); + switch (Message.messageId) + { + case OneMillisec: + MillisecLowLoop(Message.tick); + break; + default: + break; + } + } +} /*uint32_t getMotorStatusData(int MotorId) diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c index 2110029e4..ed1e73bef 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c @@ -87,6 +87,7 @@ int ControlPhaseDelay = 300; //the control task enters only after data gathering //this parameters defines how many microseconds in the delay. it is used only on starting the control loop on the first time /******************** GLOBAL PARAMETERS ********************************************/ Mailbox_Handle ControlMsgQ = NULL; +Mailbox_Handle TenControlMsgQ = NULL; bool ControlRestart; static GateMutex_Handle gateControlDB; Task_Handle Control_Task_Handle; @@ -115,6 +116,7 @@ void ControlInit(void) //Mailbox_Params_init(&ControlMsgQ); ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL); + TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL); ControlRestart = false; @@ -258,6 +260,8 @@ uint32_t millisecondCounter = 0; void OneMilliSecondControlInterrupt(UArg arg0) { ControlMessageStruc Message; + uint32_t TenmillisecondCounter = 0; + ROM_IntMasterDisable(); ROM_TimerIntClear(Control_timerBase, TIMER_TIMA_TIMEOUT); // Clear the timer interrupt @@ -272,6 +276,10 @@ void OneMilliSecondControlInterrupt(UArg arg0) //ROM_TimerDisable(Control_timerBase, TIMER_A); return; } + if (millisecondCounter%10 == 0) + { + TenmillisecondCounter = millisecondCounter; + } //send message to the control task Message.messageId = OneMillisec; @@ -279,6 +287,12 @@ void OneMilliSecondControlInterrupt(UArg arg0) Message.msglen = sizeof(ControlMessageStruc); if (ControlMsgQ != NULL) Mailbox_post(ControlMsgQ , &Message, BIOS_NO_WAIT); + //if (TenmillisecondCounter) + { + //Message.tick = TenmillisecondCounter; + if (TenControlMsgQ != NULL) + Mailbox_post(TenControlMsgQ , &Message, BIOS_NO_WAIT); + } if (millisecondCounter == 1000000000) millisecondCounter = 0; // @@ -294,7 +308,7 @@ uint32_t ControlLoop(uint32_t tick) //test dancers and speed encoders //check all callback units (state machine waiting for completion of a change) uint32_t Device_i; - bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998; + /*bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998; Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; // Hundred_msTick = (tick%200 == 0) ?true:false; @@ -302,7 +316,7 @@ uint32_t ControlLoop(uint32_t tick) Tick98 = (tick%eHundredMillisecond == 98) ?true:false; // Tick98 = (tick%200 == 199) ?true:false; Tick998 = (tick%eOneSecond == 996) ?true:false; - +*/ //ROM_IntMasterDisable(); for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++) { @@ -323,6 +337,40 @@ uint32_t ControlLoop(uint32_t tick) else LOG_ERROR (Device_i, "Invalid callback ptr"); break; + default: + break; + } //switch + } //if control active + } //for + //ROM_IntMasterEnable(); + + return OK; +} +uint32_t ControlLowLoop(uint32_t tick) +{ + //call all modules control functions + //test dancers and speed encoders + //check all callback units (state machine waiting for completion of a change) + uint32_t Device_i; + bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998; + Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; + Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; + Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; + Tick98 = (tick%eHundredMillisecond == 98) ?true:false; + Tick998 = (tick%eOneSecond == 996) ?true:false; + + //ROM_IntMasterDisable(); + for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++) + { + if (ControlArray[Device_i].ControlActive) + { + ControlBacklog[backlogindex]=Device_i; + if ( ++backlogindex >= 999) + backlogindex = 0; + switch (ControlArray[Device_i].ControlTiming) + { + case eOneMillisecond: + break; case eTenMillisecond: if (Ten_msTick) { @@ -374,7 +422,17 @@ uint32_t ControlLoop(uint32_t tick) memset (&ControlArray[Device_i],0,sizeof(ControlDeviceStruc) ); break; default: - LOG_ERROR (ControlArray[Device_i].ControlTiming, "Invalid control timing value"); + if (tick%ControlArray[Device_i].ControlTiming == 0) + { + if(ControlArray[Device_i].ControlDataReadPtr) + ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); + if(ControlArray[Device_i].ControlCallbackPtr) + ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]); + else + LOG_ERROR (Device_i, "Invalid callback ptr"); + } break; } //switch } //if control active @@ -410,3 +468,30 @@ void controlTask(UArg arg0, UArg arg1) } } } +/****************************************************************************** + * ======== messageTsk ======== + * Task for this function is created statically. See the project's .cfg file. + * this message task is created statically in system initialization, + ******************************************************************************/ +void controlLowTask(UArg arg0, UArg arg1) +{ + ControlMessageStruc Message; + //char str[60]; + //uint16_t length; + //Clock_setTimeout(HostKAClock, 1000); + //Clock_start(HostKAClock); + Control_Task_Handle = Task_self(); + + while(1) + { + Mailbox_pend(TenControlMsgQ , &Message, BIOS_WAIT_FOREVER); + switch (Message.messageId) + { + case OneMillisec: + ControlLowLoop(Message.tick); + break; + default: + break; + } + } +} |