Merge pull request #100 from RoboTeamTwente/feature/timestamps-on-sd-…

…cards Feature/timestamps on sd cards
RoboTeamTwente · Dec 13, 2023 · 27a9d90 · 27a9d90
2 parents b9ed5e1 + f6b4115
commit 27a9d90
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 6 deletions.
diff --git a/Core/Inc/roboteam_embedded_messages b/Core/Inc/roboteam_embedded_messages
diff --git a/Core/Src/robot.c b/Core/Src/robot.c
@@ -93,6 +93,8 @@ volatile uint32_t counter_TIM_SHOOT = 0;
 volatile uint32_t counter_RobotCommand = 0;
 volatile uint32_t counter_RobotBuzzer = 0;
 uint32_t timestamp_initialized = 0;
+uint64_t unix_timestamp = 0;
+bool unix_initalized = false;
 
 bool flag_send_PID_gains = false;
 bool flag_sdcard_write_feedback = false;
@@ -166,9 +168,11 @@ void Wireless_Readpacket_Cplt(void){
 	// TODO insert REM_SX1280Filler packet if total_packet_length < 6. Fine for now since feedback is already more than 6 bytes
 	WritePacket_DMA(SX, &txPacket, &Wireless_Writepacket_Cplt);
 }
+
 void Wireless_Default(){
 	WaitForPacket(SX);
 }
+
 void Wireless_RXDone(SX1280_Packet_Status *status){
   /* It is possible that random noise can trigger the syncword.
    * Correct syncword from noise have a very weak signal.
@@ -375,7 +379,7 @@ void init(void){
     dribbler_Init();
 	// TODO: Currently the ball sensor initialization is just disabled. 
 	// Since we will no longer use it anymore this should be fully removed from the code.
-    // if(ballSensor_Init()) LOG("[init:"STRINGIZE(__LINE__)"] Ballsensor initialized\n");
+    // if(ballSensor_Init()) LOG("[init:"STRINGIZE(__LINE__)"] Ballsensor initialized\n");	
 	LOG_sendAll();
 }
 
@@ -577,7 +581,6 @@ void loop(void){
 	}
 	if(flag_sdcard_write_command){
 		flag_sdcard_write_command = false;
-		activeRobotCommand.timestamp = current_time;
 		encodeREM_RobotCommand( &robotCommandPayload, &activeRobotCommand );
 		SDCard_Write(robotCommandPayload.payload, REM_PACKET_SIZE_REM_ROBOT_COMMAND, false);
 	}
@@ -819,7 +822,6 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
 	}else if(GPIO_Pin == MTi_IRQ_pin.PIN){
 		MTi_IRQ_Handler(MTi);
 	}else if (GPIO_Pin == BS_IRQ_pin.PIN){
-		// TODO: make this work and use instead of the thing in the while loop
 		ballSensor_IRQ_Handler();
 	}
 }
@@ -831,6 +833,11 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 	if(htim->Instance == TIM_CONTROL->Instance) {
 		if(!ROBOT_INITIALIZED) return;
 
+		if (!unix_initalized && activeRobotCommand.timestamp != 0){
+			unix_timestamp = activeRobotCommand.timestamp;
+			unix_initalized = true;
+		}
+
 		counter_TIM_CONTROL++;
 
 		// Update the dribbler at 10Hz
@@ -895,7 +902,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 		wheels_Update();
 
 		/* == Fill robotFeedback packet == */ {
-			robotFeedback.timestamp = current_time;
+			robotFeedback.timestamp = unix_timestamp;
 			robotFeedback.XsensCalibrated = xsens_CalibrationDone;
 			// robotFeedback.batteryLevel = (batCounter > 1000);
 			robotFeedback.ballSensorWorking = ballSensor_isInitialized();
@@ -915,7 +922,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 		}
 
 		/* == Fill robotStateInfo packet == */ {	
-			robotStateInfo.timestamp = current_time;	
+			robotStateInfo.timestamp = unix_timestamp;
 			robotStateInfo.xsensAcc1 = stateInfo.xsensAcc[0];
 			robotStateInfo.xsensAcc2 = stateInfo.xsensAcc[1];
 			robotStateInfo.xsensYaw = yaw_GetCalibratedYaw();
@@ -934,6 +941,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 		}
 
 		flag_sdcard_write_feedback = true;
+
 	}
 	else if (htim->Instance == TIM_BUZZER->Instance) {
 		counter_TIM_BUZZER++;
@@ -944,4 +952,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 		counter_TIM_SHOOT++;
 		shoot_Callback();
 	}
+
+	unix_timestamp += 1	;
 }