From 67c0c2c90ecc677ebf74cd0fb2059a1cac97d641 Mon Sep 17 00:00:00 2001
From: sdes1 <sdes1@stu-workstation3.net.ida>
Date: Tue, 11 Nov 2025 17:37:12 +0100
Subject: [PATCH] read sensor

---
 src/APP/Aufgabe4/ps7/core0/main.c    |  2 +-
 src/APP/Aufgabe4/ps7/core0/src/imu.c | 48 ++++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/src/APP/Aufgabe4/ps7/core0/main.c b/src/APP/Aufgabe4/ps7/core0/main.c
index 73666be..e43b154 100644
--- a/src/APP/Aufgabe4/ps7/core0/main.c
+++ b/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)
diff --git a/src/APP/Aufgabe4/ps7/core0/src/imu.c b/src/APP/Aufgabe4/ps7/core0/src/imu.c
index f61a1ef..951e68d 100644
--- a/src/APP/Aufgabe4/ps7/core0/src/imu.c
+++ b/src/APP/Aufgabe4/ps7/core0/src/imu.c
@@ -26,6 +26,13 @@
 static int16_t _imu_accMaxData[3]={0,0,0}; //x, y, z
 static int16_t _imu_accMinData[3]={0,0,0}; //x, y, z
 
+int16_t accelData[3];
+int16_t gyroData[3];
+
+int16_t drawBuffer[1000];
+
+static XIicPs Iic;
+XIicPs_Config *Config;
 
 /**
  * Function Prototypes
@@ -50,8 +57,49 @@ int mpu9250_CalculateAngle(void *pdata){
  * Initialize I2C and MPU
  */
 int mpu9250_Imu_Init(void *pdata){
+	//mpu9250_Write_Reg();
+	mpu9250_Iic_Init();
+	mpu9250_Write_Reg(29, 5);
+
+	//uint8_t rxbuf[20];
+	uint16_t i=0;
+	while(1){
+		mpu9250_Read_Data(0x3B, 6, (u8*)accelData);
+		mpu9250_Read_Data(0x43, 6, (u8*)gyroData);
+		drawBuffer[i]=(int16_t)((accelData[0]&0xFF)<<8 | (accelData[8]&0xFF00)>>8);
+		i=(i+1)%1000;
+		OSTimeDly(1);
+	}
 
 }
 
 
+static uint8_t mpu9250_Iic_Init(){
+	Config = XIicPs_LookupConfig(IIC_DEVICE_ID) ;
+	XIicPs_CfgInitialize(&Iic, Config, Config->BaseAddress);
+
+	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");
+}
+
+static uint8_t mpu9250_Write_Reg(uint8_t iic_address, uint8_t data){
+	uint8_t tx_buf[] = {iic_address, data};
+	return XIicPs_MasterSendPolled(&Iic, tx_buf, sizeof(tx_buf), MPU9250_AD);
+}
+
+static int8_t mpu9250_Read_Data(uint8_t iic_address, uint8_t length, u8 RecvBuffer []){
+	s32 err = XIicPs_MasterSendPolled(&Iic, &iic_address, 1, MPU9250_AD);
+	if(err != XST_SUCCESS)
+		return -1;
+	return XIicPs_MasterRecvPolled(&Iic, RecvBuffer, length, MPU9250_AD);
+}