Keeping track of number of IIC errors.

quad/src/quad_app/quad_app.c

@@ -49,7 +49,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		timer_start_loop();
 
 		// Process all received data
-
 		process_received(&structs);
 
 		// Get the user input and put it into user_input_struct

quad/src/quad_app/sensor.c

@@ -10,6 +10,9 @@
 #include "commands.h"
 #include "type_def.h"
 
+//TODO: Put this in a dedicated error-handling file
+static void updateError(SensorError_t*, int);
+
 int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 {
   struct IMUDriver *imu = &hardware_struct->imu;
@@ -40,19 +43,32 @@ int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* us
   struct IMUDriver *imu = &hardware_struct->imu;
   struct LidarDriver *lidar = &hardware_struct->lidar;
   struct OpticalFlowDriver *of = &hardware_struct->of;
+  static lidar_t lidar_val;
   int status = 0;
 
-  status |= imu->read(imu, &raw_sensor_struct->gam);
-  static lidar_t lidar_val;
-  int lidar_status = lidar->read(lidar, &lidar_val);
-  if (lidar_status == 0) {
+  status = imu->read(imu, &raw_sensor_struct->gam);
+  updateError(&(raw_sensor_struct->gam.error), status);
+
+  status = lidar->read(lidar, &lidar_val);
+  updateError(&(raw_sensor_struct->lidar.error), status);
+  if (status == 0) {
 		raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
   }
-  status |= lidar_status;
   
-  status |= of->read(of, &raw_sensor_struct->optical_flow);
+  status = of->read(of, &raw_sensor_struct->optical_flow);
+  updateError(&(raw_sensor_struct->optical_flow.error), status);
+
   log_struct->gam = raw_sensor_struct->gam;
 
   return 0;
 }
  
+void updateError(SensorError_t *error, int status) {
+	if(status) {
+		error->errorCount++;
+		error->consErrorCount++;
+	}
+	else {
+		error->consErrorCount = 0;
+	}
+}