I2c watchdog timer.

quad/src/quad_app/control_algorithm.c

@@ -17,7 +17,7 @@
 #define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 #define PX4FLOW_QUAL_MIN			(100)
-#define OF_OFFSET_ANGLE             (-0.62204) // -35.64 degrees
+#define OF_OFFSET_ANGLE             (0.62204) // 35.64 degrees
 
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
@@ -416,8 +416,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
 
     // Compute VRPN blocks so they can be logged
-    int outputs[1] = {ps->mixer};
-    graph_compute_nodes(graph, outputs, 1);
+    int outputs[3] = {ps->mixer, ps->of_integ_x, ps->of_integ_y};
+    graph_compute_nodes(graph, outputs, 3);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);

quad/src/quad_app/iic_utils.c

@@ -64,7 +64,7 @@ void iic0_mpu9150_stop(){
 	iic0_mpu9150_write(0x6B, 0b01000000);
 }
 
-void iic0_mpu9150_write(u8 register_addr, u8 data){
+int iic0_mpu9150_write(u8 register_addr, u8 data){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr, data};
@@ -78,10 +78,10 @@ void iic0_mpu9150_write(u8 register_addr, u8 data){
 		device_addr = MPU9150_COMPASS_ADDR;
 	}
 
-	i2c->write(i2c, device_addr, buf, 2);
+	return i2c->write(i2c, device_addr, buf, 2);
 }
 
-void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr};
@@ -96,8 +96,11 @@ void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 	}
 
 
-	i2c->write(i2c, device_addr, buf, 1);
-	i2c->read(i2c, device_addr, recv_buffer, size);
+	int status = i2c->write(i2c, device_addr, buf, 1);
+	if (!status) {
+		status = i2c->read(i2c, device_addr, recv_buffer, size);
+	}
+	return status;
 }
 
 void iic0_mpu9150_calc_mag_sensitivity(){
@@ -156,6 +159,15 @@ void iic0_mpu9150_read_mag(gam_t* gam){
 
 }
 
+static int iic_rx_timeout_failures;
+int iic_get_gam_timeout_failures() {
+	return iic_rx_timeout_failures;
+}
+static int iic_cons_timeout_failures;
+int iic_get_consecutive_gam_timeout_failures() {
+	return iic_cons_timeout_failures;
+}
+
 /**
  * Get Gyro Accel Mag (GAM) information
  */
@@ -170,7 +182,15 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
 	//Xint8 mag_data[6] = {};
 
 	//readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
-	iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	int status = iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	if (status == IIC_RX_TIMEOUT_FAILURE) {
+		iic_rx_timeout_failures++;
+		iic_cons_timeout_failures++;
+		return status;
+	} else if (status) {
+		return status;
+	}
+	iic_cons_timeout_failures = 0; // Successful read
 
 	//Calculate accelerometer data
 	raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];

quad/src/quad_app/iic_utils.h

@@ -46,6 +46,9 @@
 #define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
 #define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
 
+// Error code could be returned on an iic read if the watchdog timer triggers
+#define IIC_RX_TIMEOUT_FAILURE (-88)
+
 ///////////////////////////////////////////////////////////////////////////////
 // MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
 ///////////////////////////////////////////////////////////////////////////////
@@ -98,8 +101,8 @@
 
 void iic_set_globals(struct I2CDriver *given_i2c, struct SystemDriver *given_system);
 
-void iic0_mpu9150_write(u8 register_addr, u8 data);
-void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
+int iic0_mpu9150_write(u8 register_addr, u8 data);
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
 
 // Wake up the MPU for data collection
 // Configure Gyro/Accel/Mag
@@ -112,6 +115,8 @@ void iic0_mpu9150_calc_mag_sensitivity();
 void iic0_mpu9150_read_mag(gam_t* gam);
 void iic0_mpu9150_read_gyro_accel(gam_t* gam);
 
