Not running optical flow lowpass filter. Logging IIC failures. Disabled LiDAR.

quad/src/quad_app/log_data.c

@@ -126,6 +126,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addParamToLog(log_struct, ps->x_set, CONST_VAL, m);
 	addParamToLog(log_struct, ps->y_set, CONST_VAL, m);
 	addParamToLog(log_struct, ps->alt_set, CONST_VAL, m);
+	addParamToLog(log_struct, ps->yaw_set, CONST_VAL, m);
 	addParamToLog(log_struct, ps->lidar, CONST_VAL, m);
 	addParamToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
 	addParamToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
@@ -185,7 +186,7 @@ int log_data(log_t* log_struct, parameter_t* ps)
  * TODO: This should probably be transmitting in binary instead of ascii
  */
 
-void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps){
+void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors){
 	if (arrayIndex == 0) {
 		// Don't send any logs if nothing was logged
 		return;
@@ -196,14 +197,16 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	// Let user know that allocation failed
 	if (arraySize != LOG_STARTING_SIZE) {
 		safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
-		send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
+		send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 		return;
 	}
 
 	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: I'm not implemented yet\n");
+	safe_sprintf_cat(&buf, "# IMU IIC failures: %d\n", raw_sensors->gam.error.consErrorCount);
+	safe_sprintf_cat(&buf, "# LiDAR IIC failures: %d\n", raw_sensors->lidar.error.consErrorCount);
+	safe_sprintf_cat(&buf, "# Optical Flow IIC failures: %d\n", raw_sensors->optical_flow.error.consErrorCount);
 
 	// List Pid Constants
 	for (i = 0; i < ps->graph->n_nodes; ++i) {
@@ -237,7 +240,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	safe_sprintf_cat(&buf, "\n");
 
 	// Send header names
-	send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
+	send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 
 	// Send units header
 	buf.size = 0;
@@ -245,17 +248,17 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	safe_sprintf_cat(&buf, units_output.str);
 	safe_sprintf_cat(&buf, units_param.str);
 	safe_sprintf_cat(&buf, "\n");
-	send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
+	send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 
 	/*************************/
 	/* print & send log data */
 	for(i = 0; i < arrayIndex; i++){
 		format_log(i, log_struct, &buf);
-		send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
+		send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 	}
 
 	// Empty message of type LOG_END to indicate end of log
-	send_data(&hardware_struct->uart, LOG_END_ID, 0, buf.str, 0);
+	send_data(&hardware_struct->uart, LOG_END_ID, 0, (u8*)buf.str, 0);
 }
 
 void resetLogging() {

quad/src/quad_app/log_data.h

@@ -47,7 +47,7 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
  /**
   * Prints all the log information.
   */
- void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps);
+ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors);
 
  /**
   * Resets and clears logged data

quad/src/quad_app/quad_app.c

@@ -103,7 +103,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		if (this_kill_condition == 1 && last_kill_condition == 0) {
 		  // Just disabled
-		  printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+		  printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 		  resetLogging();
 		  MIO7_led_off();
 		}
@@ -115,7 +115,9 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		if (structs.raw_sensor_struct.gam.error.consErrorCount > 10) {
 			kill_motors(&(structs.hardware_struct.motors));
-			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+			char err_msg[] = "More than 10 IMU errors";
+			send_data(&structs.hardware_struct.uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
+			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 			break;
 		}
 	}

quad/src/quad_app/sensor.c

@@ -22,9 +22,9 @@ int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, s
   if (imu->reset(imu, &raw_sensor_struct->gam)) {
     return -1;
   }
-  if (lidar->reset(lidar, &raw_sensor_struct->lidar)) {
-     return -1;
-  }
+  //if (lidar->reset(lidar, &raw_sensor_struct->lidar)) {
+  //   return -1;
+  //}
   if (of->reset(of, &raw_sensor_struct->optical_flow)) {
      return -1;
   }
@@ -49,11 +49,11 @@ int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* us
   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->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 = of->read(of, &raw_sensor_struct->optical_flow);
   updateError(&(raw_sensor_struct->optical_flow.error), status);

quad/src/quad_app/sensor_processing.c

@@ -109,8 +109,10 @@ 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 = biquad_execute(&sensor_struct->flow_x_filt, -sensor_struct->optical_flow.xVel);
-	sensor_struct->optical_flow.yVel = biquad_execute(&sensor_struct->flow_y_filt, -sensor_struct->optical_flow.yVel);
+	//sensor_struct->optical_flow.xVel = biquad_execute(&sensor_struct->flow_x_filt, -sensor_struct->optical_flow.xVel);
+	//sensor_struct->optical_flow.yVel = biquad_execute(&sensor_struct->flow_y_filt, -sensor_struct->optical_flow.yVel);
+	sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
+	sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
 	return 0;
 }
 

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c

 #include "hw_impl_zybo.h"
 #include "xiicps.h"
-#include "timer.h"
-
 // System configuration registers
 // (Please see Appendix B: System Level Control Registers in the Zybo TRM)
 #define IIC_SYSTEM_CONTROLLER_RESET_REG_ADDR 	(0xF8000224)
@@ -225,6 +223,13 @@ int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 	return XST_SUCCESS;
 }
 
+static XTime get_time() {
+    XTime time;
+    XTime_GetTime(&time);
+    u64 us = (u64)time * (1000000 / COUNTS_PER_SECOND); // (ticks)(1000000us/s)(s/ticks)
+    return us;
+}
+
 /*****************************************************************************/
 /**
 * This function initiates a polled mode receive in master mode.
@@ -303,7 +308,7 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 	// (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();
+	u64 start_time = get_time();
 /* <--- End hack ---> */
 
 	/*
@@ -313,7 +318,7 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 	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;
+		u64 usec_passed = get_time() - start_time;
 		// Add 1 so it has a chance to read
 		if (usec_passed > max_usec_per_byte * (1 + ByteCount - InstancePtr->RecvByteCount)) {
 			return IIC_RX_TIMEOUT_FAILURE;

quad/xsdk_workspace/real_quad/src/main.c

@@ -29,9 +29,9 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
-  test_zybo_i2c_imu();
+  //test_zybo_i2c_imu();
   //test_zybo_i2c_px4flow();
-  //test_zybo_i2c_all();
+  test_zybo_i2c_all();
   //test_zybo_rc_receiver();
   //test_zybo_motors();
   //test_zybo_uart();