wip: more changes

quad/src/quad_app/communication.c

@@ -96,7 +96,7 @@ int process_packet(modular_structs_t *structs) {
 
 void process_received(modular_structs_t *structs) {
   // Parse as many packets as possible
-  struct UARTDriver *uart = &structs->hardware_struct.uart;
+  struct UARTDriver *uart = &structs->hardware_struct.comm->uart;
   while (!try_receive_packet(uart)) {
     process_packet(structs);
   }
@@ -111,33 +111,34 @@ int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t
 	//	   bytes||	   1	|	       2             |   2    |        2         | var	|	 1	   |
 	//----------------------------------------------------------------------------------------------
 
-	unsigned char formattedHeader[PACKET_HEADER_SIZE];
-
-	// Begin Char:
-	formattedHeader[0] = BEGIN_CHAR;
-	// Msg type 2 bytes:
-	formattedHeader[1] = type_id & 0x000000ff;
-	formattedHeader[2] = (type_id >> 8) & 0x000000ff;
-	// Msg id 2 bytes
-	formattedHeader[3] = msg_id & 0x000000ff;
-	formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
-	// Data length and data - bytes 5&6 for len, 7+ for data
-	formattedHeader[5] = size & 0x000000ff;
-	formattedHeader[6] = (size >> 8) & 0x000000ff;
-
-	// Compute checksum
-	unsigned char packet_checksum = 0;
-	int i;
-	for(i = 0; i < PACKET_HEADER_SIZE; i++) {
-		packet_checksum ^= formattedHeader[i];
-		uart->write(uart, formattedHeader[i]);
-	}
-	for (i = 0; i < size; i++) {
-		packet_checksum ^= data[i];
-		uart->write(uart, data[i]);
-	}
-
-	uart->write(uart, packet_checksum);
-
-	return 0;
+  
+  unsigned char formattedHeader[PACKET_HEADER_SIZE];
+
+  // Begin Char:
+  formattedHeader[0] = BEGIN_CHAR;
+  // Msg type 2 bytes:
+  formattedHeader[1] = type_id & 0x000000ff;
+  formattedHeader[2] = (type_id >> 8) & 0x000000ff;
+  // Msg id 2 bytes
+  formattedHeader[3] = msg_id & 0x000000ff;
+  formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
+  // Data length and data - bytes 5&6 for len, 7+ for data
+  formattedHeader[5] = size & 0x000000ff;
+  formattedHeader[6] = (size >> 8) & 0x000000ff;
+
+  // Compute checksum
+  unsigned char packet_checksum = 0;
+  int i;
+  for(i = 0; i < PACKET_HEADER_SIZE; i++) {
+    packet_checksum ^= formattedHeader[i];
+    uart->write(uart, formattedHeader[i]);
+  }
+  for (i = 0; i < size; i++) {
+    packet_checksum ^= data[i];
+    uart->write(uart, data[i]);
+  }
+
+  uart->write(uart, packet_checksum);
+
+  return 0;
 }

quad/src/quad_app/hw_iface.h

@@ -37,7 +37,6 @@ struct MotorDriver {
 
 struct UARTDriver {
   void *state;
-  int devId;
   int (*reset)(struct UARTDriver *self);
   int (*write)(struct UARTDriver *self, unsigned char c);
   int (*read)(struct UARTDriver *self, unsigned char *c);
@@ -65,7 +64,6 @@ struct SystemDriver {
 
 struct I2CDriver {
   void *state;
-  int busId;
   int (*reset)(struct I2CDriver *self);
   int (*write)(struct I2CDriver *self,
                unsigned short device_addr,
@@ -96,16 +94,13 @@ struct OpticalFlowDriver {
 };
 
 struct GPSDriver {
-	struct UARTDriver * uart;
-	int (*reset)(struct GPSDriver *self, struct gps *gps);
-	int (*read)(struct GPSDriver *self, struct gps *gps);
+  struct UARTDriver *uart;
+  int (*reset)(struct GPSDriver *self, struct gps *gps);
+  int (*read)(struct GPSDriver *self, struct gps *gps);
 };
 
 struct CommDriver {
-	struct UARTDriver * uart;
-	int (*reset)(struct CommDriver *self);
-	int (*write)(struct CommDriver *self, unsigned char c);
-	int (*read)(struct CommDriver *self, unsigned char *c);
+  struct UARTDriver *uart;
 };
 
 #endif

quad/src/quad_app/log_data.c

@@ -186,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, raw_sensor_t* raw_sensors){
+void printLogging(struct CommDriver *comm, 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;
@@ -197,7 +197,7 @@ 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, (u8*)buf.str, buf.size);
+		send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 		return;
 	}
 
@@ -240,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, (u8*)buf.str, buf.size);
+	send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 
 	// Send units header
 	buf.size = 0;
@@ -248,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, (u8*)buf.str, buf.size);
+	send_data(comm->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, (u8*)buf.str, buf.size);
+		send_data(comm->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, (u8*)buf.str, 0);
+	send_data(comm->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, raw_sensor_t* raw_sensors);
+ void printLogging(struct CommDriver *comm, 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), &structs.raw_sensor_struct);
+		  printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 		  resetLogging();
 		  MIO7_led_off();
 		}
@@ -116,8 +116,8 @@ 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));
 			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);