+int iic_get_gam_timeout_failures();
+int iic_get_consecutive_gam_timeout_failures();
 int iic0_mpu9150_read_gam(gam_t* gam);
 
 ////////////////////////////////////////////////////////////////////////////////////////////

quad/src/quad_app/log_data.c

@@ -15,6 +15,7 @@
 #include "communication.h"
 #include "computation_graph.h"
 #include "graph_blocks.h"
+#include "iic_utils.h"
 
 // Current index of the log array
 static int arrayIndex = 0;
@@ -203,6 +204,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	int i;
 	// Comment header
 	safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+	safe_sprintf_cat(&buf, "# IIC failures: %d\n", iic_get_gam_timeout_failures());
 
 	// List Pid Constants
 	for (i = 0; i < ps->graph->n_nodes; ++i) {

quad/src/quad_app/quad_app.c

@@ -42,6 +42,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 	// Main control loop
 	while (1)
 	{
+		// Kill quad if kill switch or more than 10 GAM
 		int this_kill_condition = kill_condition(&(structs.user_input_struct));
 
 		// Processing of loop timer at the beginning of the control loop
@@ -112,6 +113,12 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		// Processing of loop timer at the end of the control loop
 		timer_end_loop(&(structs.log_struct));
+
+		if (iic_get_consecutive_gam_timeout_failures() > 10) {
+			kill_motors(&(structs.hardware_struct.pwm_outputs));
+			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+			break;
+		}
 	}
 
 	kill_motors(&(structs.hardware_struct.motors));

quad/src/quad_app/sensor_processing.c

@@ -101,6 +101,8 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 
 	// Simply copy optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
+	sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
+	sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
 	return 0;
 }
 

quad/src/quad_app/timer.c

@@ -47,7 +47,7 @@ float get_last_loop_time() {
   return (float)LOOP_TIME / 1000000;
 }
 
-u32 timer_get_count() {
+u64 timer_get_count() {
   u64 time;
   return axi_timer->read(axi_timer, &time);
   return time;

quad/src/quad_app/timer.h

@@ -36,7 +36,7 @@ int timer_end_loop(log_t *log_struct);
 // Returns the number of seconds the last loop took
 float get_last_loop_time();
 
-u32 timer_get_count();
+u64 timer_get_count();
 
 void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
 #endif /* TIMER_H_ */

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c

 #include "hw_impl_zybo.h"
 #include "iic_utils.h"
 #include "xiicps.h"
+#include "timer.h"
 
 #define IO_CLK_CONTROL_REG_ADDR	(0xF800012C)
 
@@ -266,12 +267,26 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 			 ByteCount);
 	}
 
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+	u32 iic_freq = XIicPs_GetSClk(InstancePtr);
+	// (1000000 * 9 / iic_freq) is the number of microseconds required to send 1 byte of data
+	// Using 5 times as an upper bound
+	u32 max_usec_per_byte = 5 * 1000000 * 9 / iic_freq;
+	u64 start_time = timer_get_count();
+/* <--- End hack ---> */
+
 	/*
 	 * Pull the interrupt status register to find the errors.
 	 */
 	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
 	while ((InstancePtr->RecvByteCount > 0) &&
 			((IntrStatusReg & Intrs) == 0) && !(IntrStatusReg & XIICPS_IXR_COMP_MASK)) {
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+		u64 usec_passed = timer_get_count() - start_time;
+		if (usec_passed > max_usec_per_byte * (ByteCount - InstancePtr->RecvByteCount)) {
+			return IIC_RX_TIMEOUT_FAILURE;
+		}
+/* <--- End hack ---> */
 		StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
 
 		/*

quad/xsdk_workspace/real_quad/src/main.c

@@ -39,5 +39,11 @@ int main()
 
   // Run the main quad application
   quad_main(setup_hardware);
+
+  while(1) {
+	  volatile int i = 0;
+	  i++;
+  }
+
   return 0;
 }