diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index cffb7fc05bcf5b280cc45ac26b6902fe2686f08c..1dd5dc9e888b5ecce863f1870d6d31a94c3cbc82 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -80,13 +80,17 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 
 	// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
 	row_size = n_outputs + n_params + 6 + 1;
-	logArray = malloc(sizeof(float) * row_size * LOG_STARTING_SIZE);
+	size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
+	logArray = malloc(needed_memory);
+	if (!logArray) { // malloc failed
+		arraySize = 0;
+	}
 }
 
 
 int log_data(log_t* log_struct, parameter_t* ps)
 {
-	if(arrayIndex >= arraySize - 1)
+	if(arrayIndex >= arraySize)
 	{
 		return 1;
 	}
@@ -126,6 +130,13 @@ void printLogging(log_t* log_struct, parameter_t* ps){
 	char header1[256] = {};
 	char header2[1024] = {};
 
+	// Let user know that allocation failed
+	if (arraySize != LOG_STARTING_SIZE) {
+		size_t send_len = sprintf(header1, "Failed to allocate enough log memory\n");
+		send_data(LOG_ID, 0, header1, send_len);
+		return;
+	}
+
 	sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z");
 
 	int end = 0;