+			send_data(structs.hardware_struct.comm->uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
+			printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 			break;
 		}
 	}

quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c

@@ -2,7 +2,6 @@
 
 struct UARTDriver create_zybo_uart(int devId) {
   struct UARTDriver uart;
-  uart.devId = devId;
   uart.state = NULL;
   uart.reset = zybo_uart_reset;
   uart.write = zybo_uart_write;
@@ -11,31 +10,19 @@ struct UARTDriver create_zybo_uart(int devId) {
 }
 
 struct GPSDriver create_zybo_gps(struct UARTDriver *uart) {
-	struct GPSDriver gps;
-	gps.uart = uart;
-	gps.reset = zybo_gps_reset;
-	gps.read = zybo_gps_read;
-	return gps;
+  struct GPSDriver gps;
+  gps.uart = uart;
+  gps.reset = zybo_gps_reset;
+  gps.read = zybo_gps_read;
+  return gps;
 }
 
 struct CommDriver create_zybo_comm(struct UARTDriver *uart) {
-	struct CommDriver comm;
-	comm.uart = uart;
-	comm.reset = zybo_comm_reset;
-	comm.read = zybo_comm_read;
-	return comm;
+  struct CommDriver comm;
+  comm.uart = uart;
+  return comm;
 }
 
-/*
- * struct LidarDriver create_zybo_lidar(struct I2CDriver *i2c) {
-  struct LidarDriver lidar;
-  lidar.i2c = i2c;
-  lidar.reset = zybo_lidar_reset;
-  lidar.read = zybo_lidar_read;
-  return lidar;
-}
- */
-
 struct MotorDriver create_zybo_motors() {
   struct MotorDriver motors;
   motors.state = NULL;
@@ -54,7 +41,6 @@ struct RCReceiverDriver create_zybo_rc_receiver() {
 
 struct I2CDriver create_zybo_i2c(int busId) {
   struct I2CDriver i2c;
-  i2c.busId = busId;
   i2c.state = NULL;
   i2c.reset = zybo_i2c_reset;
   i2c.write = zybo_i2c_write;

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c

@@ -42,16 +42,18 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 				int ByteCount, u16 SlaveAddr);
 int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
 
-int zybo_i2c_reset(struct I2CDriver *self) {
-  int i2cID = self->busId;
-  if(i2cID > 1) {
-	  return -1;
-  }
+struct ZyboI2CState {
+  XIcPs *inst;
+  int busId;
+};
 
+int zybo_i2c_reset(struct I2CDriver *self) {
   if (self->state == NULL) {
-    self->state = malloc(sizeof(XIicPs));
+    self->state = malloc(sizeof(struct ZyboI2CState));
+    self->state->inst = malloc(sizeof(XIcPs));
   }
-  XIicPs *inst = self->state;
+  int i2cID = self->state.busId;
+  XIicPs *inst = self->state->inst;
 
   //Make sure CPU_1x clk is enabled fostatusr I2C controller
   u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;

quad/xsdk_workspace/real_quad/src/main.c

@@ -6,11 +6,28 @@
 
 #define RUN_TESTS
 
+/**
+ * Create the hardware drivers, and place them on the hardware struct.
+ *
+ * Things to keep in mind:
+ * - The CommDriver requires a UART. Hence, make the a UARTDriver first
+ *   and then give that instance to the CommDriver.
+ * - Same for GPSDriver, except it needs a different hardware device.
+ * - The I2C devices (IMU, LIDAR, Optical Flow), need a I2C bus. Hence
+ *   make a I2CDriver first, then give the appropariate instance the
+ *   device drivers.
+ *   - Currently, we are putting the Lidar on its own bus. Apparently,
+ *     when looking at an OScope with the Lidar on a bus with other
+ *     devices, it does not look right. The SCL line would never
+ *     go all the way back down to 0 volts
+ */
 int setup_hardware(hardware_t *hardware) {
   hardware->rc_receiver = create_zybo_rc_receiver();
   hardware->motors = create_zybo_motors();
   hardware->uart_0 = create_zybo_uart(0);
   hardware->uart_1 = create_zybo_uart(1);
+  hardware->comm = create_zybo_comm(&hardware->uart_0);
+  hardware->gps = create_zybo_gps(&hardware->uart_1);
   hardware->global_timer = create_zybo_global_timer();
   hardware->axi_timer = create_zybo_axi_timer();
   hardware->mio7_led = create_zybo_mio7_led();