Merge branch 'praktikum' of git.ida.ing.tu-bs.de:sdes1/lab_sdes_student into praktikum

antworten

@@ -0,0 +1,42 @@
+aufgabe 5
+
+
+CPU_Init()
+
+initialisiert die CPU, zb Stack growth direction
+
+Mem_init() richtet memory partitionen ein für kernel speicher. muss aufgerufen werden laut code commentar
+
+OS_Init erstellt 2 Taks, einen IDLE und einen Monitoring Task
+
+
+
+
+OS_InitMisc() is called to initialize miscellaneous variables. It performs various initialization tasks related to miscellaneous features and options within the uC/OS-II kernel. The actions that are performed are:
+a) Clear the 32-bit OS System clock.
+b) Clear the interrupt nesting counter.
+c) Clear the scheduling lock counter.
+d) Clear the number of tasks.
+e) Indicate the OS that the multitasking has not started.
+f) Clear the context switch counter.
+g) Let know the kernel that Statistic task is not ready.
+h) Initialize the OSSafetyCriticalStartFlag(Related to windows) to FALSE indicating safety functionality is not yet enabled or active.
+i) Initialize the task register ID.
+
+OS_InitRdyList() is called to initialize the ready list. The ready list is the task of priorities maintained by the kernel. It keeps tracks of which tasks are ready to run based on the priority levels. It is called to ensure that the ready list is in clean state before the scheduler starts scheduling. All the values in the ready table is set to 0 by this function. The Ready table size is defined by the macro OS_RDY_TBL_SIZE in uC/OS-II . It is defined as:
+#define OS_RDY_TBL_SIZE ((OS_LOWEST/PRIO) / 8u +1u).  The OS_LOWEST is 63 that can be found in os_cfg_r.h of uC/OS-II source code.
+
+OS_InitTCBList() is called to initialize the free list of OS_TCBs. The initialization process typically involves setting up the necessary data structures and variables to manage the free list. This includes linking the TCBs together in the list and configuring any associated fields or flags within the TCBs.
+
+
+
+OSStart() is then called to start multitasking and give control to µC/OS-II. It is very important that you create at least one task before calling OSStart(). Failure to do this will certainly make your application crash. In fact, you may always want to create only one task if you are planning on using the CPU usage statistic task.
+
+
+
+OSTaskCreateExt()  komplizierte Fkt um Tasks zu erstellen mit 9 Argumenten, wie Prio und Interrupt foo
+
+
+OSTaskStackInit()  nur OSTaskStckInit() gefunden,  which is responsible for setting up the task stack. 
+
+OS_TCBInit entnimmt einen OS_TCB aus dem Pool, initialisiert die relevanten Felder abhängig von den aktivierten Features (z. B. erweiterte Task-Felder, Event-Pointer, Delete-Flag) und ruft dabei – mit wieder aktivierten Interrupts – die Hook-Funktionen OSTCBInitHook und OSTaskCreateHook auf. Anschließend werden die Interrupts kurz deaktiviert, der TCB an den Anfang der verketteten Task-Liste eingefügt und der Task als “ready” markiert. Zuletzt gibt die Funktion einen Statuscode zurück, der den erfolgreichen Abschluss der Initialisierung signalisiert.

src/APP/Aufgabe3/ps7/core0/main.c

