A new forlder for embedded software in our common structure

4 files changed, 420 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
new file mode 100644
index 000000000..fbea57f7d
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -0,0 +1,39 @@
+
+#include "PIDAlgo.h"
+
+float PIDAlgorithmCalculation(uint32_t _setPoint,uint32_t _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
+{
+	float error;
+	float derivative;
+	float output;
+
+	//Calculate P,I,D
+	error = _setPoint - _mesuredParam;
+
+	//In case of error too small then stop integration
+	if(abs(error) > params->epsilon)
+	{
+		*_integral = *_integral + error*params->dt;
+	}
+
+	derivative = (error - *_pre_error)/params->dt;
+
+	output = params->Kp*error + params->Ki**_integral + params->Kd*derivative;
+
+	//Saturation Filter
+	if(output > params->MAX)
+	{
+		output = params->MAX;
+	}
+	else if(output < params->MIN)
+	{
+		output = params->MIN;
+	}
+
+	//Update error
+	*_pre_error = error;
+
+	return output;
+}
+
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
new file mode 100644
index 000000000..5f8f4dfe6
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -0,0 +1,18 @@
+
+#ifndef MODULES_PIDALGO_H_
+#define MODULES_PIDALGO_H_
+#include <stdint.h>
+
+typedef struct
+{
+    float epsilon;
+    float dt;
+    float MAX;
+    float MIN;
+    float Kp;
+    float Kd;
+    float Ki;
+}PID_Config_Params;
+float PIDAlgorithmCalculation(uint32_t _setPoint,uint32_t _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
+
+#endif /* MODULES_PIDALGO_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
new file mode 100644
index 000000000..8f389855e
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -0,0 +1,333 @@
+/************************************************************************************************************************
+ * control.c
+ * Control module
+ *
+ * The control module is hardware agnostic, not related to a specific hardware module, to enable it to deal with hardware and system changes easily.
+ * The control module contains a high priority task that connects the hardware drivers
+ * below to the control algorithms of the high level system modules above.
+ *
+ * The control task is invoked by a message from a 1 millisecond timer interrupt.
+ * The module is based on system components registered in the control module, supplying callback hooks for information gathering and distribution.
+ * All control clients (drivers and modules) will not perform long procedures that will block the control task.
+ *
+ * Control interfaces:
+ * System Devices list:
+ *
+ * All control system input devices   will be listed in a system-shared enumareted list, to create a common language between the modules and the drivers.
+ * (The list will include all hardware devices, but for this module only control input devices are valid)
+ * Drivers:
+ * Device registration: a driver that initializes a control input device will call the ControlDeviceRegister function with the deviceId,
+ * and a control callback hook. This callback is called with a deviceId as the parameter, and returns an unsigned 32bit integer
+ * as a read value and a call status. The callback will be a non-blocking call, and will indicate in the status value if the data is valid.
+ *
+ * It is the responsibility of the device driver to update the control information according to the module hardware requirements.
+ * If the polling of the information is immediate, it can be collected at the callback call (e.g. local GPI).
+ *
+ * Module registration – control:  a module is registering to receive the value of the input from a specific device.
+ * The module indicates what will be the desired frequency of the device polling (in milliseconds).
+ * It supplies a callback routine that will receive the deviceId, the control value status and the control value.
+ * The callback will be a non-blocking call.
+ * The polling frequency is one of a specific list of frequencies: 1/10/100/1000 Hz. (others - TBD)
+ *
+ *
+ * Registering a request for control information from a device that is not registered will be rejected.
+ * The device registration process must be performed before module registration process.
+ *
+ * Unregistering: when control information is not needed, the module will unregister the device polling request from the control module.
+ * There is a separate call for control hooks and for report hooks.
+ * The unregistering command contains the callback function pointer, to enable distribution of control
+ * information of the same device to more than one destination.
+ *
+ **************************************************************************************************************************/
+
+////////////////////////////////State machine operation////////////////////////////////////
+//the state machine operation is used to operate in runtime correct profile flow execution
+//by recieved esign flow of the user from the UI
+///////////////////////////////////////////////////////////////////////////////////////////
+#include "include.h"
+#include "Modules/General/GeneralHardware.h"
+
+#include <driverlib/timer.h>
+#include <inc/hw_ints.h>
+
+#include "drivers/adc_sampling/adc.h"
+#include "control.h"
+/******************** Definitions  ********************************************/
+#define INVALID_MSG_ID    0xFFFF
+#define MAX_TANGO_CONTROL_DEVICES 200
+/******************** STRUCTURES AND ENUMs  ********************************************/
+typedef struct
+{
+  uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
+  DeviceDataFunction Callback;
+  uint32_t lastStatus;
+}DriverDeviceStruc;
+DriverDeviceStruc DevicesArray[MAX_PORT_ENUM];
+
+
+typedef struct
+{
+  uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
+  bool ControlActive;
+  uint32_t Parameter;
+  DataReadCBFunction ControlDataReadPtr;
+  ControlCBFunction ControlCallbackPtr;
+  CTRL_TIMING_ENUM ControlTiming;
+}ControlDeviceStruc;
+
+typedef enum
+{
+    OneMillisec,
+}controlMessages;
+
+typedef struct ControlMessage{
+    uint16_t messageId;
+    uint16_t msglen;
+    uint32_t tick;
+    uint8_t messageData[20];
+}ControlMessageStruc;
+/******************** GLOBAL PARAMETERS  ********************************************/
+Mailbox_Handle          ControlMsgQ = NULL;
+bool                    ControlRestart;
+static GateMutex_Handle gateControlDB;
+
+ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
+uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
+uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
+/******************** Functions  ********************************************/
+void OneMilliSecondFunction(UArg arg0);
+
+//**********************************************************************
+/******************** CODE  ********************************************/
+//**********************************************************************
+
+void ControlInit(void)
+{
+    int Device_i;
+    Error_Block eb;
+
+    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
+
+    ControlRestart = false;
+
+    memset(ControlDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
+
+    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++)
+    {
+        DevicesArray[Device_i].Callback = NULL;
+        DevicesArray[Device_i].lastStatus = ERROR;
+    }
+
+    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++)
+    {
+        ControlArray[Device_i].ControlActive = false;
+        ControlArray[Device_i].ControlCallbackPtr = NULL;
+        ControlArray[Device_i].ControlDataReadPtr = NULL;
+        ControlArray[Device_i].ControlTiming = eNoControl;
+    }
+    gateControlDB = GateMutex_create(NULL, &eb);
+    if (gateControlDB == NULL)
+    {
+        System_abort("Could not create USB Wait gate");
+    }
+
+    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
+    ROM_TimerConfigure(Control_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    //TimerIntRegister(Control_timerBase, TIMER_A, Timer0Isr);    // Registering  isr
+    ROM_TimerEnable(Control_timerBase, TIMER_A);
+    ROM_IntEnable(INT_TIMER0A);
+    ROM_TimerIntEnable(Control_timerBase, TIMER_TIMA_TIMEOUT);
+
+    ADCAcquireInit();
+
+    return;
+}
+void ControlStop(void)
+{
+    ControlRestart = false;
+    ADCAcquireStop();
+}
+
+void ControlStart(void)
+{
+    ControlRestart = true;
+    ROM_TimerLoadSet(Control_timerBase, TIMER_BOTH,120000/*one millisecond*/);
+    ADCAcquireStart(0,1);
+}
+
+/************************************************************************************************************************************************
+ * the control task reads the data from the devices every millisecond.
+ * (the data might be old data, if it is polled in a slower rate)
+ * for every polled device, there is a need to add a Data read callback, with a device id. the read value is always 32bits unsigned integer
+ * if there is a need to run a control function based on the read data, then the hardware module will add a control function, specifying the control calling rate
+ * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
+ *
+ ***************************************************************************************************************************************************/
+int  AddControlCallback(uint32_t deviceId, ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency )
+{
+    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
+    assert(Callback);
+    unsigned int key;
+
+    if(ControlArray[deviceId].ControlDataReadPtr == NULL)
+    {
+        LOG_ERROR (deviceId, "No dataread function for device");
+        return ERROR;
+    }
+
+    key = GateMutex_enter(gateControlDB);
+    ControlArray[deviceId].ControlTiming = CtrlFrequency;
+    ControlArray[deviceId].ControlCallbackPtr = Callback;
+    ControlArray[deviceId].ControlActive = true;
+    GateMutex_leave(gateControlDB, key);
+
+    return OK;
+
+}
+int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
+{
+    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
+    unsigned int key;
+
+    if (Callback == ControlArray[deviceId].ControlCallbackPtr)
+    {
+        key = GateMutex_enter(gateControlDB);
+        ControlArray[deviceId].ControlTiming = eNoControl;
+        ControlArray[deviceId].ControlCallbackPtr = NULL;
+        ControlArray[deviceId].ControlActive = false;
+        GateMutex_leave(gateControlDB, key);
+        return OK;
+    }
+    else
+        return ERROR;
+
+}
+
+int  RegisterDevice(uint32_t deviceId, DataReadCBFunction Callback, uint32_t Parameter)
+{
+    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
+    assert(Callback);
+    unsigned int key;
+    key = GateMutex_enter(gateControlDB);
+    ControlArray[deviceId].Parameter = Parameter;
+    ControlArray[deviceId].ControlDataReadPtr = Callback;
+    GateMutex_leave(gateControlDB, key);
+    return OK;
+
+}
+int  UnRegisterDevice(uint32_t deviceId, DataReadCBFunction Callback )
+{
+    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
+    unsigned int key;
+    if (Callback == ControlArray[deviceId].ControlDataReadPtr)
+    {
+        key = GateMutex_enter(gateControlDB);
+        ControlArray[deviceId].Parameter = 0;
+        ControlArray[deviceId].ControlDataReadPtr = NULL;
+        ControlDatalog[deviceId] = 0;
+        GateMutex_leave(gateControlDB, key);
+        return OK;
+    }
+    else
+        return ERROR;
+
+}
+
+void OneMilliSecondControlInterrupt(UArg arg0)
+{
+    ControlMessageStruc Message;
+    bool retcode = false;
+    ROM_IntMasterDisable();
+
+    //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
+    //we might want to call it from the task, afetr execution of other taks!!!
+    ADC_TriggerCollection();
+    //send message to the control task
+    Message.messageId = OneMillisec;
+    Message.tick = UsersysTickGet();
+    Message.msglen = sizeof(ControlMessageStruc);
+    if (ControlMsgQ != NULL)
+        retcode = Mailbox_post(ControlMsgQ , &Message, BIOS_NO_WAIT);
+    if (ControlRestart == true)
+    {
+        ROM_TimerLoadSet(Control_timerBase, TIMER_BOTH,120000/*one millisecond*/);
+    }
+
+    ROM_TimerIntClear(Control_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
+    //
+    // Enable all interrupts.
+    //
+    ROM_IntMasterEnable();
+    return ;
+}
+
+uint32_t ControlLoop(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;
+    Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
+    Hundred_msTick  =   (tick%eHunderdMillisecond == 0)  ?true:false;
+    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
+
+    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
+    {
+        if (ControlArray[Device_i].ControlDataReadPtr)
+            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter);
+        if (ControlArray[Device_i].ControlActive)
+        {
+            switch (ControlArray[Device_i].ControlTiming)
+            {
+                case eOneMillisecond:
+                        ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    break;
+                case eTenMilliSecond:
+                    if (Ten_msTick)
+                        ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    break;
+                case eHunderdMillisecond:
+                    if (Hundred_msTick)
+                        ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    break;
+                case eOneSecond:
+                    if (Onesecond_Tick)
+                        ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    break;
+                default:
+                    LOG_ERROR (ControlArray[Device_i].ControlTiming, "Invalid control timing value");
+                    break;
+            }
+        }
+    }
+
+    return OK;
+}
+/******************************************************************************
+ *  ======== messageTsk ========
+ *  Task for this function is created statically. See the project's .cfg file.
+ *  this message task is created statically in system initialization,
+ ******************************************************************************/
+void controlTask(UArg arg0, UArg arg1)
+{
+    ControlMessageStruc Message;
+    //char str[60];
+    //uint16_t length;
+    //Clock_setTimeout(HostKAClock, 1000);
+    //Clock_start(HostKAClock);
+
+
+    while(1)
+    {
+        Mailbox_pend(ControlMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
+        {
+            case OneMillisec:
+                ControlLoop(Message.tick);
+                break;
+            default:
+                break;
+        }
+    }
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
new file mode 100644
index 000000000..ab259993a
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -0,0 +1,30 @@
+/************************************************************************************************************************
+ * control.c
+ **************************************************************************************************************************/
+
+////////////////////////////////State machine operation////////////////////////////////////
+//the state machine operation is used to operate in runtime correct profile flow execution
+//by recieved esign flow of the user from the UI
+///////////////////////////////////////////////////////////////////////////////////////////
+#include "include.h"
+
+typedef  uint32_t (* DeviceDataFunction)(uint32_t deviceID, uint32_t *Value);
+typedef  uint32_t (* ControlCBFunction)(uint32_t deviceID, uint32_t ReadValue);
+typedef  uint32_t (* DataReadCBFunction)(uint32_t deviceID, uint32_t Parameter);
+typedef enum {
+    eNoControl = 0,
+    eOneMillisecond = 1,
+    eTenMilliSecond = 10,
+    eHunderdMillisecond = 100,
+    eOneSecond = 1000
+}CTRL_TIMING_ENUM;
+
+
+void    ControlInit(void);
+void    ControlStop(void);
+void    ControlStart(void);
+int     AddControlCallback(uint32_t deviceID, ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency );
+int     RemoveControlCallback(uint32_t deviceID, ControlCBFunction Callback );
+int     RegisterDevice(uint32_t deviceID, DataReadCBFunction Callback, uint32_t Parameter);
+int     UnRegisterDevice(uint32_t deviceID, DataReadCBFunction Callback );
+