From 2cffb0a4019271a184dae82d3ee04b1e4b6592c2 Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Sat, 22 Apr 2017 00:38:16 -0500
Subject: [PATCH] wip: more changes
---
quad/src/quad_app/communication.c | 61 ++++++++++---------
quad/src/quad_app/hw_iface.h | 13 ++--
quad/src/quad_app/log_data.c | 12 ++--
quad/src/quad_app/log_data.h | 2 +-
quad/src/quad_app/quad_app.c | 6 +-
.../real_quad/src/hw_impl_zybo.c | 30 +++------
.../real_quad/src/hw_impl_zybo_i2c.c | 16 ++---
quad/xsdk_workspace/real_quad/src/main.c | 17 ++++++
8 files changed, 79 insertions(+), 78 deletions(-)
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index aa7561a20..210ae1c6b 100644
--- a/quad/src/quad_app/communication.c
+++ b/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;
}
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 38dbe6dad..66eac0541 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/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
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index cd724d2b2..2a29ddb8f 100644
--- a/quad/src/quad_app/log_data.c
+++ b/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() {
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index c3df9ec19..9c4b8b7d9 100644
--- a/quad/src/quad_app/log_data.h
+++ b/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
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index b6bde4fd0..106a3e038 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/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;
}
}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index 396c07c46..412478dcd 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/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;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index 27f85b241..fd436bf80 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/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;
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index d192b9c6c..599a602d3 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/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();
--
GitLab