@@ -27,28 +27,22 @@ void InitDoneCallback(void * p_arg) {
 	(void) p_arg;
 	UCOS_Print("OS started!\r\n");
 
-	//OSTaskCreateExt(timer_Task,
-	//		0,
-	//		&TimerStk[TIMER_STK_SIZE-1],
-	//		TIMER_PRIO,
-	//		TIMER_PRIO,
-	//		&TimerStk[0],
-	//		TIMER_STK_SIZE,
-	//		0,
-	//		0
-	//	       );
-
-	timer_Init();
+	OSTaskCreateExt(timer_Task,
+			0,
+			&TimerStk[TIMER_STK_SIZE-1],
+			TIMER_PRIO,
+			TIMER_PRIO,
+			&TimerStk[0],
+			TIMER_STK_SIZE,
+			0,
+			0
+		       );
 
 	uint16_t melody[][2] = {{440, 500}, {440, 500}, {440, 500}, {349, 350}, {523, 150}, {440, 500}, {349, 350}, {523, 150}, {440, 1000}, {659, 500}, {659, 500}, {659, 500}, {698, 350}, {523, 150}, {415, 500}, {349, 350}, {523, 150}, {440, 1000}};
 
 	while(1){
 		for(uint8_t i=0; i<(sizeof(melody)/sizeof(melody[0])); i++){
-			CPU_SR cpu_sr;
-			OS_ENTER_CRITICAL();
 			pidValue = melody[i][0];
-			OS_EXIT_CRITICAL();
-			timer_Task(0);
 			OSTimeDly(melody[i][1]);
 		}
 		//for(int16_t i = -1000; i<=1000; i++){

src/APP/Aufgabe3/ps7/core0/src/ttc_timer.c

@@ -28,26 +28,31 @@ XTtcPs xttcps[2];
 XGpioPs xgpiops;
 XGpioPs_Config* gpioCfg;
 
-extern int16_t pidValue;
+extern int32_t pidValue;
 
 void timer_Task(void *pdata) {
-	if(pidValue){
-		uint16_t interval = 1.337*(64000/abs(pidValue));
-		uint8_t direction = pidValue<0;
-		//UCOS_Printf("%d %d\r\n", direction, interval);
-		//UCOS_Printf("counter value: %d\r\n", XTtcPs_GetCounterValue(&xttcps));
-		timer_Stop(0);
-		timer_Stop(1);
-		timer_Set_Interval_Length(0,interval);
-		timer_Set_Interval_Length(1,interval);
-		timer_Start(0);
-		timer_Start(1);
-		motor_Set_Moving_Direction(54, direction);
-		motor_Set_Moving_Direction(55, !direction);
-	}
-	else
+	while(1)
 	{
-		timer_Stop(0);
+		if(pidValue){
+			uint16_t interval = 1.337*(64000/abs(pidValue));
+			uint8_t direction = pidValue<0;
+			//UCOS_Printf("%d %d\r\n", direction, interval);
+			//UCOS_Printf("counter value: %d\r\n", XTtcPs_GetCounterValue(&xttcps));
+			timer_Stop(0);
+			timer_Stop(1);
+			timer_Set_Interval_Length(0,interval);
+			timer_Set_Interval_Length(1,interval);
+			timer_Start(0);
+			timer_Start(1);
+			motor_Set_Moving_Direction(54, direction);
+			motor_Set_Moving_Direction(55, !direction);
+		}
+		else
+		{
+			timer_Stop(0);
+			timer_Stop(1);
+		}
+		OSTimeDly(1);
 	}
 }
 
@@ -56,9 +61,7 @@ int timer_Init(){
 	UCOS_Print("Setting up Timer!\r\n");
 
 	timer_Dual_Timer_Setup();
-
 	timer_gpio_Init();
-	motor_Set_Moving_Direction(54, 1);
 
 	UCOS_Print("Done!\r\n");
 	return 0;

src/APP/Aufgabe4/ps7/core0/main.c

@@ -20,10 +20,10 @@
 void InitDoneCallback(void * p_arg) {
 	(void) p_arg;
 	UCOS_Print("OS started!\r\n");
+	mpu9250_Imu_Init(0);
 }
 
 int main(void) {
-
 	MMUInit();
 	UCOSStartup(InitDoneCallback);
 	while (1)

src/APP/Aufgabe4/ps7/core0/src/imu.c

@@ -18,6 +18,7 @@
 #include "ucos_ii.h"
 #include "ucos_bsp.h"
 
+#include <math.h>
 /**
  * Global variables
  */
@@ -26,6 +27,18 @@
 static int16_t _imu_accMaxData[3]={0,0,0}; //x, y, z
 static int16_t _imu_accMinData[3]={0,0,0}; //x, y, z
 
+float imu_current_angle;
+
+int16_t accelData[3];
+int16_t gyroData[3];
+
+int16_t accelBuffer[5000];
+int16_t gyroBuffer[5000];
+int16_t angleBuffer[5000];
+
+static XIicPs Iic;
+XIicPs_Config *Config;
+OS_EVENT * iic_sem;
 
 /**
  * Function Prototypes
@@ -43,15 +56,75 @@ static void mpu9250_Get_Acc_Min_Max();
  * Read sensor data and calculate angle 
  */
 int mpu9250_CalculateAngle(void *pdata){
+	static uint16_t i=0;
 
+	mpu9250_Read_Data(59, 6, (u8*)accelData);
+	mpu9250_Read_Data(67, 6, (u8*)gyroData);
+
+	int16_t accelX = (int16_t)((accelData[0]&0xFF)<<8 | (accelData[0]&0xFF00)>>8);
+	int16_t accelZ = (int16_t)((accelData[2]&0xFF)<<8 | (accelData[2]&0xFF00)>>8);
+	double accelAngle = atan2(accelX,accelZ)*180/PI;
+	double gyroAngleD = (int16_t)((gyroData[1]&0xFF)<<8 | (gyroData[1]&0xFF00)>>8);
+
+	accelBuffer[i] = accelAngle;
+	gyroBuffer[i] = gyroAngleD;
+
+	imu_current_angle = 0.98*(imu_current_angle - (0.012*250*gyroAngleD/INT16_MAX)) + 0.02*accelAngle;
+	//current_angle = current_angle - gyroBuffer[i];
+	//UCOS_Printf("%d %d %d\n", current_angle, accelBuffer[i], gyroBuffer[i]);
+	angleBuffer[i] = imu_current_angle;
+	i=(i+1)%(sizeof(angleBuffer)/sizeof(angleBuffer[0]));
 }
 
 /**
  * Initialize I2C and MPU
  */
 int mpu9250_Imu_Init(void *pdata){
-
+	mpu9250_Iic_Init();
+	mpu9250_Write_Reg(29, 5); // accel dlpf 10Hz
+	mpu9250_Write_Reg(26, 5); // gyro dlpf 10Hz
 }
 
+void iic_isr_recv(){
+       OSSemPost(iic_sem);
+}
 
+static uint8_t mpu9250_Iic_Init(){
+	Config = XIicPs_LookupConfig(IIC_DEVICE_ID) ;
+	XIicPs_CfgInitialize(&Iic, Config, Config->BaseAddress);
+	iic_sem = OSSemCreate(1);
+
+	s32 err;
+
+	err = XIicPs_SelfTest(&Iic);
+	if(err == XST_SUCCESS)
+		UCOS_Printf("i2c selftest succcess\r\n");
+	else
+		UCOS_Printf("i2c selftest fail\r\n");
+
+	err = XIicPs_SetSClk(&Iic, 400000);
+	if(err != XST_SUCCESS)
+		UCOS_Printf("failed to set i2c speed\r\n");
+
+	Iic.StatusHandler = iic_isr_recv;
+	UCOS_IntVectSet(XPS_I2C1_INT_ID, 0, 0, XIicPs_MasterInterruptHandler, &Iic);
+	UCOS_IntSrcEn(XPS_I2C1_INT_ID);
+}
+
+static uint8_t mpu9250_Write_Reg(uint8_t iic_address, uint8_t data){
+	uint8_t tx_buf[] = {iic_address, data};
+	uint8_t err;
+	OSSemPend(iic_sem, 0, &err);
+	XIicPs_MasterSend(&Iic, tx_buf, sizeof(tx_buf), MPU9250_AD);
+	return err;
+}
+
+static int8_t mpu9250_Read_Data(uint8_t iic_address, uint8_t length, u8 RecvBuffer []){
+	uint8_t err;
+	OSSemPend(iic_sem, 0, &err);
+	XIicPs_MasterSend(&Iic, &iic_address, 1, MPU9250_AD);
+	OSSemPend(iic_sem, 0, &err);
+	XIicPs_MasterRecv(&Iic, RecvBuffer, length, MPU9250_AD);
+	return err;
+}
 

src/APP/Aufgabe7/ps7/core0/cfg/gt_cfg.h

@@ -10,7 +10,7 @@
 
 #define GT_USE_NP_REGIONS	1
 #define GT_USE_NP_MEAS		1
-#define GT_NUM_OF_TASKS		3
+#define GT_NUM_OF_TASKS		4
 #define GT_REPORT_CONSOLE	0
 #define GT_REPORT_SVC		0
 

src/APP/Aufgabe7/ps7/core0/main.c

@@ -26,6 +26,10 @@ void InitDoneCallback(void * p_arg) {
 	(void) p_arg;
 	UCOS_Print("OS started!\r\n");
 	GT_Init();
+	uint8_t err;
+	OSTaskNameSet(31, "IMU Task", &err);
+	OSTaskNameSet(32, "Timer Task", &err);
+	OSTaskNameSet(33, "PID Task", &err);
 }
 
 int main(void) {

src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c

@@ -32,7 +32,7 @@ GT_TASK_EXT_T GT_AllTasks[GT_NUM_OF_TASKS] = {
 		{50,	0,		0,		{1,		1}},
 		{120,	0,		0,		{2,		4}},
 		{50,	0,		0,		{3,		5}},
-		//{50,	0,		0,		{3,		5}},
+		{50,	0,		0,		{3,		5}},
 };
 
 /*
@@ -41,10 +41,10 @@ GT_TASK_EXT_T GT_AllTasks[GT_NUM_OF_TASKS] = {
 GT_TASK_T GT_Tasks[GT_NUM_OF_TASKS] =
 {
 		/*Type			Activation Type	Id	Prio	init Function 		Task Function			TaskArg		WL/Runable						Internal 			External				*/
-		{ GT_TASK_EXT , GT_ACT_INT	,	0,	32, 	your-timer-taskinitfunction			,your-timer_Task			, NULL,		GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[0]},
-		{ GT_TASK_EXT , GT_ACT_INT 	,	1,	31, 	your-imu-taskinitfunction			,your-imu_Task				, NULL, 	GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[1]},
-		{ GT_TASK_EXT , GT_ACT_INT	,	2,	33, 	your-pid-taskinitfunction			,your-pid_Task				, NULL, 	GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[2]},
-		//{ GT_TASK_DEF , GT_ACT_INT	,			3,	33, 	_GT_GetNextActivation,		_GT_GetTaskCet,			NULL								,NULL						, NULL, 	20,		GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[3]},
+		{ GT_TASK_EXT , GT_ACT_INT	,	0,	32, 	timer_Init,		timer_Task			, NULL,		GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[0]},
+		{ GT_TASK_EXT , GT_ACT_INT 	,	1,	31, 	mpu9250_Imu_Init,	mpu9250_CalculateAngle		, NULL, 	GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[1]},
+		{ GT_TASK_EXT , GT_ACT_INT	,	2,	33, 	pid_Init,		pid_Task			, NULL, 	GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[2]},
+		{ GT_TASK_DEF , GT_ACT_INT	,	3,	33, 	NULL,			NULL				, NULL, 	GT_RUNABLE_NULL,		GT_INTERNAL_NULL,	(void *)&GT_AllTasks[3]},
 
 };