diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c | 131 |
1 files changed, 102 insertions, 29 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 3a558f434..b4ceebbc2 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -34,14 +34,13 @@ /******************** STRUCTURES AND ENUMs ********************************************/ typedef struct MillisecMotorData { - bool Active; bool WaitForData; bool DataRequired; MSecFptr Callback; unsigned long Data; int Length; + bool Active; }MillisecMotorDataStruc; - typedef enum { OneMillisec, @@ -63,9 +62,11 @@ bool MotorBusy_Data[MOTOR_SPARE1_1] = {true}; uint32_t Dancer_Data[NUM_OF_DANCERS] = {0}; MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0}; - +MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0}; +MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0}; /******************** GLOBAL PARAMETERS ********************************************/ Mailbox_Handle MillisecMsgQ = NULL; +Mailbox_Handle MotorsMsgQ[NUM_OF_MOTORS] = {NULL}; bool MillisecRestart; static GateMutex_Handle gateMillisecDB; @@ -80,18 +81,20 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea void MillisecInit(void) { Error_Block eb; + int i; - MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,NULL); + Error_init(&eb); + + MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb); + for (i=0;i<NUM_OF_MOTORS;i++) + { + MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 6, NULL,&eb); + } MillisecRestart = false; memset(MillisecDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES); - int i; - for (i=0;i<NUM_OF_MOTORS;i++) - { - MotorData[i].Active = false; - } gateMillisecDB = GateMutex_create(NULL, &eb); if (gateMillisecDB == NULL) { @@ -149,32 +152,62 @@ void OneMilliSecondMillisecInterrupt(UArg arg0) ROM_IntMasterEnable(); return ; } +int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback) +{ + if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1; + PT100Data[TempSensorId].Callback = Callback; + PT100Data[TempSensorId].Data = Data; + PT100Data[TempSensorId].Length = Length; + PT100Data[TempSensorId].DataRequired = false; + PT100Data[TempSensorId].Active = true; +} +int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback) +{ + if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1; + PT100Data[TempSensorId].Callback = Callback; + PT100Data[TempSensorId].Data = Data; + PT100Data[TempSensorId].Length = Length; + PT100Data[TempSensorId].DataRequired = true; + PT100Data[TempSensorId].Active = true; + +} //typedef uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue); +int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback) +{ + if (MotorId >= NUM_OF_MOTORS) return -1; + SpeedSetPending[MotorId].Callback = Callback; + SpeedSetPending[MotorId].Data = Data; + SpeedSetPending[MotorId].Length = Length; + SpeedSetPending[MotorId].DataRequired = false; + SpeedSetPending[MotorId].Active = true; +} int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback) { + MillisecMotorDataStruc MotorData = {0}; if (MotorId >= NUM_OF_MOTORS) return -1; - if (MotorData[MotorId].Active == true) return -2; - MotorData[MotorId].Callback = Callback; - MotorData[MotorId].Data = Data; - MotorData[MotorId].Length = Length; - MotorData[MotorId].Active = true; - MotorData[MotorId].DataRequired = false; - return OK; + MotorData.Callback = Callback; + MotorData.Data = Data; + MotorData.Length = Length; + MotorData.DataRequired = false; + if (MotorsMsgQ[MotorId] != NULL) + return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT); + else return false; } int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback) { + MillisecMotorDataStruc MotorData = {0}; if (MotorId >= NUM_OF_MOTORS) return -1; - if (MotorData[MotorId].Active == true) return -2; - MotorData[MotorId].Callback = Callback; - MotorData[MotorId].Data = Data; - MotorData[MotorId].Length = Length; - MotorData[MotorId].Active = true; - MotorData[MotorId].DataRequired = true; - return OK; + MotorData.Callback = Callback; + MotorData.Data = Data; + MotorData.Length = Length; + MotorData.DataRequired = true; + if (MotorsMsgQ[MotorId] != NULL) + return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT); + else return false; } uint32_t MillisecLoop(uint32_t tick) { - int Motor_i; + int Motor_i,Sensor_i; unsigned int MotorInfo = 0; //call all modules Millisec functions //test dancers and speed encoders @@ -185,34 +218,74 @@ uint32_t MillisecLoop(uint32_t tick) Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; //gather Motor data from FPGA + FPGA_GetBusy(); //load the busy motor information to all motors for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++) { + if (MotorDriverResponse[Motor_i].Busy == true) + continue; if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting { if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA { MotorData[Motor_i].WaitForData = false; - MotorData[Motor_i].Callback(Motor_i,MotorInfo); + if (MotorData[Motor_i].Callback) + MotorData[Motor_i].Callback(Motor_i,MotorInfo); } } - if (MotorData[Motor_i].Active == true) //new data to send + if (SpeedSetPending[Motor_i].Active == true) + { + MotorSendFPGARequest(Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length); + if (SpeedSetPending[Motor_i].Callback) + SpeedSetPending[Motor_i].Callback(Motor_i,0); + } + else if (Mailbox_pend(MotorsMsgQ[Motor_i] , &MotorData[Motor_i], BIOS_NO_WAIT)==true) { if (MotorSendFPGARequest(Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA { - MotorData[Motor_i].Active = false; //set the Active to false first, because the callback might send a new request immediately if (MotorData[Motor_i].DataRequired == true) { MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round } else { - MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution + if (MotorData[Motor_i].Callback) + MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution } } } } Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER); - + if (Hundred_msTick) + { + //FPGA_GetTempSensorBusy(); + for (Sensor_i = 0;Sensor_i < NUM_OF_MOTORS;Sensor_i++) + { + //if (TempDriverDriverResponse[Sensor_i].Busy == true) + // continue; + if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting + { + if (MotorGetFPGAResponse(Sensor_i,&MotorInfo) == OK) //got the data from the FPGA + { + PT100Data[Sensor_i].WaitForData = false; + if (PT100Data[Sensor_i].Callback) + PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo); + } + } + if (PT100Data[Sensor_i].Active == true) + { + MotorSendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length); + if (PT100Data[Sensor_i].DataRequired == true) + { + PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round + } + else + { + if (PT100Data[Sensor_i].Callback) + PT100Data[Sensor_i].Callback(Sensor_i,0); // call the callback to report execution + } + } + } + } #ifndef EVALUATION_BOARD /* this cannot be done within one millisecond, and not needed * instead, check if there is a motor waiting with data to send or read request |