From f1347b9bea5562781e1f2d0440981a15c52b5144 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 24 Apr 2018 10:04:46 +0300 Subject: change milisecond handling of the motors to mailboxes to enable concurrent request (that will be handled one after another. set speed request is prioritized. busy is checked --- .../Embedded/Modules/Control/MillisecTask.c | 82 ++++++++++++++-------- .../Embedded/Modules/Control/MillisecTask.h | 3 +- 2 files changed, 55 insertions(+), 30 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index d9571f3ff..44e5e682f 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}; /******************** 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; + + Error_init(&eb); - MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,NULL); + MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb); + for (i=0;i= 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) { @@ -185,28 +199,38 @@ 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 } } } @@ -227,9 +251,9 @@ uint32_t MillisecLoop(uint32_t tick) MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING); MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);*/ //gather Dancer data from FPGA - Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER,0); - Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER,0); - Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER,0); + 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); //gather data from FPGA if (Ten_msTick) { diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h index 286ed7ca0..d27018031 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h @@ -15,7 +15,8 @@ typedef uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue); int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback); - +int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback); +int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback); uint32_t getMotorStatusData(int MotorId); uint32_t getMotorSpeedData(int MotorId); -- cgit v1.3.1 From 56d8d91c96cfa1bb7b0f48ee50ebca8dbf6aa71c Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 24 Apr 2018 10:26:10 +0300 Subject: PT100 FPGA interface through 1 millisec task --- .../Embedded/Modules/Control/MillisecTask.c | 55 ++++++++++++++++++++-- .../Embedded/Modules/Control/MillisecTask.h | 3 ++ 2 files changed, 55 insertions(+), 3 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 44e5e682f..b4ceebbc2 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -63,7 +63,7 @@ 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}; @@ -152,6 +152,25 @@ 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) { @@ -188,7 +207,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len } 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 @@ -236,7 +255,37 @@ uint32_t MillisecLoop(uint32_t tick) } } 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 diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h index d27018031..f0009b43b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h @@ -18,6 +18,9 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback); int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback); +int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback); +int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback); + uint32_t getMotorStatusData(int MotorId); uint32_t getMotorSpeedData(int MotorId); uint32_t getTemperatureSensorData(int SensorId); -- cgit v1.3.1