debug error handling

1 files changed, 20 insertions, 3 deletions

diff --git a/robot/sim.c b/robot/sim.c
index 8fc8d00..3f7f686 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -1,15 +1,23 @@
 #include <stdio.h>
+#include <time.h>
+#include <string.h>
 
 #include "sim.h"
 
+struct timespec reference_time; // NOLINT
+
 void time_reset() {
+	// printf("SIM: time_reset()\n");
+	clock_gettime(CLOCK_MONOTONIC, &reference_time);
 	return;
-	printf("SIM: time_reset()\n");
 }
 
 unsigned long get_ms() {
-	printf("SIM: get_ms()\n");
-	return 0;
+	// printf("SIM: get_ms()\n");
+	struct timespec elapsed;
+	clock_gettime(CLOCK_MONOTONIC, &elapsed);
+	return ((elapsed.tv_sec * 1000) + (elapsed.tv_nsec / 1000000)) -
+		((reference_time.tv_sec * 1000) + (reference_time.tv_nsec / 1000000));
 }
 
 void red_led(unsigned char on) {
@@ -36,3 +44,12 @@ void serial_set_baud_rate(unsigned int rate) {
 	printf("SIM: serial_set_baud_rate(%u)\n", rate);
 	return;
 }
+
+// TODO: hexdump binary data
+void serial_send(char* message, unsigned int length) {
+	char message_copy[length];
+	strncpy(message_copy, message, length);
+	printf("SIM: serial_send(\"%s\", %u)\n", message_copy, length);
+	return;
+}
+