From 8811d36e17f0a721fca93c73d001c4cca7aba487 Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Mon, 27 Feb 2017 09:45:56 -0600
Subject: [PATCH] wip: Separate Xilinx code into hardware interfaces
---
quad/.gitignore | 3 +
quad/lib/queue/.gitignore | 1 +
quad/lib/queue/Makefile | 13 +
quad/lib/queue/queue.c | 68 +++
quad/lib/queue/queue.h | 25 +
quad/lib/queue/test_queue.c | 143 ++++++
quad/lib/test/Makefile | 14 +-
quad/sw/.gitignore | 2 +-
quad/sw/imu_logger/src/iic_mpu9150_utils.h | 11 +-
quad/sw/modular_quad_pid/src/callbacks.c | 60 +--
quad/sw/modular_quad_pid/src/callbacks.h | 4 +-
quad/sw/modular_quad_pid/src/communication.c | 222 +++------
quad/sw/modular_quad_pid/src/communication.h | 11 +-
quad/sw/modular_quad_pid/src/controllers.c | 1 -
quad/sw/modular_quad_pid/src/gam.h | 2 -
.../modular_quad_pid/src/hardware/hw_iface.h | 50 ++
.../src/hardware/hw_impl_zybo.h | 51 ++
.../src/hardware/hw_impl_zybo_axi_timer.c | 23 +
.../src/hardware/hw_impl_zybo_global_timer.c | 18 +
.../src/hardware/hw_impl_zybo_i2c.c | 231 +++++++++
.../src/hardware/hw_impl_zybo_mio7_led.c | 22 +
.../src/hardware/hw_impl_zybo_pwm_input.c | 32 ++
.../src/hardware/hw_impl_zybo_pwm_output.c | 52 ++
.../src/hardware/hw_impl_zybo_uart.c | 290 +++++++++++
quad/sw/modular_quad_pid/src/hardware/queue.c | 1 +
quad/sw/modular_quad_pid/src/hardware/queue.h | 1 +
quad/sw/modular_quad_pid/src/iic_utils.c | 228 +--------
quad/sw/modular_quad_pid/src/iic_utils.h | 6 +-
.../src/initialize_components.c | 104 ++--
.../src/initialize_components.h | 6 +-
quad/sw/modular_quad_pid/src/log_data.c | 7 +-
quad/sw/modular_quad_pid/src/log_data.h | 2 +-
quad/sw/modular_quad_pid/src/main.c | 21 +-
quad/sw/modular_quad_pid/src/mio7_led.c | 54 +-
quad/sw/modular_quad_pid/src/mio7_led.h | 15 +-
quad/sw/modular_quad_pid/src/new_log_data.h | 1 -
quad/sw/modular_quad_pid/src/old_log_data.h | 1 -
.../modular_quad_pid/src/packet_processing.c | 1 -
.../src/send_actuator_commands.c | 15 +-
.../src/send_actuator_commands.h | 2 +-
quad/sw/modular_quad_pid/src/sensor.h | 1 -
quad/sw/modular_quad_pid/src/timer.c | 76 ++-
quad/sw/modular_quad_pid/src/timer.h | 13 +-
quad/sw/modular_quad_pid/src/type_def.h | 34 +-
quad/sw/modular_quad_pid/src/uart.c | 381 --------------
quad/sw/modular_quad_pid/src/uart.h | 82 ---
quad/sw/modular_quad_pid/src/uart_buff.c | 417 ----------------
quad/sw/modular_quad_pid/src/uart_buff.h | 29 --
quad/sw/modular_quad_pid/src/user_input.c | 31 +-
quad/sw/modular_quad_pid/src/user_input.h | 2 +-
quad/sw/modular_quad_pid/src/util.c | 471 +-----------------
quad/sw/modular_quad_pid/src/util.h | 97 +---
52 files changed, 1383 insertions(+), 2065 deletions(-)
create mode 100644 quad/lib/queue/.gitignore
create mode 100644 quad/lib/queue/Makefile
create mode 100644 quad/lib/queue/queue.c
create mode 100644 quad/lib/queue/queue.h
create mode 100644 quad/lib/queue/test_queue.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_iface.h
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo.h
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_axi_timer.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_global_timer.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_i2c.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_mio7_led.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_input.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_output.c
create mode 100644 quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_uart.c
create mode 120000 quad/sw/modular_quad_pid/src/hardware/queue.c
create mode 120000 quad/sw/modular_quad_pid/src/hardware/queue.h
delete mode 100644 quad/sw/modular_quad_pid/src/uart.c
delete mode 100644 quad/sw/modular_quad_pid/src/uart.h
delete mode 100644 quad/sw/modular_quad_pid/src/uart_buff.c
delete mode 100644 quad/sw/modular_quad_pid/src/uart_buff.h
diff --git a/quad/.gitignore b/quad/.gitignore
index 53a35d022..47696c7b6 100644
--- a/quad/.gitignore
+++ b/quad/.gitignore
@@ -1,3 +1,6 @@
zybo_fsbl_bsp/
zybo_fsbl/
test.log
+inc/
+obj/
+TAGS
\ No newline at end of file
diff --git a/quad/lib/queue/.gitignore b/quad/lib/queue/.gitignore
new file mode 100644
index 000000000..30d74d258
--- /dev/null
+++ b/quad/lib/queue/.gitignore
@@ -0,0 +1 @@
+test
\ No newline at end of file
diff --git a/quad/lib/queue/Makefile b/quad/lib/queue/Makefile
new file mode 100644
index 000000000..25a22026d
--- /dev/null
+++ b/quad/lib/queue/Makefile
@@ -0,0 +1,13 @@
+OBJ = ../../obj
+INC = ../../inc
+
+queue: queue.h queue.c
+ [ -d $(OBJ) ] || mkdir -p $(OBJ)
+ [ -d $(INC) ] || mkdir -p $(INC)
+ gcc -c -o $(OBJ)/libqueue.a queue.c
+ cp queue.h $(INC)/
+
+.PHONY : test
+test: queue test_queue.c
+ gcc -o test test_queue.c -I$(INC) -L$(OBJ) -lqueue -ltest
+ ./test
diff --git a/quad/lib/queue/queue.c b/quad/lib/queue/queue.c
new file mode 100644
index 000000000..3eb19fc2d
--- /dev/null
+++ b/quad/lib/queue/queue.c
@@ -0,0 +1,68 @@
+#include "queue.h"
+
+struct Queue queue_create(unsigned int max_size) {
+ struct Queue q;
+ q.buff = malloc(sizeof(char) * (max_size + 1));
+ q.max_size = max_size + 1;
+ q.start = max_size;
+ q.end = 0;
+ return q;
+}
+
+int queue_init(struct Queue *q, unsigned int max_size) {
+ q->buff = malloc(sizeof(unsigned char) * (max_size + 1));
+ if (q->buff == NULL) return -1;
+ q->max_size = max_size + 1; // required for start/end pointers
+ q->start = max_size;
+ q->end = 0;
+ return 0;
+}
+
+void queue_destroy(struct Queue *q) {
+ free(q->buff);
+}
+
+int queue_add(volatile struct Queue *q, unsigned char c) {
+ if (queue_full(q)) {
+ return -1;
+ }
+ q->buff[q->end] = c;
+ q->end += 1;
+ q->end %= q->max_size;
+ return 0;
+}
+
+int queue_remove(volatile struct Queue *q, unsigned char *c) {
+ if (queue_empty(q)) {
+ return -1;
+ }
+ q->start += 1;
+ q->start %= q->max_size;
+ *c = q->buff[q->start];
+ return 0;
+}
+
+int queue_size(volatile struct Queue *q) {
+ return (q->max_size + q->end - q->start - 1) % q->max_size;
+}
+
+int queue_full(volatile struct Queue *q) {
+ return q->start == q->end;;
+}
+
+int queue_empty(volatile struct Queue *q) {
+ return queue_size(q) == 0;
+}
+
+void queue_print(struct Queue *q) {
+ int i;
+ printf("buffer contents:\n");
+ for (i = 0; i < q->max_size; i += 1) {
+ char c = (char) q->buff[i];
+ if (c == 0) c = '-';
+ printf("%c", c);;
+ if (i == q->start) printf(" <- start");
+ if (i == q->end) printf(" <- end");
+ printf("\n");
+ }
+}
diff --git a/quad/lib/queue/queue.h b/quad/lib/queue/queue.h
new file mode 100644
index 000000000..8e3461844
--- /dev/null
+++ b/quad/lib/queue/queue.h
@@ -0,0 +1,25 @@
+#ifndef QUEUE_H
+#define QUEUE_H
+
+#include <stdlib.h>
+#include <stdio.h>
+
+struct Queue {
+ unsigned char *buff;
+ unsigned int max_size;
+ unsigned int start;
+ unsigned int end;
+};
+
+
+struct Queue queue_create(unsigned int max_size);
+int queue_init(struct Queue *q, unsigned int max_size);
+void queue_destory(struct Queue *q);
+int queue_add(volatile struct Queue *q, unsigned char c);
+int queue_remove(volatile struct Queue *q, unsigned char *c);
+int queue_size(volatile struct Queue *q);
+int queue_full(volatile struct Queue *q);
+int queue_empty(volatile struct Queue *q);
+void queue_print(struct Queue *q);
+
+#endif
diff --git a/quad/lib/queue/test_queue.c b/quad/lib/queue/test_queue.c
new file mode 100644
index 000000000..58b9edc8f
--- /dev/null
+++ b/quad/lib/queue/test_queue.c
@@ -0,0 +1,143 @@
+#include "test.h"
+#include "queue.h"
+
+int test_add() {
+ puts("---");
+ puts("test_add");
+ int failed = 0;
+ struct Queue *q = malloc(sizeof(struct Queue));
+ queue_init(q, 5);
+ puts("initialized");
+ unsigned char c;
+ failed = failed || queue_add(q, 'a');
+ return failed;
+}
+
+int test_size() {
+ puts("---");
+ puts("test_size");
+ int failed = 0;
+ struct Queue *q = malloc(sizeof(struct Queue));
+ queue_init(q, 4);
+ unsigned char c;
+ failed = failed || queue_size(q) != 0;
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+
+ queue_add(q, 'a');
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 1;
+
+ queue_add(q, 'b');
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 2;
+
+ queue_add(q, 'c');
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 3;
+
+ queue_add(q, 'd');
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 4;
+
+ queue_remove(q, &c);
+ queue_remove(q, &c);
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 2;
+
+ queue_add(q, 'e');
+
+ printf("size: %d\n", queue_size(q));
+ queue_print(q);
+ failed = failed || queue_size(q) != 3;
+
+ return failed;
+}
+
+int test_full_when_full() {
+ puts("---");
+ puts("test_full_when_full");
+ int failed = 0;
+ struct Queue *q = malloc(sizeof(struct Queue));
+ queue_init(q, 4);
+ unsigned char c;
+ queue_add(q, 'a');
+ queue_add(q, 'b');
+ queue_add(q, 'c');
+ queue_add(q, 'd');
+ failed = failed || !queue_full(q);
+ return failed;
+}
+
+int test_add_when_full() {
+ puts("---");
+ puts("test_add_when_full");
+ int failed = 0;
+ struct Queue *q = malloc(sizeof(struct Queue));
+ queue_init(q, 4);
+ unsigned char c;
+ queue_add(q, 'a');
+ queue_add(q, 'b');
+ queue_add(q, 'c');
+ queue_add(q, 'd');
+ queue_print(q);
+ failed = failed || !queue_add(q, 'x');
+ queue_remove(q, &c);
+ queue_print(q);
+ failed = failed || !(c == 'a');
+ queue_remove(q, &c);
+ queue_print(q);
+ failed = failed || !(c == 'b');
+ queue_remove(q, &c);
+ queue_print(q);
+ failed = failed || !(c == 'c');
+ queue_remove(q, &c);
+ queue_print(q);
+ failed = failed || !(c == 'd');
+ return failed;
+}
+
+int test_add_over_edge() {
+ int failed = 0;
+ struct Queue *q = malloc(sizeof(struct Queue));
+ queue_init(q, 4);
+ unsigned char c;
+ queue_add(q, 'x');
+ queue_add(q, 'x');
+ queue_remove(q, &c);
+ queue_remove(q, &c);
+ queue_add(q, 'a');
+ queue_add(q, 'b');
+ queue_add(q, 'c');
+ queue_add(q, 'd');
+ queue_print(q);
+ failed = failed || !queue_add(q, 'x');
+ queue_remove(q, &c);
+ failed = failed || !(c == 'a');
+ queue_remove(q, &c);
+ failed = failed || !(c == 'b');
+ queue_remove(q, &c);
+ failed = failed || !(c == 'c');
+ queue_remove(q, &c);
+ failed = failed || !(c == 'd');
+ return failed;
+}
+
+int main() {
+ test(test_add, "test_add");
+ test(test_size, "test_size");
+ test(test_full_when_full, "test_full_when_full");
+ test(test_add_when_full, "test_add_when_full");
+ test(test_add_over_edge, "test_add_over_edge");
+ return test_summary();
+}
diff --git a/quad/lib/test/Makefile b/quad/lib/test/Makefile
index b7b47546a..dc7703f75 100644
--- a/quad/lib/test/Makefile
+++ b/quad/lib/test/Makefile
@@ -1,8 +1,14 @@
-test.o: test.c
- gcc -c test.c
+OBJ = ../../obj
+INC = ../../inc
-example: example.c test.c test.h
- gcc -g -o example example.c test.c test.h
+test: test.c test.h
+ [ -d $(OBJ) ] || mkdir -p $(OBJ)
+ [ -d $(INC) ] || mkdir -p $(INC)
+ gcc -c -o $(OBJ)/libtest.a test.c
+ cp test.h $(INC)
+
+example: test_lib example.c
+ gcc -g -o example example.c -I$(INC) -L$(OBJ) -ltest
.PHONY: clean
clean:
diff --git a/quad/sw/.gitignore b/quad/sw/.gitignore
index 97d34666e..250db3108 100644
--- a/quad/sw/.gitignore
+++ b/quad/sw/.gitignore
@@ -2,4 +2,4 @@ SDK.log
test.log
.metadata/
system_bsp/
-
+TAGS
\ No newline at end of file
diff --git a/quad/sw/imu_logger/src/iic_mpu9150_utils.h b/quad/sw/imu_logger/src/iic_mpu9150_utils.h
index 3c9907933..2ccb8cf39 100644
--- a/quad/sw/imu_logger/src/iic_mpu9150_utils.h
+++ b/quad/sw/imu_logger/src/iic_mpu9150_utils.h
@@ -15,9 +15,8 @@
#define IIC_MPU9150_UTILS_H
-#include "xbasic_types.h"
-#include "xiicps.h"
#include "type_def.h"
+#include "hardware/hw_iface.h"
// System configuration registers
// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
@@ -97,6 +96,8 @@
#define ACCEL_Y_BIAS 0.009f
#define ACCEL_Z_BIAS 0.087f
+void iic_set_global(struct I2CDriver *given_i2c);
+
// Initialize hardware; Call this FIRST before calling any other functions
int initI2C0();
@@ -134,14 +135,14 @@ void stop_mpu9150();
// Write a byte of data at the given register address on the MPU
-void iic0_write(Xuint16 reg_addr, Xuint8 data);
+void iic0_write(u16 reg_addr, u8 data);
// Read a single byte at a given register address on the MPU
-Xuint8 iic0_read(Xuint16 reg_addr);
+u8 iic0_read(u16 reg_addr);
// Read multiple bytes consecutively at a starting register address
// places the resulting bytes in rv
-int iic0_read_bytes(Xuint8* rv, Xuint16 reg_addr, int bytes);
+int iic0_read_bytes(u8* rv, u16 reg_addr, int bytes);
// Helper function to initialize I2C0 controller on the Zybo board
// Called by init_iic0
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 36f393434..183ecf854 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -1,8 +1,8 @@
#include "communication.h"
#include "commands.h"
#include "type_def.h"
-#include "uart.h"
#include "computation_graph.h"
+#include "util.h"
/*
* Static variables used to keep track of packet counts
@@ -16,26 +16,26 @@ static size_t total_payload_received = 0;
/**
* Currently does nothing.
*/
-int cb_debug(modular_structs_t *structs)
+int cb_debug(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
char buf[255];
// Get the node ID, parameter ID, parameter value
- u8 node_id = uart_buff_data_get_u8(0);
+ u8 node_id = data[0];
struct computation_graph* graph = structs->parameter_struct.graph;
float param_val = graph_get_output(graph, node_id, 0);
- int length = snprintf(buf, sizeof buf, "%f", param_val);
- send_data(DEBUG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ int len = snprintf(buf, sizeof buf, "%f", param_val);
+ send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
return 0;
}
/**
* counts the number of packet logs.
*/
-int cb_packetlog(modular_structs_t* structs) {
+int cb_packetlog(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
n_msg_received += 1;
- total_payload_received += uart_buff_data_length();
+ total_payload_received += length;
return 0;
}
@@ -43,38 +43,38 @@ int cb_packetlog(modular_structs_t* structs) {
* Handles a get packet logs request and sends a response
* with the packet log data.
*/
-int cb_getpacketlogs(modular_structs_t* structs) {
+int cb_getpacketlogs(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
char buf[255];
// Message logging number of messages received and size of payload received
- int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
+ int len = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
- send_data(LOG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+ send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
return 0;
}
/*
* Handles receiving new location updates.
*/
-int cb_update(modular_structs_t *structs)
+int cb_update(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
//processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
// Packet must come as [NEARPY], 4 bytes each
- int packetId = uart_buff_data_get_u32(0);
+ int packetId = build_int(data + 0);
// printf("Packet ID: %d\n", packetId);
- float y_pos = uart_buff_data_get_float(4);
+ float y_pos = build_float(data + 4);
// printf("y_pos: %f\n", y_pos);
- float x_pos = uart_buff_data_get_float(8);
+ float x_pos = build_float(data + 8);
// printf("x_pos: %f\n", x_pos);
- float alt_pos = uart_buff_data_get_float(12);
+ float alt_pos = build_float(data + 12);
// printf("alt_pos: %f\n", alt_pos);
- float roll = uart_buff_data_get_float(16);
+ float roll = build_float(data + 16);
// printf("roll: %f\n", roll);
- float pitch = uart_buff_data_get_float(20);
+ float pitch = build_float(data + 20);
// printf("pitch: %f\n", pitch);
- float yaw = uart_buff_data_get_float(24);
+ float yaw = build_float(data + 24);
// printf("yaw: %f\n", yaw);
currentQuadPosition->packetId = packetId;
@@ -94,7 +94,7 @@ int cb_update(modular_structs_t *structs)
/**
* This is called on the ground station to begin sending VRPN to the quad.
*/
-int cb_beginupdate(modular_structs_t *structs) {
+int cb_beginupdate(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length) {
structs->user_input_struct.receivedBeginUpdate = 1;
return 0;
}
@@ -117,10 +117,10 @@ int cb_beginupdate(modular_structs_t *structs) {
*
* Does not send anything in response.
*/
-int cb_setparam(modular_structs_t *structs)
+int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
// Get some of the meta data
- u16 data_len = uart_buff_data_length();
+ u16 data_len = length;
// Check if the data length is correct
if (data_len != 6)
{
@@ -129,9 +129,9 @@ int cb_setparam(modular_structs_t *structs)
struct computation_graph* graph = structs->parameter_struct.graph;
// Get the node ID, parameter ID, parameter value
- u8 node_id = uart_buff_data_get_u8(0);
- u8 param_id = uart_buff_data_get_u8(1);
- float param_val = uart_buff_data_get_float(2);
+ u8 node_id = data[0];
+ u8 param_id = data[1];
+ float param_val = build_float(data + 2);
// Set the value for that parameter on that node
graph_set_param_val(graph, node_id, param_id, param_val);
@@ -162,11 +162,11 @@ int cb_setparam(modular_structs_t *structs)
* | bytes || 1 | 1 | 4 |
* |--------------------------------------------------------|
*/
-int cb_getparam(modular_structs_t* structs)
+int cb_getparam(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length)
{
// Get some of the meta data
- u16 data_len = uart_buff_data_length();
- u16 msg_id = uart_buff_get_u16(3);
+ u16 data_len = length;
+ u16 msg_id = meta->msg_id;
// Check if the data length is correct
if (data_len != 2)
{
@@ -174,8 +174,8 @@ int cb_getparam(modular_structs_t* structs)
}
// Get the controller ID, parameter ID
- u8 node_id = uart_buff_data_get_u8(0);
- u8 param_id = uart_buff_data_get_u8(1);
+ u8 node_id = data[0];
+ u8 param_id = data[1];
struct computation_graph* graph = structs->parameter_struct.graph;
float param_val = graph_get_param_val(graph, node_id, param_id);
@@ -190,7 +190,7 @@ int cb_getparam(modular_structs_t* structs)
memcpy(&resp_data[2], ¶m_val, sizeof(param_val));
// Send the response
- send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+ send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/callbacks.h b/quad/sw/modular_quad_pid/src/callbacks.h
index 7e67ddac5..80d0bbc43 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.h
+++ b/quad/sw/modular_quad_pid/src/callbacks.h
@@ -2,10 +2,10 @@
#define __callbacks_h
/* Grab some stupid stuff from legacy code */
-
struct modular_structs;
+struct metadata;
/* Make commands.c happy */
-typedef int (command_cb)(struct modular_structs *structs);
+typedef int (command_cb)(struct modular_structs *, struct metadata *, unsigned char *, unsigned short);
#endif
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index bba31571e..54922981a 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -1,5 +1,5 @@
#include "communication.h"
-#include "uart_buff.h"
+#include "hardware/hw_iface.h"
#define INTC XScuGic
#define COMM_UART_DEVICE_ID XPAR_PS7_UART_0_DEVICE_ID
@@ -12,6 +12,7 @@
// plus 128 for a little buffer
// (bit/s) * (seconds) / (10 bits / byte for UART)
#define MAX_PACKET_SIZE 256
+#define PACKET_HEADER_SIZE 7
#define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
//#define INTERRUPT_BENCHMARK
@@ -19,154 +20,75 @@
// Declaration of interrupt handler
void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
-// Pointer to the UART driver instance
-static XUartPs* uartInstPtr;
-
-
-int initUartComms() {
- uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
- // Initialize UART0 (Bluetooth/WiFi)
- if(!uartInstPtr) {
- return -1;
- }
-
- uart0_clearFIFOs();
-
- if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
- return -1;
- }
-
-
- /*
- * Setup the handlers for the UART that will be called from the
- * interrupt context when data has been sent and received, specify
- * a pointer to the UART driver instance as the callback reference
- * so the handlers are able to access the instance data
- */
- //XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);
-
- u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;
-
- XUartPs_SetInterruptMask(uartInstPtr, IntrMask);
-
-
- /*
- * Set the receiver timeout. If it is not set, and the last few bytes
- * of data do not trigger the over-water or full interrupt, the bytes
- * will not be received. By default it is disabled.
- * Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
- * timeout function.
- *
- * The setting of 8 will timeout after 8 x 4 = 32 character times.
- * Increase the time out value if baud rate is high, decrease it if
- * baud rate is low.
- */
- XUartPs_SetRecvTimeout(uartInstPtr, 8);
-
- // Second argument is the number of bytes to trigger an interrupt at
- XUartPs_SetFifoThreshold(uartInstPtr, 48);
-
- return 0;
+static u8 packet[MAX_PACKET_SIZE];
+static int bytes_recv = 0;
+
+int try_receive_packet(struct UARTDriver *uart) {
+
+ // Find start of packet
+ int attempts = 0;
+ while (bytes_recv == 0) {
+ if (attempts > MAX_PACKET_SIZE) return -1; // keep trying later
+ if (uart->read(uart, &packet[bytes_recv], 1)) return -1; // ran out
+ if (packet[bytes_recv] == 0xBE) bytes_recv += 1;
+ attempts += 1;
+ }
+
+ // Find length bytes in packet
+ while (bytes_recv < PACKET_HEADER_SIZE) {
+ if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
+ bytes_recv += 1;
+ }
+
+ // Wait for rest of data plus checksum (+1)
+ int length = (packet[6] << 8 | packet[5]) + PACKET_HEADER_SIZE + 1;
+ while (bytes_recv < length) {
+ if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
+ bytes_recv += 1;
+ }
+
+ // Fully recieved a packet
+ return 0;
}
int process_packet(modular_structs_t *structs) {
- metadata_t meta_data;
- // parse metadata
- meta_data.begin_char = uart_buff_get_u8(0);
- meta_data.msg_type = uart_buff_get_u16(1);
- meta_data.msg_id = uart_buff_get_u16(3);
- meta_data.data_len = uart_buff_get_u16(5);
- unsigned char packet_checksum = uart_buff_get_u8(7+meta_data.data_len);
- //unsigned char* packet_data = packet + sizeof(metadata_t);
-
- // Compute checksum
- int i;
- unsigned char calculated_checksum = 0;
- for(i = 0; i < meta_data.data_len + 7; i++){
- calculated_checksum ^= uart_buff_get_u8(i);
- }
- // Discard if checksum didn't match
- if(packet_checksum != calculated_checksum) {
- uart_buff_consume_packet();
- return -1;
- }
-
- // Call appropriate function for packet
- (* (MessageTypes[meta_data.msg_type].functionPtr))(structs);
-
- uart_buff_consume_packet();
-
- return 0;
-
-}
-
-/*
- * Temporarily disables interrupts and returns the interrupt state, for restoring them
- */
-u32 disable_interrupts() {
- u32 ImrRegister;
- ImrRegister = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_IMR_OFFSET);
- XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IDR_OFFSET, XUARTPS_IXR_MASK);
- return ImrRegister;
-}
-
-/*
- * Restores the interrupt state, saved from disable_interrupts
- */
-void restore_interrupts(u32 intr_state) {
- XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IER_OFFSET, intr_state);
+ metadata_t meta;
+ // parse metadata
+ meta.begin_char = packet[0];
+ meta.msg_type = packet[2] << 8 | packet[1];
+ meta.msg_id = packet[4] << 8 | packet[3];
+ meta.data_len = packet[6] << 8 | packet[5];
+ unsigned char packet_checksum = packet[7+meta.data_len];
+
+ // Compute checksum
+ int i;
+ unsigned char calculated_checksum = 0;
+ for(i = 0; i < meta.data_len + 7; i++){
+ calculated_checksum ^= packet[i];
+ }
+ // Discard if checksum didn't match
+ if(packet_checksum != calculated_checksum) {
+ return -1;
+ }
+
+ // Call appropriate function for packet
+ (* (MessageTypes[meta.msg_type].functionPtr))(structs,&meta,packet[PACKET_HEADER_SIZE], meta.data_len);
+
+ // Done processing packets, reset
+ bytes_recv = 0;
+
+ return 0;
}
void process_received(modular_structs_t *structs) {
- // Parse as many packets as possible
- while (uart_buff_packet_ready()) {
- process_packet(structs);
- }
-}
-
-void uart_interrupt_handler(XUartPs *InstancePtr) {
- u32 IsrStatus;
-
- /*
- * Read the interrupt ID register to determine which
- * interrupt is active
- */
- IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
- XUARTPS_IMR_OFFSET);
-
- IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
- XUARTPS_ISR_OFFSET);
-
- /*
- * Read the Channel Status Register to determine if there is any data in
- * the RX FIFO
- */
-
- u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
- XUARTPS_SR_OFFSET);
-
-#ifdef INTERRUPT_BENCHMARK
- u32 start_time = timer_get_count();
-#endif
-
- while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && !uart_buff_full()) {
- u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
- uart_buff_add_u8(byte);
- CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
- }
-
-#ifdef INTERRUPT_BENCHMARK
- u32 end_time = timer_get_count();
- u32 duration = end_time - start_time;
- send_data(0, 0, 0, (char *) &duration, 8);
-#endif
-
- // Clear the interrupt status.
- XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
- IsrStatus);
+ // Parse as many packets as possible
+ struct UARTDriver *uart = &structs->hardware_struct.uart;
+ while (try_receive_packet(uart)) {
+ process_packet(structs);
+ }
}
-int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size) {
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
@@ -175,7 +97,7 @@ int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
// bytes|| 1 | 2 | 2 | 2 | var | 1 |
//----------------------------------------------------------------------------------------------
- char formattedHeader[7];
+ unsigned char formattedHeader[PACKET_HEADER_SIZE];
// Begin Char:
formattedHeader[0] = BEGIN_CHAR;
@@ -189,22 +111,20 @@ int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
formattedHeader[5] = size & 0x000000ff;
formattedHeader[6] = (size >> 8) & 0x000000ff;
- // Compute checksum while sending
+ // Compute checksum
unsigned char packet_checksum = 0;
-
int i;
- for(i = 0; i < 7; i++) {
+ for(i = 0; i < PACKET_HEADER_SIZE; i++) {
packet_checksum ^= formattedHeader[i];
- uart0_sendByte(formattedHeader[i]);
}
- // Send data
for (i = 0; i < size; i++) {
packet_checksum ^= data[i];
- uart0_sendByte(data[i]);
}
- //uart0_sendBytes(data, size);
- // Send checksum
- uart0_sendByte(packet_checksum);
+
+ // Send header, data, and checksum
+ uart->write(uart, formattedHeader, PACKET_HEADER_SIZE);
+ uart->write(uart, (unsigned char *) data, size);
+ uart->write(uart, &packet_checksum, 1);
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/communication.h b/quad/sw/modular_quad_pid/src/communication.h
index 92c7c86d0..df37a8cda 100644
--- a/quad/sw/modular_quad_pid/src/communication.h
+++ b/quad/sw/modular_quad_pid/src/communication.h
@@ -1,21 +1,14 @@
#ifndef _COMMUNICATION_H
#define _COMMUNICATION_H
-#include <xuartps.h>
-#include "xparameters.h"
-#include "xscugic.h"
-#include "xil_exception.h"
-
#include "type_def.h"
-#include "uart.h"
#include "mio7_led.h"
#include "timer.h"
#include "commands.h"
-#include "uart_buff.h"
+#include "hardware/hw_iface.h"
int initUartComms();
void process_received(modular_structs_t *structs);
-void uart_interrupt_handler(XUartPs *InstancePtr);
-int send_data(u16 type_id, u16 msg_id, char* data, size_t size);
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size);
#endif
diff --git a/quad/sw/modular_quad_pid/src/controllers.c b/quad/sw/modular_quad_pid/src/controllers.c
index 7be7616e1..22f8d5d02 100644
--- a/quad/sw/modular_quad_pid/src/controllers.c
+++ b/quad/sw/modular_quad_pid/src/controllers.c
@@ -12,7 +12,6 @@
#include "iic_utils.h"
#include "quadposition.h"
#include "util.h"
-#include "uart.h"
#include "sleep.h"
#include "stdio.h"
#include <math.h>
diff --git a/quad/sw/modular_quad_pid/src/gam.h b/quad/sw/modular_quad_pid/src/gam.h
index 6c5ed8554..0f4803e8d 100644
--- a/quad/sw/modular_quad_pid/src/gam.h
+++ b/quad/sw/modular_quad_pid/src/gam.h
@@ -1,8 +1,6 @@
#ifndef _GAM_H
#define _GAM_H
-#include "xbasic_types.h"
-
//Gyro, accelerometer, and magnetometer data structure
//Used for reading an instance of the sensor data
typedef struct {
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_iface.h b/quad/sw/modular_quad_pid/src/hardware/hw_iface.h
new file mode 100644
index 000000000..a5b7d148e
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_iface.h
@@ -0,0 +1,50 @@
+#ifndef HW_IFACE_H
+#define HW_IFACE_H
+
+struct I2CDriver {
+ void *state;
+ int (*reset)(struct I2CDriver *self);
+ int (*write)(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *data,
+ unsigned int length);
+ int (*read)(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *buff,
+ unsigned int length);
+};
+
+struct PWMOutputDriver {
+ void *state;
+ int (*reset)(struct PWMOutputDriver *self);
+ int (*write)(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+};
+
+struct PWMInputDriver {
+ void *state;
+ int (*reset)(struct PWMInputDriver *self);
+ int (*read)(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+};
+
+struct UARTDriver {
+ void *state;
+ int (*reset)(struct UARTDriver *self);
+ int (*write)(struct UARTDriver *self, unsigned char *data, unsigned int length);
+ int (*read)(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+};
+
+struct TimerDriver {
+ void *state;
+ int (*reset)(struct TimerDriver *self);
+ int (*restart)(struct TimerDriver *self);
+ int (*read)(struct TimerDriver *self, unsigned long *us);
+};
+
+struct LEDDriver {
+ void *state;
+ int (*reset)(struct LEDDriver *self);
+ int (*turn_on)(struct LEDDriver *self);
+ int (*turn_off)(struct LEDDriver *self);
+};
+
+#endif
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo.h b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo.h
new file mode 100644
index 000000000..eb50756d4
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo.h
@@ -0,0 +1,51 @@
+#ifndef HW_IMPL_ZYBO
+#define HW_IMPL_ZYBO
+
+#include "hw_iface.h"
+
+#include <sleep.h>
+#include <stdlib.h>
+#include <xtmrctr.h>
+#include <xgpiops.h>
+#include "xiicps.h"
+#include "xparameters.h"
+#include "xgpiops.h"
+#include "xil_types.h"
+#include "xscugic.h"
+#include "xtime_l.h"
+#include "xuartps.h"
+#include "queue.h"
+
+int zybo_uart_reset(struct UARTDriver *self);
+int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length);
+int zybo_uart_read(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+
+int zybo_pwm_output_reset(struct PWMOutputDriver *self);
+int zybo_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+
+int zybo_pwm_input_reset(struct PWMInputDriver *self);
+int zybo_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+
+int zybo_i2c_reset(struct I2CDriver *self);
+int zybo_i2c_write(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *data,
+ unsigned int length);
+int zybo_i2c_read(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *buff,
+ unsigned int length);
+
+int zybo_global_timer_reset(struct TimerDriver *self);
+int zybo_global_timer_restart(struct TimerDriver *self);
+int zybo_global_timer_read(struct TimerDriver *self, unsigned long *us);
+
+int zybo_axi_timer_reset(struct TimerDriver *self);
+int zybo_axi_timer_restart(struct TimerDriver *self);
+int zybo_axi_timer_read(struct TimerDriver *self, unsigned long *us);
+
+int zybo_led_reset(struct LEDDriver *self);
+int zybo_led_turn_on(struct LEDDriver *self);
+int zybo_led_turn_off(struct LEDDriver *self);
+
+#endif
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_axi_timer.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_axi_timer.c
new file mode 100644
index 000000000..399c819fa
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_axi_timer.c
@@ -0,0 +1,23 @@
+#include "hw_impl_zybo.h"
+
+// Apparently, this is number of counts per usec
+#define PL_CLK_CNTS_PER_USEC 100
+
+int zybo_axi_timer_reset(struct TimerDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(XTmrCtr));
+ }
+ return XTmrCtr_Initialize(self->state, XPAR_AXI_TIMER_0_DEVICE_ID);
+}
+
+int zybo_axi_timer_restart(struct TimerDriver *self) {
+ XTmrCtr_Reset(self->state, 0);
+ XTmrCtr_Start(self->state, 0);
+ return 0;
+}
+
+int zybo_axi_timer_read(struct TimerDriver *self, unsigned long *us) {
+ // Returns the number of useconds
+ *us = XTmrCtr_GetValue(self->state, 0) / PL_CLK_CNTS_PER_USEC;
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_global_timer.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_global_timer.c
new file mode 100644
index 000000000..9f59573df
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_global_timer.c
@@ -0,0 +1,18 @@
+#include "hw_impl_zybo.h"
+
+int zybo_global_timer_reset(struct TimerDriver *self) {
+ // Nothing to initialize
+ return 0;
+}
+
+int zybo_global_timer_restart(struct TimerDriver *self) {
+ XTime_SetTime(0);
+ return 0;
+}
+
+int zybo_global_timer_read(struct TimerDriver *self, unsigned long *us) {
+ XTime time;
+ XTime_GetTime(&time);
+ *us = (time * 1000000) / COUNTS_PER_SECOND; // (ticks)(1000000us/s)(s/ticks)
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_i2c.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_i2c.c
new file mode 100644
index 000000000..c24a2b7a7
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_i2c.c
@@ -0,0 +1,231 @@
+#include "hw_impl_zybo.h"
+
+#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
+
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+ int ByteCount, u16 SlaveAddr);
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
+
+int zybo_i2c_reset(struct I2CDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(XIicPs));
+ }
+ XIicPs *inst = self->state;
+
+ //Make sure CPU_1x clk is enabled for I2C controller
+ u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;
+
+ if (*aper_ctrl & 0x00040000){
+ xil_printf("CPU_1x is set to I2C0\r\n");
+ }
+
+ else {
+ xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
+ *aper_ctrl |= 0x00040000;
+ }
+
+ // Look up
+ XIicPs_Config *i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
+ XStatus status = XIicPs_CfgInitialize(inst, i2c_config, i2c_config->BaseAddress);
+
+ // Check if initialization was successful
+ if(status != XST_SUCCESS){
+ return -1;
+ }
+
+ // Reset the controller and set the clock to 400kHz
+ XIicPs_Reset(inst);
+ XIicPs_SetSClk(inst, 400000);
+
+ return 0;
+}
+
+int zybo_i2c_write(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *data,
+ unsigned int length) {
+ XIicPs *inst = self->state;
+ return XIicPs_MasterSendPolled_ours(inst, data, length, device_addr);
+}
+
+int zybo_i2c_read(struct I2CDriver *self,
+ unsigned short device_addr,
+ unsigned char *buff,
+ unsigned int length) {
+ XIicPs *inst = self->state;
+ return XIicPs_MasterRecvPolled(inst, buff, length, device_addr);
+}
+
+/*****************************************************************************/
+/**
+* NOTE to MicroCART: This function is originally from the Xilinx library,
+* but we noticed that the original function didn't check for a NACK, which
+* would cause the original polling function to enter an infinite loop in the
+* event of a NACK. Notice that we have added that NACK check at the final
+* while loop of this function.
+*
+*
+* This function initiates a polled mode send in master mode.
+*
+* It sends data to the FIFO and waits for the slave to pick them up.
+* If slave fails to remove data from FIFO, the send fails with
+* time out.
+*
+* @param InstancePtr is a pointer to the XIicPs instance.
+* @param MsgPtr is the pointer to the send buffer.
+* @param ByteCount is the number of bytes to be sent.
+* @param SlaveAddr is the address of the slave we are sending to.
+*
+* @return
+* - XST_SUCCESS if everything went well.
+* - XST_FAILURE if timed out.
+*
+* @note This send routine is for polled mode transfer only.
+*
+****************************************************************************/
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+ int ByteCount, u16 SlaveAddr)
+{
+ u32 IntrStatusReg;
+ u32 StatusReg;
+ u32 BaseAddr;
+ u32 Intrs;
+
+ /*
+ * Assert validates the input arguments.
+ */
+ Xil_AssertNonvoid(InstancePtr != NULL);
+ Xil_AssertNonvoid(MsgPtr != NULL);
+ Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+ Xil_AssertNonvoid(XIICPS_ADDR_MASK >= SlaveAddr);
+
+ BaseAddr = InstancePtr->Config.BaseAddress;
+ InstancePtr->SendBufferPtr = MsgPtr;
+ InstancePtr->SendByteCount = ByteCount;
+
+ XIicPs_SetupMaster(InstancePtr, SENDING_ROLE);
+
+ XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
+
+ /*
+ * Intrs keeps all the error-related interrupts.
+ */
+ Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_TX_OVR_MASK |
+ XIICPS_IXR_TO_MASK | XIICPS_IXR_NACK_MASK;
+
+ /*
+ * Clear the interrupt status register before use it to monitor.
+ */
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+ XIicPs_WriteReg(BaseAddr, XIICPS_ISR_OFFSET, IntrStatusReg);
+
+ /*
+ * Transmit first FIFO full of data.
+ */
+ TransmitFifoFill(InstancePtr);
+
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+
+ /*
+ * Continue sending as long as there is more data and
+ * there are no errors.
+ */
+ while ((InstancePtr->SendByteCount > 0) &&
+ ((IntrStatusReg & Intrs) == 0)) {
+ StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
+
+ /*
+ * Wait until transmit FIFO is empty.
+ */
+ if ((StatusReg & XIICPS_SR_TXDV_MASK) != 0) {
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+ XIICPS_ISR_OFFSET);
+ continue;
+ }
+
+ /*
+ * Send more data out through transmit FIFO.
+ */
+ TransmitFifoFill(InstancePtr);
+ }
+
+ /*
+ * Check for completion of transfer.
+ */
+ // NOTE for MicroCART: Corrected function. Original left for reference.
+// while ((XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET) &
+// XIICPS_IXR_COMP_MASK) != XIICPS_IXR_COMP_MASK);
+ while (!(IntrStatusReg & (Intrs | XIICPS_IXR_COMP_MASK))) {
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+ }
+
+ /*
+ * If there is an error, tell the caller.
+ */
+ if (IntrStatusReg & Intrs) {
+ return XST_FAILURE;
+ }
+
+ return XST_SUCCESS;
+}
+
+/*****************************************************************************/
+/*
+* NOTE to MicroCART: This function is required by the send polling method above,
+* but it was originally static, so we had to copy it word-for-word here.
+*
+* This function prepares a device to transfers as a master.
+*
+* @param InstancePtr is a pointer to the XIicPs instance.
+*
+* @param Role specifies whether the device is sending or receiving.
+*
+* @return
+* - XST_SUCCESS if everything went well.
+* - XST_FAILURE if bus is busy.
+*
+* @note Interrupts are always disabled, device which needs to use
+* interrupts needs to setup interrupts after this call.
+*
+****************************************************************************/
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role)
+{
+ u32 ControlReg;
+ u32 BaseAddr;
+ u32 EnabledIntr = 0x0;
+
+ Xil_AssertNonvoid(InstancePtr != NULL);
+
+ BaseAddr = InstancePtr->Config.BaseAddress;
+ ControlReg = XIicPs_ReadReg(BaseAddr, XIICPS_CR_OFFSET);
+
+
+ /*
+ * Only check if bus is busy when repeated start option is not set.
+ */
+ if ((ControlReg & XIICPS_CR_HOLD_MASK) == 0) {
+ if (XIicPs_BusIsBusy(InstancePtr)) {
+ return XST_FAILURE;
+ }
+ }
+
+ /*
+ * Set up master, AckEn, nea and also clear fifo.
+ */
+ ControlReg |= XIICPS_CR_ACKEN_MASK | XIICPS_CR_CLR_FIFO_MASK |
+ XIICPS_CR_NEA_MASK | XIICPS_CR_MS_MASK;
+
+ if (Role == RECVING_ROLE) {
+ ControlReg |= XIICPS_CR_RD_WR_MASK;
+ EnabledIntr = XIICPS_IXR_DATA_MASK |XIICPS_IXR_RX_OVR_MASK;
+ }else {
+ ControlReg &= ~XIICPS_CR_RD_WR_MASK;
+ }
+ EnabledIntr |= XIICPS_IXR_COMP_MASK | XIICPS_IXR_ARB_LOST_MASK;
+
+ XIicPs_WriteReg(BaseAddr, XIICPS_CR_OFFSET, ControlReg);
+
+ XIicPs_DisableAllInterrupts(BaseAddr);
+
+ return XST_SUCCESS;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_mio7_led.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_mio7_led.c
new file mode 100644
index 000000000..03a192671
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_mio7_led.c
@@ -0,0 +1,22 @@
+#include "hw_impl_zybo.h"
+
+int zybo_mio7_led_reset(struct LEDDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(XGpioPs));
+ }
+
+ XGpioPs_Config *ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
+ XGpioPs_CfgInitialize(self->state, ConfigPtr, ConfigPtr->BaseAddr);
+ XGpioPs_SetDirectionPin(self->state, 7, 1);
+ return 0;
+}
+
+int zybo_mio7_led_turn_on(struct LEDDriver *self) {
+ XGpioPs_WritePin(self->state, 7, 0x01);
+ return 0;
+}
+
+int zybo_mio7_led_turn_off(struct LEDDriver *self) {
+ XGpioPs_WritePin(self->state, 7, 0x00);
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_input.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_input.c
new file mode 100644
index 000000000..b0c3cd98e
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_input.c
@@ -0,0 +1,32 @@
+#include "hw_impl_zybo.h"
+
+struct PWMInputDriverState {
+ int *channels[6];
+};
+
+int zybo_pwm_input_reset(struct PWMInputDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct PWMInputDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+
+ // Save the addresses of the input PWM recorders
+ struct PWMInputDriverState *state = self->state;
+ state->channels[0] = (int *) XPAR_PWM_RECORDER_0_BASEADDR;
+ state->channels[1] = (int *) XPAR_PWM_RECORDER_1_BASEADDR;
+ state->channels[2] = (int *) XPAR_PWM_RECORDER_2_BASEADDR;
+ state->channels[3] = (int *) XPAR_PWM_RECORDER_3_BASEADDR;
+ state->channels[4] = (int *) XPAR_PWM_RECORDER_4_BASEADDR;
+ state->channels[5] = (int *) XPAR_PWM_RECORDER_5_BASEADDR;
+ return 0;
+}
+
+int zybo_pwm_input_read(struct PWMInputDriver *self,
+ unsigned int channel,
+ unsigned long *pulse_width_us) {
+ struct PWMInputDriverState *state = self->state;
+ *pulse_width_us = (long) *((int *) state->channels[channel]);
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_output.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_output.c
new file mode 100644
index 000000000..e4c75bd50
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_pwm_output.c
@@ -0,0 +1,52 @@
+#include "hw_impl_zybo.h"
+
+#define THROTTLE_PULSE_WIDTH_LOW 1000 // us
+#define THROTTLE_PULSE_WIDTH_HIGH 2000 // us
+#define PERIOD_WIDTH 222222 // 100000000 ns / 450
+#define PULSE_WIDTH_ADDR_OFFSET 4
+
+struct PWMOutputDriverState {
+ int *outputs[4];
+};
+
+int zybo_pwm_output_reset(struct PWMOutputDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct PWMOutputDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+ struct PWMOutputDriverState *state = self->state;
+
+ state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR;
+ state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR;
+ state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR;
+ state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR;
+
+ // Set period width of PWM pulse
+ *(state->outputs[0]) = PERIOD_WIDTH;
+ *(state->outputs[1]) = PERIOD_WIDTH;
+ *(state->outputs[2]) = PERIOD_WIDTH;
+ *(state->outputs[3]) = PERIOD_WIDTH;
+
+ // Set a low pulse (1 ms) so that outputs are off
+ *(state->outputs[0] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
+ *(state->outputs[1] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
+ *(state->outputs[2] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
+ *(state->outputs[3] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
+
+ usleep(1000000);
+ return 0;
+}
+
+int zybo_pwm_output_write(struct PWMOutputDriver *self,
+ unsigned int channel,
+ unsigned long pulse_width_us) {
+ if (pulse_width_us > THROTTLE_PULSE_WIDTH_HIGH)
+ pulse_width_us = THROTTLE_PULSE_WIDTH_HIGH;
+ if (pulse_width_us < THROTTLE_PULSE_WIDTH_LOW)
+ pulse_width_us = THROTTLE_PULSE_WIDTH_LOW;
+ struct PWMOutputDriverState *state = self->state;
+ *(state->outputs[channel] + PULSE_WIDTH_ADDR_OFFSET) = pulse_width_us;
+ return 0;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_uart.c b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_uart.c
new file mode 100644
index 000000000..263e4ad9e
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_uart.c
@@ -0,0 +1,290 @@
+#include "hw_impl_zybo.h"
+
+#define BAUD_RATE 921600
+#define MAX_UART_BUFFER_SIZE 2048
+
+int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate);
+void uart_interrupt_handler(XUartPs *InstancePtr);
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler);
+void uart_interrupt_handler(XUartPs *InstancePtr);
+
+// Queue has to be global to be accessible to ISR
+static volatile struct Queue queue;
+static XScuGic xscugic;
+
+int zybo_uart_reset(struct UARTDriver *self) {
+ // Instantiate the Driver state
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(XUartPs));
+ if (self->state == NULL) {
+ return -1;
+ }
+ queue = queue_create(MAX_UART_BUFFER_SIZE);
+ }
+
+ XUartPs *inst = self->state;;
+
+ // Configure XUartPs instance
+ XUartPs_Config* config = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);
+ if (XUartPs_CfgInitialize(inst, config, config->BaseAddress) != XST_SUCCESS) {
+ return -1;
+ }
+
+ // Set UART Baudrate
+ if(XUartPs_SetBaudRate_ours(inst, BAUD_RATE) != XST_SUCCESS) {
+ return -1;
+ }
+
+ // Clear Tx/Rx FIFOs
+ int* uart_ctrl_reg = (int*) inst->Config.BaseAddress;
+ *uart_ctrl_reg |= 0x00000003; // clear TX & RX
+
+ // Setup Interrupts
+ if (SetupInterruptSystem(inst, XPAR_PS7_UART_0_INTR, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
+ return -1;
+ }
+
+ // Interrupts selected: Rx FIFO threashold reached, RX overflow, timeout
+ u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;
+ XUartPs_SetInterruptMask(inst, IntrMask);
+
+ /*
+ * Set the receiver timeout. If it is not set, and the last few bytes
+ * of data do not trigger the over-water or full interrupt, the bytes
+ * will not be received. By default it is disabled.
+ * Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
+ * timeout function.
+ *
+ * The setting of 8 will timeout after 8 x 4 = 32 character times.
+ * Increase the time out value if baud rate is high, decrease it if
+ * baud rate is low.
+ */
+ XUartPs_SetRecvTimeout(inst, 8);
+
+ // Second argument is the number of bytes to trigger an interrupt at
+ XUartPs_SetFifoThreshold(inst, 48);
+
+ return 0;
+}
+
+int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length) {
+ XUartPs *inst = self->state;
+ int i;
+ for (i = 0; i < length; i += 1) {
+ XUartPs_SendByte(inst->Config.BaseAddress, data[i]);
+ }
+
+ return 0;
+}
+
+int zybo_uart_read(struct UARTDriver *self, unsigned char *buff, unsigned int length) {
+ int i = 0;
+ while (queue_remove(&queue, &buff[i]) == 0 && i < length) i += 1;
+ return i;
+}
+
+//This is copied from xuart driver
+/***************************************************/
+
+#define XUARTPS_MAX_BAUD_ERROR_RATE 3 /* max % error allowed */
+int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
+{
+ u8 IterBAUDDIV; /* Iterator for available baud divisor values */
+ u32 BRGR_Value; /* Calculated value for baud rate generator */
+ u32 CalcBaudRate; /* Calculated baud rate */
+ u32 BaudError; /* Diff between calculated and requested baud rate */
+ u32 Best_BRGR = 0; /* Best value for baud rate generator */
+ u8 Best_BAUDDIV = 0; /* Best value for baud divisor */
+ u32 Best_Error = 0xFFFFFFFF;
+ u32 PercentError;
+ u32 ModeReg;
+ u32 InputClk;
+
+ /*
+ * Asserts validate the input arguments
+ */
+ Xil_AssertNonvoid(InstancePtr != NULL);
+ Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+ //Xil_AssertNonvoid(BaudRate <= XUARTPS_MAX_RATE);
+ Xil_AssertNonvoid(BaudRate >= XUARTPS_MIN_RATE);
+
+ /*
+ * Make sure the baud rate is not impossilby large.
+ * Fastest possible baud rate is Input Clock / 2.
+ */
+ if ((BaudRate * 2) > InstancePtr->Config.InputClockHz) {
+ return XST_UART_BAUD_ERROR;
+ }
+ /*
+ * Check whether the input clock is divided by 8
+ */
+ ModeReg = XUartPs_ReadReg( InstancePtr->Config.BaseAddress,
+ XUARTPS_MR_OFFSET);
+
+ InputClk = InstancePtr->Config.InputClockHz;
+ if(ModeReg & XUARTPS_MR_CLKSEL) {
+ InputClk = InstancePtr->Config.InputClockHz / 8;
+ }
+
+ /*
+ * Determine the Baud divider. It can be 4to 254.
+ * Loop through all possible combinations
+ */
+ for (IterBAUDDIV = 4; IterBAUDDIV < 255; IterBAUDDIV++) {
+
+ /*
+ * Calculate the value for BRGR register
+ */
+ BRGR_Value = InputClk / (BaudRate * (IterBAUDDIV + 1));
+
+ /*
+ * Calculate the baud rate from the BRGR value
+ */
+ CalcBaudRate = InputClk/ (BRGR_Value * (IterBAUDDIV + 1));
+
+ /*
+ * Avoid unsigned integer underflow
+ */
+ if (BaudRate > CalcBaudRate) {
+ BaudError = BaudRate - CalcBaudRate;
+ }
+ else {
+ BaudError = CalcBaudRate - BaudRate;
+ }
+
+ /*
+ * Find the calculated baud rate closest to requested baud rate.
+ */
+ if (Best_Error > BaudError) {
+
+ Best_BRGR = BRGR_Value;
+ Best_BAUDDIV = IterBAUDDIV;
+ Best_Error = BaudError;
+ }
+ }
+
+ /*
+ * Make sure the best error is not too large.
+ */
+ PercentError = (Best_Error * 100) / BaudRate;
+ if (XUARTPS_MAX_BAUD_ERROR_RATE < PercentError) {
+ return XST_UART_BAUD_ERROR;
+ }
+
+ /*
+ * Disable TX and RX to avoid glitches when setting the baud rate.
+ */
+ XUartPs_DisableUart(InstancePtr);
+
+ XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
+ XUARTPS_BAUDGEN_OFFSET, Best_BRGR);
+ XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
+ XUARTPS_BAUDDIV_OFFSET, Best_BAUDDIV);
+
+ /*
+ * Enable device
+ */
+ XUartPs_EnableUart(InstancePtr);
+
+ InstancePtr->BaudRate = BaudRate;
+
+ return XST_SUCCESS;
+
+}
+
+void uart_interrupt_handler(XUartPs *InstancePtr) {
+ u32 IsrStatus;
+
+ /*
+ * Read the interrupt ID register to determine which
+ * interrupt is active
+ */
+ IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+ XUARTPS_IMR_OFFSET);
+
+ IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+ XUARTPS_ISR_OFFSET);
+
+ /*
+ * Read the Channel Status Register to determine if there is any data in
+ * the RX FIFO
+ */
+
+ u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+ XUARTPS_SR_OFFSET);
+
+ while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && !queue_full(&queue)) {
+ u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
+ queue_add(&queue, byte);
+ CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
+ }
+
+ // Clear the interrupt status.
+ XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
+ IsrStatus);
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sets up the interrupt system so interrupts can occur for the
+* Uart. This function is application-specific.
+*
+* @param IntcInstancePtr is a pointer to the instance of the INTC.
+* @param UartInstancePtr contains a pointer to the instance of the UART
+* driver which is going to be connected to the interrupt
+* controller.
+* @param UartIntrId is the interrupt Id and is typically
+* XPAR_<UARTPS_instance>_INTR value from xparameters.h.
+*
+* @return XST_SUCCESS if successful, otherwise XST_FAILURE.
+*
+* @note None.
+*
+****************************************************************************/
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
+{
+ int Status;
+
+ XScuGic_Config *IntcConfig; /* Config for interrupt controller */
+
+ /* Initialize the interrupt controller driver */
+ IntcConfig = XScuGic_LookupConfig(XPAR_SCUGIC_SINGLE_DEVICE_ID);
+ if (NULL == IntcConfig) {
+ return XST_FAILURE;
+ }
+
+ Status = XScuGic_CfgInitialize(&xscugic, IntcConfig,
+ IntcConfig->CpuBaseAddress);
+ if (Status != XST_SUCCESS) {
+ return XST_FAILURE;
+ }
+
+ /*
+ * Connect the interrupt controller interrupt handler to the
+ * hardware interrupt handling logic in the processor.
+ */
+ Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
+ (Xil_ExceptionHandler) XScuGic_InterruptHandler,
+ &xscugic);
+
+ /*
+ * Connect a device driver handler that will be called when an
+ * interrupt for the device occurs, the device driver handler
+ * performs the specific interrupt processing for the device
+ */
+ Status = XScuGic_Connect(&xscugic, UartIntrId,
+ handler,
+ (void *) UartInstancePtr);
+ if (Status != XST_SUCCESS) {
+ return XST_FAILURE;
+ }
+
+ /* Enable the interrupt for the device */
+ XScuGic_Enable(&xscugic, UartIntrId);
+
+ /* Enable interrupts */
+ Xil_ExceptionEnable();
+
+ return XST_SUCCESS;
+}
diff --git a/quad/sw/modular_quad_pid/src/hardware/queue.c b/quad/sw/modular_quad_pid/src/hardware/queue.c
new file mode 120000
index 000000000..6b67b81f5
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/queue.c
@@ -0,0 +1 @@
+../../../../lib/queue/queue.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/hardware/queue.h b/quad/sw/modular_quad_pid/src/hardware/queue.h
new file mode 120000
index 000000000..b9524b354
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/hardware/queue.h
@@ -0,0 +1 @@
+../../../../lib/queue/queue.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.c b/quad/sw/modular_quad_pid/src/iic_utils.c
index 290d5f9b6..9bbfd82ed 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.c
+++ b/quad/sw/modular_quad_pid/src/iic_utils.c
@@ -14,44 +14,13 @@
#include "xbasic_types.h"
#include "xiicps.h"
-XIicPs_Config* i2c_config;
-XIicPs I2C0;
-double magX_correction = -1, magY_correction, magZ_correction;
-int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr, int ByteCount, u16 SlaveAddr);
-int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
-s32 TransmitFifoFill(XIicPs *InstancePtr);
-
-int iic0_init(){
-
- //Make sure CPU_1x clk is enabled for I2C controller
- Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
-
- if(*aper_ctrl & 0x00040000){
- xil_printf("CPU_1x is set to I2C0\r\n");
- }
-
- else{
- xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
- *aper_ctrl |= 0x00040000;
- }
-
-
- // Look up
- i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
- XStatus status = XIicPs_CfgInitialize(&I2C0, i2c_config, i2c_config->BaseAddress);
-
- // Check if initialization was successful
- if(status != XST_SUCCESS){
- return -1;
- }
-
- // Reset the controller and set the clock to 400kHz
- XIicPs_Reset(&I2C0);
- XIicPs_SetSClk(&I2C0, 400000);
+static struct I2CDriver *i2c;
+double magX_correction = -1, magY_correction, magZ_correction;
- return 0;
+void iic_set_global(struct I2CDriver *given_i2c) {
+ i2c = given_i2c;
}
int iic0_mpu9150_start(){
@@ -111,8 +80,7 @@ void iic0_mpu9150_write(u8 register_addr, u8 data){
device_addr = MPU9150_COMPASS_ADDR;
}
- XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, device_addr);
-
+ i2c->write(i2c, device_addr, buf, 2);
}
void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
@@ -130,8 +98,8 @@ void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
}
- XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, device_addr);
- XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
+ i2c->write(i2c, device_addr, buf, 1);
+ i2c->read(i2c, device_addr, recv_buffer, size);
}
void iic0_mpu9150_calc_mag_sensitivity(){
@@ -241,16 +209,16 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
int iic0_lidarlite_write(u8 register_addr, u8 data) {
u8 buf[] = {register_addr, data};
-
- return XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, LIDARLITE_DEVICE_ADDR);
+
+ return i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 2);
}
int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
u8 buf[] = {register_addr};
int status = 0;
- status = XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, LIDARLITE_DEVICE_ADDR);
- status |= XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size, LIDARLITE_DEVICE_ADDR);
+ status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
+ status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
return status;
}
@@ -282,177 +250,3 @@ int iic0_lidarlite_read_distance(lidar_t *lidar) {
return status;
}
-
-/*****************************************************************************/
-/**
-* NOTE to MicroCART: This function is originally from the Xilinx library,
-* but we noticed that the original function didn't check for a NACK, which
-* would cause the original polling function to enter an infinite loop in the
-* event of a NACK. Notice that we have added that NACK check at the final
-* while loop of this function.
-*
-*
-* This function initiates a polled mode send in master mode.
-*
-* It sends data to the FIFO and waits for the slave to pick them up.
-* If slave fails to remove data from FIFO, the send fails with
-* time out.
-*
-* @param InstancePtr is a pointer to the XIicPs instance.
-* @param MsgPtr is the pointer to the send buffer.
-* @param ByteCount is the number of bytes to be sent.
-* @param SlaveAddr is the address of the slave we are sending to.
-*
-* @return
-* - XST_SUCCESS if everything went well.
-* - XST_FAILURE if timed out.
-*
-* @note This send routine is for polled mode transfer only.
-*
-****************************************************************************/
-int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
- int ByteCount, u16 SlaveAddr)
-{
- u32 IntrStatusReg;
- u32 StatusReg;
- u32 BaseAddr;
- u32 Intrs;
-
- /*
- * Assert validates the input arguments.
- */
- Xil_AssertNonvoid(InstancePtr != NULL);
- Xil_AssertNonvoid(MsgPtr != NULL);
- Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
- Xil_AssertNonvoid(XIICPS_ADDR_MASK >= SlaveAddr);
-
- BaseAddr = InstancePtr->Config.BaseAddress;
- InstancePtr->SendBufferPtr = MsgPtr;
- InstancePtr->SendByteCount = ByteCount;
-
- XIicPs_SetupMaster(InstancePtr, SENDING_ROLE);
-
- XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
-
- /*
- * Intrs keeps all the error-related interrupts.
- */
- Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_TX_OVR_MASK |
- XIICPS_IXR_TO_MASK | XIICPS_IXR_NACK_MASK;
-
- /*
- * Clear the interrupt status register before use it to monitor.
- */
- IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
- XIicPs_WriteReg(BaseAddr, XIICPS_ISR_OFFSET, IntrStatusReg);
-
- /*
- * Transmit first FIFO full of data.
- */
- TransmitFifoFill(InstancePtr);
-
- IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
-
- /*
- * Continue sending as long as there is more data and
- * there are no errors.
- */
- while ((InstancePtr->SendByteCount > 0) &&
- ((IntrStatusReg & Intrs) == 0)) {
- StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
-
- /*
- * Wait until transmit FIFO is empty.
- */
- if ((StatusReg & XIICPS_SR_TXDV_MASK) != 0) {
- IntrStatusReg = XIicPs_ReadReg(BaseAddr,
- XIICPS_ISR_OFFSET);
- continue;
- }
-
- /*
- * Send more data out through transmit FIFO.
- */
- TransmitFifoFill(InstancePtr);
- }
-
- /*
- * Check for completion of transfer.
- */
- // NOTE for MicroCART: Corrected function. Original left for reference.
-// while ((XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET) &
-// XIICPS_IXR_COMP_MASK) != XIICPS_IXR_COMP_MASK);
- while (!(IntrStatusReg & (Intrs | XIICPS_IXR_COMP_MASK))) {
- IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
- }
-
- /*
- * If there is an error, tell the caller.
- */
- if (IntrStatusReg & Intrs) {
- return XST_FAILURE;
- }
-
- return XST_SUCCESS;
-}
-
-/*****************************************************************************/
-/*
-* NOTE to MicroCART: This function is required by the send polling method above,
-* but it was originally static, so we had to copy it word-for-word here.
-*
-* This function prepares a device to transfers as a master.
-*
-* @param InstancePtr is a pointer to the XIicPs instance.
-*
-* @param Role specifies whether the device is sending or receiving.
-*
-* @return
-* - XST_SUCCESS if everything went well.
-* - XST_FAILURE if bus is busy.
-*
-* @note Interrupts are always disabled, device which needs to use
-* interrupts needs to setup interrupts after this call.
-*
-****************************************************************************/
-int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role)
-{
- u32 ControlReg;
- u32 BaseAddr;
- u32 EnabledIntr = 0x0;
-
- Xil_AssertNonvoid(InstancePtr != NULL);
-
- BaseAddr = InstancePtr->Config.BaseAddress;
- ControlReg = XIicPs_ReadReg(BaseAddr, XIICPS_CR_OFFSET);
-
-
- /*
- * Only check if bus is busy when repeated start option is not set.
- */
- if ((ControlReg & XIICPS_CR_HOLD_MASK) == 0) {
- if (XIicPs_BusIsBusy(InstancePtr)) {
- return XST_FAILURE;
- }
- }
-
- /*
- * Set up master, AckEn, nea and also clear fifo.
- */
- ControlReg |= XIICPS_CR_ACKEN_MASK | XIICPS_CR_CLR_FIFO_MASK |
- XIICPS_CR_NEA_MASK | XIICPS_CR_MS_MASK;
-
- if (Role == RECVING_ROLE) {
- ControlReg |= XIICPS_CR_RD_WR_MASK;
- EnabledIntr = XIICPS_IXR_DATA_MASK |XIICPS_IXR_RX_OVR_MASK;
- }else {
- ControlReg &= ~XIICPS_CR_RD_WR_MASK;
- }
- EnabledIntr |= XIICPS_IXR_COMP_MASK | XIICPS_IXR_ARB_LOST_MASK;
-
- XIicPs_WriteReg(BaseAddr, XIICPS_CR_OFFSET, ControlReg);
-
- XIicPs_DisableAllInterrupts(BaseAddr);
-
- return XST_SUCCESS;
-}
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.h b/quad/sw/modular_quad_pid/src/iic_utils.h
index 8a8a9a3bb..41673799a 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.h
+++ b/quad/sw/modular_quad_pid/src/iic_utils.h
@@ -18,6 +18,7 @@
#include "xbasic_types.h"
#include "xiicps.h"
#include "type_def.h"
+#include "hardware/hw_iface.h"
// System configuration registers
// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
@@ -48,9 +49,6 @@
#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)
-// Initialize hardware; Call this FIRST before calling any other functions
-int iic0_init();
-
///////////////////////////////////////////////////////////////////////////////
// MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
///////////////////////////////////////////////////////////////////////////////
@@ -101,6 +99,8 @@ int iic0_init();
#define ACCEL_Y_BIAS 0.009f
#define ACCEL_Z_BIAS 0.087f
+void iic_set_global(struct I2CDriver *given_i2c);
+
void iic0_mpu9150_write(u8 register_addr, u8 data);
void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 91af69ad0..7328e381a 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -15,19 +15,20 @@ extern int Xil_AssertWait;
int protection_loops(modular_structs_t *structs)
{
- int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
-
- read_rec_all(rc_commands);
+ u32 rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
+
+ struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
+ read_rec_all(pwm_inputs, rc_commands);
// wait for RC receiver to connect to transmitter
while(rc_commands[THROTTLE] < 75000)
- read_rec_all(rc_commands);
+ read_rec_all(pwm_inputs, rc_commands);
// wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
// also wait for the flight mode to be set to manual
while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP])) {
process_received(structs);
- read_rec_all(rc_commands);
+ read_rec_all(pwm_inputs, rc_commands);
}
// let the pilot/observers know that the quad is now active
@@ -36,49 +37,60 @@ int protection_loops(modular_structs_t *structs)
return 0;
}
-int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_struct,
- sensor_t * sensor_struct, setpoint_t * setpoint_struct, parameter_t * parameter_struct, user_defined_t *user_defined_struct,
- raw_actuator_t * raw_actuator_struct, actuator_command_t * actuator_command_struct)
-{
- // Turn off LED 7 to let observers know that the quad is not yet active
- MIO7_led_off();
-
- // Initialize the controller
- control_algorithm_init(parameter_struct);
-
- // Initialize the logging
- initialize_logging(log_struct, parameter_struct);
-
- // Xilinx given initialization
- init_platform();
-
-
- // Initialize loop timers
- if (timer_init()) {
- return -1;
- }
-
- // Initialize UART0 (Bluetooth)
- if (initUartComms()) {
- return -1;
- }
-
- // Initialize I2C controller and start the sensor board
- if (iic0_init() == -1) {
- return -1;
- }
-
- // Initialize PWM Recorders and Motor outputs
- pwm_init();
-
- //manual flight mode
- user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
+int init_structs(modular_structs_t *structs) {
+ struct LEDDriver *mio7_led = &structs->hardware_struct.mio7_led;
+ if (mio7_led->reset(mio7_led)) return -1;
+ mio7_init_global(mio7_led);
+ // Turn off LED 7 to let observers know that the quad is not yet active
+
+ mio7_led->turn_off(mio7_led);
+
+ // Initialize the controller
+ control_algorithm_init(&structs->parameter_struct);
+
+ // Xilinx given initialization
+ init_platform();
+
+ // Initialize loop timers
+ struct TimerDriver *global_timer = &structs->hardware_struct.global_timer;
+ struct TimerDriver *axi_timer = &structs->hardware_struct.axi_timer;
+ if (global_timer->reset(global_timer)) {
+ return -1;
+ }
+ if (axi_timer->reset(axi_timer)) {
+ return -1;
+ }
+ timer_init_globals(global_timer, axi_timer);
+
+ // Initialize UART0
+ struct UARTDriver *uart = &structs->hardware_struct.uart;
+ if (uart->reset(uart)) {
+ return -1;
+ }
+
+ // Initialize I2C controller and start the sensor board
+ struct I2CDriver *i2c = &structs->hardware_struct.i2c;
+ if (i2c->reset(i2c)) {
+ return -1;
+ }
+ iic_set_global(i2c);
+
+ // Initialize PWM Recorders and Motor outputs
+ struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
+ if (pwm_inputs->reset(pwm_inputs)) return -1;
+ struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
+ if (pwm_outputs->reset(pwm_outputs)) return -1;
+
+ // Initialize sensors
+
+ //manual flight mode
+ structs->user_defined_struct.flight_mode = MANUAL_FLIGHT_MODE;
#ifndef BENCH_TEST
- // Get the first loop data from accelerometer for the gyroscope to use
- if(sensor_init(raw_sensor_struct, sensor_struct) == -1)
- return -1;
+ // Get the first loop data from accelerometer for the gyroscope to use
+ if(sensor_init(&structs->raw_sensor_struct, &structs->sensor_struct) == -1)
+ return -1;
#endif
- return 0;
+ return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.h b/quad/sw/modular_quad_pid/src/initialize_components.h
index 151f68296..aa2c0e962 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.h
+++ b/quad/sw/modular_quad_pid/src/initialize_components.h
@@ -11,9 +11,9 @@
#include "timer.h"
#include "control_algorithm.h"
#include "platform.h"
-#include "uart.h"
#include "iic_utils.h"
#include "util.h"
+#include "type_def.h"
#include "controllers.h"
/**
@@ -35,9 +35,7 @@ int protection_loops(modular_structs_t *structs);
* error message
*
*/
-int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_struct,
- sensor_t * sensor_struct, setpoint_t * setpoint_struct, parameter_t * parameter_struct, user_defined_t *user_defined_struct,
- raw_actuator_t * raw_actuator_struct, actuator_command_t * actuator_command_struct);
+int init_structs(modular_structs_t *structs);
#endif /* INITALIZE_COMPONENTS_H_ */
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 5af76302b..ee106bef6 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -10,7 +10,6 @@
#include <string.h>
#include "PID.h"
#include "type_def.h"
-#include "uart.h"
#include "sleep.h"
#include "log_data.h"
#include "communication.h"
@@ -123,7 +122,7 @@ int log_data(log_t* log_struct, parameter_t* ps)
* TODO: This should probably be transmitting in binary instead of ascii
*/
-void printLogging(log_t* log_struct, parameter_t* ps){
+void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps){
if (arrayIndex == 0) {
// Don't send any logs if nothing was logged
return;
@@ -162,13 +161,13 @@ void printLogging(log_t* log_struct, parameter_t* ps){
strcat(buf,header1);
strcat(buf,header2);
- send_data(LOG_ID, 0, buf, strlen(buf));
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf, strlen(buf));
/*************************/
/* print & send log data */
for(i = 0; i < arrayIndex; i++){
int size = format_log(i, log_struct, buf);
- send_data(LOG_ID, 0, buf, size);
+ send_data(&hardware_struct->uart, LOG_ID, 0, buf, size);
}
char data[1] = {0}; // 1 byte to make compilers happy
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index 0db1751ba..baa21990c 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -47,7 +47,7 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id);
/**
* Prints all the log information.
*/
- void printLogging(log_t* log_struct, parameter_t* ps);
+ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps);
/**
* Resets and clears logged data
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index 52f8c0a83..f06bab8c9 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -15,6 +15,7 @@
#include "actuator_command_processing.h"
#include "send_actuator_commands.h"
#include "update_gui.h"
+#include "communication.h"
//#define BENCH_TEST
//#define UART_BENCHMARK
@@ -28,9 +29,7 @@ int main()
// Initialize all required components and structs:
// Uart, PWM receiver/generator, I2C, Sensor Board
// Xilinx Platform, Loop Timer, Control Algorithm
- int init_error = initializeAllComponents(&(structs.user_input_struct), &(structs.log_struct),
- &(structs.raw_sensor_struct), &(structs.sensor_struct), &(structs.setpoint_struct), &(structs.parameter_struct),
- &(structs.user_defined_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
+ int init_error = init_structs(&(structs));
if (init_error != 0) {
return -1;
@@ -72,7 +71,7 @@ int main()
#ifndef BENCH_TEST
// Get the user input and put it into user_input_struct
- get_user_input(&(structs.log_struct), &(structs.user_input_struct));
+ get_user_input(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct));
// Get data from the sensors and put it into raw_sensor_struct
get_sensors(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
@@ -86,7 +85,7 @@ int main()
&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
// send the actuator commands
- send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+ send_actuator_commands(&(structs.hardware_struct.pwm_outputs), &(structs.log_struct), &(structs.actuator_command_struct));
}
else {
pwm_kill();
@@ -125,18 +124,16 @@ int main()
}
if (this_kill_condition == 1 && last_kill_condition == 0) {
- // Just disabled
- printLogging(&(structs.log_struct), &(structs.parameter_struct));
- resetLogging();
- MIO7_led_off();
+ // Just disabled
+ printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+ resetLogging();
+ MIO7_led_off();
}
last_kill_condition = this_kill_condition;
}
- pwm_kill();
-
-
+ kill_motors();
flash_MIO_7_led(10, 100);
diff --git a/quad/sw/modular_quad_pid/src/mio7_led.c b/quad/sw/modular_quad_pid/src/mio7_led.c
index 4742add5b..1da5ad79f 100644
--- a/quad/sw/modular_quad_pid/src/mio7_led.c
+++ b/quad/sw/modular_quad_pid/src/mio7_led.c
@@ -5,49 +5,29 @@
* Author: Amy Seibert
*/
- #include "mio7_led.h"
-
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
-{
- if(how_many_times <= 0)
- return;
-
- while(how_many_times)
- {
- MIO7_led_on();
+#include "mio7_led.h"
- usleep(ms_between_flashes * 500);
+static struct LEDDriver *mio7_led;
- MIO7_led_off();
-
- usleep(ms_between_flashes * 500);
-
- how_many_times--;
- }
+void mio7_init_global(struct LEDDriver *given_mio7_led) {
+ mio7_led = given_mio7_led;
}
-void MIO7_led_off()
+void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Disable LED
- XGpioPs_WritePin(&Gpio, 7, 0x00);
+ int i;
+ for (i = 0; i < how_many_times; i += 1) {
+ mio7_led->turn_on(mio7_led);
+ usleep(ms_between_flashes * 500);
+ mio7_led->turn_off(mio7_led);
+ usleep(ms_between_flashes * 500);
+ }
}
-void MIO7_led_on()
-{
- XGpioPs Gpio;
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Enable LED
- XGpioPs_WritePin(&Gpio, 7, 0x01);
+void MIO7_led_on() {
+ mio7_led->turn_on(mio7_led);
+}
+void MIO7_led_off() {
+ mio7_led->turn_off(mio7_led);
}
diff --git a/quad/sw/modular_quad_pid/src/mio7_led.h b/quad/sw/modular_quad_pid/src/mio7_led.h
index 2517f6241..157a04691 100644
--- a/quad/sw/modular_quad_pid/src/mio7_led.h
+++ b/quad/sw/modular_quad_pid/src/mio7_led.h
@@ -9,8 +9,8 @@
#define MIO7_LED_H_
#include <stdio.h>
-#include <xgpiops.h>
#include "sleep.h"
+#include "hardware/hw_iface.h"
/**
* @brief
@@ -25,18 +25,9 @@
*/
void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
-/**
- * @brief
- * Turns off MIO7 LED.
- *
- */
+void mio7_init_global(struct LEDDriver *given_mio7_led);
+void MIO7_led_on();
void MIO7_led_off();
-/**
- * @brief
- * Turns on MIO7 LED.
- *
- */
-void MIO7_led_on();
#endif /* MIO7_LED_H_ */
diff --git a/quad/sw/modular_quad_pid/src/new_log_data.h b/quad/sw/modular_quad_pid/src/new_log_data.h
index 932eb1a92..13e713a9e 100644
--- a/quad/sw/modular_quad_pid/src/new_log_data.h
+++ b/quad/sw/modular_quad_pid/src/new_log_data.h
@@ -13,7 +13,6 @@
#include <string.h>
#include "PID.h"
#include "type_def.h"
-#include "uart.h"
#include "sleep.h"
#include "communication.h"
diff --git a/quad/sw/modular_quad_pid/src/old_log_data.h b/quad/sw/modular_quad_pid/src/old_log_data.h
index 932eb1a92..13e713a9e 100644
--- a/quad/sw/modular_quad_pid/src/old_log_data.h
+++ b/quad/sw/modular_quad_pid/src/old_log_data.h
@@ -13,7 +13,6 @@
#include <string.h>
#include "PID.h"
#include "type_def.h"
-#include "uart.h"
#include "sleep.h"
#include "communication.h"
diff --git a/quad/sw/modular_quad_pid/src/packet_processing.c b/quad/sw/modular_quad_pid/src/packet_processing.c
index c6764a00b..5dc8df320 100644
--- a/quad/sw/modular_quad_pid/src/packet_processing.c
+++ b/quad/sw/modular_quad_pid/src/packet_processing.c
@@ -5,7 +5,6 @@
* Author: ucart
*/
#include "packet_processing.h"
-#include "uart.h"
#include "type_def.h"
#include "sleep.h"
#include "util.h"
diff --git a/quad/sw/modular_quad_pid/src/send_actuator_commands.c b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
index 3ef6f1a87..efc4e44f7 100644
--- a/quad/sw/modular_quad_pid/src/send_actuator_commands.c
+++ b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
@@ -8,13 +8,12 @@
#include "send_actuator_commands.h"
#include "util.h"
-int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
-{
- int i;
- // write the PWMs to the motors
- for (i = 0; i < 4; i++)
- pwm_write_channel(actuator_command_struct->pwms[i], i);
+int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct) {
+ int i;
+ // write the PWMs to the motors
+ for (i = 0; i < 4; i++) {
+ pwm_outputs->write(pwm_outputs, i, actuator_command_struct->pwms[i]);
+ }
- return 0;
+ return 0;
}
-
diff --git a/quad/sw/modular_quad_pid/src/send_actuator_commands.h b/quad/sw/modular_quad_pid/src/send_actuator_commands.h
index ca337ad7b..3f6fb0fc8 100644
--- a/quad/sw/modular_quad_pid/src/send_actuator_commands.h
+++ b/quad/sw/modular_quad_pid/src/send_actuator_commands.h
@@ -27,6 +27,6 @@
* error message
*
*/
-int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct);
+int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct);
#endif /* SEND_ACTUATOR_COMMANDS_H_ */
diff --git a/quad/sw/modular_quad_pid/src/sensor.h b/quad/sw/modular_quad_pid/src/sensor.h
index d9592b0ea..45791a5aa 100644
--- a/quad/sw/modular_quad_pid/src/sensor.h
+++ b/quad/sw/modular_quad_pid/src/sensor.h
@@ -14,7 +14,6 @@
#include "user_input.h"
#include "iic_utils.h"
#include "packet_processing.h"
-#include "uart.h"
/**
* @brief
diff --git a/quad/sw/modular_quad_pid/src/timer.c b/quad/sw/modular_quad_pid/src/timer.c
index c82b373a5..29b697b5e 100644
--- a/quad/sw/modular_quad_pid/src/timer.c
+++ b/quad/sw/modular_quad_pid/src/timer.c
@@ -11,67 +11,55 @@
#include "xtime_l.h"
#include <xtmrctr.h>
-static XTime before = 0;
-static XTime after = 0;
-static XTmrCtr axi_timer;
-static float LOOP_TIME;
-static float time_stamp = 0;
+u32 before = 0, after = 0;
+float LOOP_TIME;
+float time_stamp = 0;
-int timer_init()
-{
+struct TimerDriver *global_timer;
+struct TimerDriver *axi_timer;
- // using a axi_timer core because we've had problems with the Global Timer
- return XTmrCtr_Initialize(&axi_timer, XPAR_AXI_TIMER_0_DEVICE_ID);
+void timer_init_globals(struct TimerDriver *given_global_timer, struct TimerDriver *given_axi_timer) {
+ global_timer = given_global_timer;
+ axi_timer = given_axi_timer;
}
int timer_start_loop()
{
- //timing code
- LOOP_TIME = ((float)(after - before)) / ((float) COUNTS_PER_SECOND);
- XTime_GetTime(&before);
- XTmrCtr_Reset(&axi_timer, 0);
- XTmrCtr_Start(&axi_timer, 0);
-
- return 0;
+ //timing code
+ LOOP_TIME = ((float)(after - before));
+ global_timer->read(global_timer, &before);
+ axi_timer->restart(axi_timer);
+ return 0;
}
int timer_end_loop(log_t *log_struct)
{
- // get number of useconds its taken to run the loop thus far
- int usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
-
- // attempt to make each loop run for the same amount of time
- while(usec_loop < DESIRED_USEC_PER_LOOP)
- {
- usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
- }
-
-// static int counter = 0;
-// counter++;
-// if(counter % 50 == 0)
-// printf("usec_loop: %d\n", usec_loop);
-
- //timing code
- XTime_GetTime(&after);
- time_stamp += LOOP_TIME;
- XTmrCtr_Stop(&axi_timer, 0);
+ // get number of useconds its taken to run the loop thus far
+ u32 usec_loop;
+ axi_timer->read(axi_timer, &usec_loop);
- // for timing debugging, its a separate hardware PL timer not associated with the PS
- // used this to compare to the PS clock to make sure the timing matched
-// float axi_timer_val = ((float)XTmrCtr_GetValue(&axi_timer, 0)) / ((float)100000000);
+ // attempt to make each loop run for the same amount of time
+ while(usec_loop < DESIRED_USEC_PER_LOOP) {
+ axi_timer->read(axi_timer, &usec_loop);
+ }
+ //timing code
+ global_timer->read(global_timer, &after);
+ time_stamp += LOOP_TIME;
- // Log the timing information
- log_struct->time_stamp = time_stamp;
- log_struct->time_slice = LOOP_TIME;
+ // Log the timing information
+ log_struct->time_stamp = time_stamp;
+ log_struct->time_slice = LOOP_TIME;
- return 0;
+ return 0;
}
float get_last_loop_time() {
- return LOOP_TIME;
+ return LOOP_TIME;
}
-uint32_t timer_get_count() {
- return XTmrCtr_GetValue(&axi_timer, 0);
+u32 timer_get_count() {
+ u32 time;
+ return axi_timer->read(axi_timer, &time);
+ return time;
}
diff --git a/quad/sw/modular_quad_pid/src/timer.h b/quad/sw/modular_quad_pid/src/timer.h
index 5ebc293e8..c12a2f6eb 100644
--- a/quad/sw/modular_quad_pid/src/timer.h
+++ b/quad/sw/modular_quad_pid/src/timer.h
@@ -13,18 +13,6 @@
// desired loop time is not guaranteed (its possible that the loop may take longer than desired)
#define DESIRED_USEC_PER_LOOP 5000 // gives 5ms loops
-#define PL_CLK_CNTS_PER_USEC 100
-
-/**
- * @brief
- * Initializes the items necessary for loop timing.
- *
- * @return
- * error message
- *
- */
-int timer_init();
-
/**
* @brief
* Does processing of the loop timer at the beginning of the control loop.
@@ -50,4 +38,5 @@ float get_last_loop_time();
uint32_t timer_get_count();
+void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
#endif /* TIMER_H_ */
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index e67da1705..2a9de7fb6 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -11,6 +11,14 @@
#include <stdint.h>
#include "commands.h"
#include "computation_graph.h"
+#include "hardware/hw_iface.h"
+
+typedef unsigned char u8;
+typedef unsigned short u16;
+typedef unsigned long u32;
+typedef signed char i8;
+typedef signed short i16;
+typedef signed long i32;
/**
* @brief
@@ -140,7 +148,7 @@ typedef struct PID_values{
*
*/
typedef struct user_input_t {
- int rc_commands[6]; // Commands from the RC transmitter
+ u32 rc_commands[6]; // Commands from the RC transmitter
// float cam_x_pos; // Current x position from the camera system
@@ -371,6 +379,29 @@ typedef struct actuator_command_t {
int pwms[4];
} actuator_command_t;
+enum PWMChannels {
+ MOTOR_0,
+ MOTOR_1,
+ MOTOR_2,
+ MOTOR_3,
+ RC_INPUT_0,
+ RC_INPUT_1,
+ RC_INPUT_2,
+ RC_INPUT_3,
+ RC_INPUT_4,
+ RC_INPUT_5,
+};
+
+typedef struct hardware_t {
+ struct I2CDriver i2c;
+ struct PWMInputDriver pwm_inputs;
+ struct PWMOutputDriver pwm_outputs;
+ struct UARTDriver uart;
+ struct TimerDriver global_timer;
+ struct TimerDriver axi_timer;
+ struct LEDDriver mio7_led;
+} hardware_t;
+
/**
* @brief
* Structures to be used throughout
@@ -385,6 +416,7 @@ typedef struct modular_structs {
user_defined_t user_defined_struct;
raw_actuator_t raw_actuator_struct;
actuator_command_t actuator_command_struct;
+ hardware_t hardware_struct;
} modular_structs_t;
//////// END MAIN MODULAR STRUCTS
diff --git a/quad/sw/modular_quad_pid/src/uart.c b/quad/sw/modular_quad_pid/src/uart.c
deleted file mode 100644
index 8328bed7b..000000000
--- a/quad/sw/modular_quad_pid/src/uart.c
+++ /dev/null
@@ -1,381 +0,0 @@
-/*
- * uart.c
- *
- * Created on: Nov 10, 2014
- * Author: ucart
- */
-
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include "uart.h"
-#include <xuartps.h>
-#include <xstatus.h>
-
-// Global PS's
-XUartPs* _Uart0PS;
-XUartPs* _Uart1PS;
-static INTC Intc;
-
-//This is copied from xuart driver
-/***************************************************/
-
-#define XUARTPS_MAX_BAUD_ERROR_RATE 3 /* max % error allowed */
-int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
-{
- u8 IterBAUDDIV; /* Iterator for available baud divisor values */
- u32 BRGR_Value; /* Calculated value for baud rate generator */
- u32 CalcBaudRate; /* Calculated baud rate */
- u32 BaudError; /* Diff between calculated and requested baud rate */
- u32 Best_BRGR = 0; /* Best value for baud rate generator */
- u8 Best_BAUDDIV = 0; /* Best value for baud divisor */
- u32 Best_Error = 0xFFFFFFFF;
- u32 PercentError;
- u32 ModeReg;
- u32 InputClk;
-
- /*
- * Asserts validate the input arguments
- */
- Xil_AssertNonvoid(InstancePtr != NULL);
- Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
- //Xil_AssertNonvoid(BaudRate <= XUARTPS_MAX_RATE);
- Xil_AssertNonvoid(BaudRate >= XUARTPS_MIN_RATE);
-
- /*
- * Make sure the baud rate is not impossilby large.
- * Fastest possible baud rate is Input Clock / 2.
- */
- if ((BaudRate * 2) > InstancePtr->Config.InputClockHz) {
- return XST_UART_BAUD_ERROR;
- }
- /*
- * Check whether the input clock is divided by 8
- */
- ModeReg = XUartPs_ReadReg( InstancePtr->Config.BaseAddress,
- XUARTPS_MR_OFFSET);
-
- InputClk = InstancePtr->Config.InputClockHz;
- if(ModeReg & XUARTPS_MR_CLKSEL) {
- InputClk = InstancePtr->Config.InputClockHz / 8;
- }
-
- /*
- * Determine the Baud divider. It can be 4to 254.
- * Loop through all possible combinations
- */
- for (IterBAUDDIV = 4; IterBAUDDIV < 255; IterBAUDDIV++) {
-
- /*
- * Calculate the value for BRGR register
- */
- BRGR_Value = InputClk / (BaudRate * (IterBAUDDIV + 1));
-
- /*
- * Calculate the baud rate from the BRGR value
- */
- CalcBaudRate = InputClk/ (BRGR_Value * (IterBAUDDIV + 1));
-
- /*
- * Avoid unsigned integer underflow
- */
- if (BaudRate > CalcBaudRate) {
- BaudError = BaudRate - CalcBaudRate;
- }
- else {
- BaudError = CalcBaudRate - BaudRate;
- }
-
- /*
- * Find the calculated baud rate closest to requested baud rate.
- */
- if (Best_Error > BaudError) {
-
- Best_BRGR = BRGR_Value;
- Best_BAUDDIV = IterBAUDDIV;
- Best_Error = BaudError;
- }
- }
-
- /*
- * Make sure the best error is not too large.
- */
- PercentError = (Best_Error * 100) / BaudRate;
- if (XUARTPS_MAX_BAUD_ERROR_RATE < PercentError) {
- return XST_UART_BAUD_ERROR;
- }
-
- /*
- * Disable TX and RX to avoid glitches when setting the baud rate.
- */
- XUartPs_DisableUart(InstancePtr);
-
- XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
- XUARTPS_BAUDGEN_OFFSET, Best_BRGR);
- XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
- XUARTPS_BAUDDIV_OFFSET, Best_BAUDDIV);
-
- /*
- * Enable device
- */
- XUartPs_EnableUart(InstancePtr);
-
- InstancePtr->BaudRate = BaudRate;
-
- return XST_SUCCESS;
-
-}
-
-/***************************************************/
-
-/************************************************/
-/************** Main UART Interface *************/
-
-XUartPs* uart_init(XUartPs* uartps_ptr, u16 deviceID, int baudRate) {
- XUartPs_Config* config = XUartPs_LookupConfig(deviceID);
-
- //Configure XUartPs instance
- int Status = XUartPs_CfgInitialize(uartps_ptr, config, config->BaseAddress);
- if (Status != XST_SUCCESS){
- return NULL;
- }
-
- //Set Baudrate for BT
- //XUartPs_SetBaudRate(uartps_ptr, baudRate);
- XUartPs_SetBaudRate_ours(uartps_ptr, baudRate);
-
- return uartps_ptr;
-}
-
-void uart_clearFIFOs(XUartPs* uartps_ptr) {
- //Get UART0 Control Register and clear the TX and RX Fifos
- int* uart_ctrl_reg = (int*) uartps_ptr->Config.BaseAddress;
- *uart_ctrl_reg |= 0x00000003; // clear TX & RX
-}
-
-void uart_sendByte(XUartPs* uartps_ptr, char data) {
- XUartPs_SendByte(uartps_ptr->Config.BaseAddress, data);
-}
-
-void uart_sendStr(XUartPs* uartps_ptr, char* str) {
- uart_sendBytes(uartps_ptr, str, strlen(str));
-}
-
-void uart_sendBytes(XUartPs* uartps_ptr, char* data, int numBytes) {
- while (uart_isSending(uartps_ptr))
- ;
-
- int done = 0;
- while (done < numBytes) {
- done += XUartPs_Send(uartps_ptr, (unsigned char*) (&data[done]), numBytes - done);
- }
-}
-
-int uart_isSending(XUartPs* uartps_ptr) {
- return XUartPs_IsSending(uartps_ptr);
-}
-
-int uart_hasData(XUartPs* uartps_ptr) {
- return XUartPs_IsReceiveData(uartps_ptr->Config.BaseAddress);
-}
-
-void uart_recvBytes(XUartPs* uartps_ptr, char* buffer, int numBytes) {
- int received = 0;
- while (received < numBytes) {
- received += XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[received], (numBytes - received));
- }
-}
-
-char uart_recvByte(XUartPs* uartps_ptr) {
- return XUartPs_RecvByte(uartps_ptr->Config.BaseAddress);
- //char buffer[1];
- //XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[0], 1);
-
-// return buffer[0];
-}
-
-
-/*****************************************************************************/
-/**
-*
-* This function sets up the interrupt system so interrupts can occur for the
-* Uart. This function is application-specific.
-*
-* @param IntcInstancePtr is a pointer to the instance of the INTC.
-* @param UartInstancePtr contains a pointer to the instance of the UART
-* driver which is going to be connected to the interrupt
-* controller.
-* @param UartIntrId is the interrupt Id and is typically
-* XPAR_<UARTPS_instance>_INTR value from xparameters.h.
-*
-* @return XST_SUCCESS if successful, otherwise XST_FAILURE.
-*
-* @note None.
-*
-****************************************************************************/
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
-{
- int Status;
-
- XScuGic_Config *IntcConfig; /* Config for interrupt controller */
-
- /* Initialize the interrupt controller driver */
- IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
- if (NULL == IntcConfig) {
- return XST_FAILURE;
- }
-
- Status = XScuGic_CfgInitialize(&Intc, IntcConfig,
- IntcConfig->CpuBaseAddress);
- if (Status != XST_SUCCESS) {
- return XST_FAILURE;
- }
-
- /*
- * Connect the interrupt controller interrupt handler to the
- * hardware interrupt handling logic in the processor.
- */
- Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
- (Xil_ExceptionHandler) XScuGic_InterruptHandler,
- &Intc);
-
- /*
- * Connect a device driver handler that will be called when an
- * interrupt for the device occurs, the device driver handler
- * performs the specific interrupt processing for the device
- */
- Status = XScuGic_Connect(&Intc, UartIntrId,
- handler,
- (void *) UartInstancePtr);
- if (Status != XST_SUCCESS) {
- return XST_FAILURE;
- }
-
- /* Enable the interrupt for the device */
- XScuGic_Enable(&Intc, UartIntrId);
-
- /* Enable interrupts */
- Xil_ExceptionEnable();
-
- return XST_SUCCESS;
-}
-/************************************************/
-/************************************************/
-
-
-
-
-
-/************************************************/
-/********** UART 0 convenience methods **********/
-
-XUartPs* uart0_init(u16 deviceID, int baudRate){
- if (_Uart0PS) {
- free(_Uart0PS);
- }
- _Uart0PS = malloc(sizeof(XUartPs));
- return uart_init(_Uart0PS, deviceID, baudRate);
-}
-
-// Initializes the interrupt system for UART 0
-int uart0_int_init(u16 UartIntrId, Xil_ExceptionHandler handler) {
- return SetupInterruptSystem(_Uart0PS, UartIntrId, handler);
-}
-
-void uart0_clearFIFOs(){
- uart_clearFIFOs(_Uart0PS);
-}
-
-void uart0_sendByte(u8 data){
- uart_sendByte(_Uart0PS, data);
-}
-
-void uart0_sendStr(char* str) {
- uart_sendStr(_Uart0PS, str);
-}
-
-void uart0_sendMetaData(metadata_t md)
-{
- uart0_sendByte(md.begin_char);
- uart0_sendByte(md.msg_type & 0x00ff);
- uart0_sendByte((md.msg_type >> 8) & 0x00ff);
- uart0_sendByte(md.msg_id & 0x00ff);
- uart0_sendByte((md.msg_id >> 8) & 0x00ff);
- uart0_sendByte(md.data_len & 0x00ff);
- uart0_sendByte((md.data_len >> 8) & 0x00ff);
-}
-
-void uart0_sendBytes(char* data, int numBytes){
- uart_sendBytes(_Uart0PS, data, numBytes);
-}
-
-int uart0_isSending(){
- return uart_isSending(_Uart0PS);
-}
-
-int uart0_hasData(){
- return uart_hasData(_Uart0PS);
-}
-
-void uart0_recvBytes(char* buffer, int numBytes) {
- uart_recvBytes(_Uart0PS, buffer, numBytes);
-}
-
-char uart0_recvByte() {
- return uart_recvByte(_Uart0PS);
-}
-
-/************************************************/
-/************************************************/
-
-
-
-
-
-
-/************************************************/
-/********** UART 1 convenience methods **********/
-
-XUartPs* uart1_init(u16 deviceID, int baudRate){
- if (_Uart1PS) {
- free(_Uart1PS);
- }
- _Uart1PS = malloc(sizeof(XUartPs));
- return uart_init(_Uart1PS, deviceID, baudRate);
-}
-
-void uart1_clearFIFOs(){
- uart_clearFIFOs(_Uart1PS);
-}
-
-void uart1_sendByte(char data){
- uart_sendByte(_Uart1PS, data);
-}
-
-void uart1_sendStr(char* str) {
- uart_sendStr(_Uart1PS, str);
-}
-
-void uart1_sendBytes(char* data, int numBytes){
- uart_sendBytes(_Uart1PS, data, numBytes);
-}
-
-int uart1_isSending(){
- return uart_isSending(_Uart1PS);
-}
-
-int uart1_hasData(){
- return uart_hasData(_Uart1PS);
-}
-
-void uart1_recvBytes(char* buffer, int numBytes) {
- uart_recvBytes(_Uart1PS, buffer, numBytes);
-}
-
-char uart1_recvByte() {
- return uart_recvByte(_Uart1PS);
-}
-
-/************************************************/
-
diff --git a/quad/sw/modular_quad_pid/src/uart.h b/quad/sw/modular_quad_pid/src/uart.h
deleted file mode 100644
index 6629e21af..000000000
--- a/quad/sw/modular_quad_pid/src/uart.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/*
- * uart.h
- *
- * Created on: Nov 10, 2014
- * Author: ucart
- */
-
-#ifndef UART_H_
-#define UART_H_
-
-#include "xparameters.h"
-#include "xuartps.h"
-#include "xscugic.h"
-#include "communication.h"
-
-#define PACKET_START_CHAR 2
-#define PACKET_END_CHAR 3
-#define UPDATE_SIZE 28
-
-#define INTC XScuGic // Interrupt controller type
-#define INTC_DEVICE_ID XPAR_SCUGIC_SINGLE_DEVICE_ID
-
-extern XUartPs* _Uart0PS;
-extern XUartPs* _Uart1PS;
-
-
-/************************************************/
-/************** Main UART Interface *************/
-XUartPs* uart_init(XUartPs* uartps_ptr, u16 deviceID, int baudRate);
-void uart_clearFIFOs(XUartPs* uartps_ptr);
-void uart_sendByte(XUartPs* uartps_ptr, char data);
-void uart_sendStr(XUartPs* uartps_ptr, char* str);
-void uart_sendBytes(XUartPs* uartps_ptr, char* data, int numBytes);
-int uart_isSending(XUartPs* uartps_ptr);
-int uart_hasData(XUartPs* uartps_ptr);
-
-//char uart_recvByte(); // block until char received
-
-//int uart_recvBytes(char* buffer, int numBytes, int timeoutMicros); // block until all received
-
-//void uart_recvCallback(void (*func)(char data));
-
-void uart_recvBytes(XUartPs* uartps_ptr, char* buffer, int numBytes);
-
-char uart_recvByte(XUartPs* uartps_ptr);
-/************************************************/
-/************************************************/
-
-
-
-/************************************************/
-/********** UART 0 convenience methods **********/
-XUartPs* uart0_init(u16 deviceID, int baudRate);
-int uart0_int_init(u16 UartIntrId, Xil_ExceptionHandler handler);
-void uart0_clearFIFOs();
-void uart0_sendByte(u8 data);
-void uart0_sendStr(char* data);
-void uart0_sendBytes(char* data, int numBytes);
-int uart0_isSending();
-int uart0_hasData();
-void uart0_recvBytes(char* buffer, int numBytes);
-char uart0_recvByte();
-void uart0_sendMetaData(metadata_t md);
-/************************************************/
-/************************************************/
-
-
-
-/************************************************/
-/********** UART 1 convenience methods **********/
-XUartPs* uart1_init(u16 deviceID, int baudRate);
-void uart1_clearFIFOs();
-void uart1_sendByte(char data);
-void uart1_sendStr(char* data);
-void uart1_sendBytes(char* data, int numBytes);
-int uart1_isSending();
-int uart1_hasData();
-void uart1_recvBytes(char* buffer, int numBytes);
-char uart1_recvByte();
-/************************************************/
-
-#endif /* UART_H_ */
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.c b/quad/sw/modular_quad_pid/src/uart_buff.c
deleted file mode 100644
index 2ee7edbb8..000000000
--- a/quad/sw/modular_quad_pid/src/uart_buff.c
+++ /dev/null
@@ -1,417 +0,0 @@
-#include <stdio.h>
-#include "uart_buff.h"
-#include <stdlib.h>
-
-// function prototype declarations specific to the circular buffer
-void uart_buff_scan_packet();
-void uart_buff_scan_packet_start();
-void uart_buff_scan_packet_size();
-size_t uart_buff_packet_size();
-size_t uart_buff_calc_index(int);
-
-static volatile u8 buff[UART_MAX_BUFF_SIZE];
-u8 packet_buff[UART_MAX_PACKET_SIZE];
-
-static size_t start = 0;
-static volatile size_t end = 0;
-static size_t packet_data_length = 0;
-
-static u8 packet_ready = 0;
-
-// TODO: Delete these debugging variables
-static u32 packets_processed = 0;
-
-/**
- * Put a byte into the buffer.
- */
-void uart_buff_add_u8(u8 c) {
- if (uart_buff_full()) {
- return;
- }
-
- buff[end] = c;
- end += 1;
- if (end >= UART_MAX_BUFF_SIZE) {
- end -= UART_MAX_BUFF_SIZE;
- }
-}
-
-/**
- * Scan for a packet, updating any necessary indices and flags.
- */
-void uart_buff_scan_packet() {
- size_t scan_limit = uart_buff_size();
- size_t scan_iteration = 0;
- while (!packet_ready && scan_iteration < scan_limit) {
- scan_iteration += 1;
-
- if (buff[start] != 0xBE) {
- start += 1;
- if (start >= UART_MAX_BUFF_SIZE) {
- start -= UART_MAX_BUFF_SIZE;
- }
- continue;
- }
-
- if (uart_buff_size() < UART_PACKET_HEADER_SIZE) {
- // Haven't received the "length" bytes yet. Check back later.
- break;
- }
-
- packet_data_length = (uart_buff_get_u8(6) << 8) | uart_buff_get_u8(5);
- if (uart_buff_packet_size() > UART_MAX_PACKET_SIZE) {
- // Packet size is too big. Abort this packet.
- start += 1;
- if (start >= UART_MAX_BUFF_SIZE) {
- start -= UART_MAX_BUFF_SIZE;
- }
- continue;
- }
-
- if (uart_buff_size() < uart_buff_packet_size()) {
- // Haven't received the whole packet. Check back later.
- break;
- }
-
- packet_ready = 1;
- }
-}
-
-/**
- * Return 1 if a packet is ready to processed, 0 otherwise.
- */
-int uart_buff_packet_ready() {
- if (!packet_ready) {
- uart_buff_scan_packet();
- }
- return packet_ready;
-}
-
-/**
- * Retrieve a 8-bit from the buffer according to the given offset with respect
- * to the start index of the current packet.
- */
-u8 uart_buff_get_u8(size_t offset) {
- size_t index = start + offset;
-
- if (index >= UART_MAX_BUFF_SIZE) {
- index -= UART_MAX_BUFF_SIZE;
- }
-
- return buff[index];
-}
-
-/**
- * Retrieve a 16-bit unsigned integer from the buffer according to the given
- * offset with respect to the start index of the current packet.
- */
-u16 uart_buff_get_u16(size_t offset) {
- size_t index_1 = start + offset + 1;
- size_t index = start + offset;
-
- // Check common cases first
- if (index_1 < UART_MAX_BUFF_SIZE) {
- // in range, no need to correct indices
- }
- else if (index >= UART_MAX_BUFF_SIZE) {
- index_1 -= UART_MAX_BUFF_SIZE;
- index -= UART_MAX_BUFF_SIZE;
- }
- else {
- index_1 -= UART_MAX_BUFF_SIZE;
- }
-
- return buff[index_1] << 8
- | buff[index];
-}
-
-/**
- * Retrieve a 32-bit from the buffer according to the given offset with respect
- * to the start index of the current packet.
- */
-u32 uart_buff_get_u32(size_t offset) {
- size_t index_3 = start + offset + 3;
- size_t index_2 = start + offset + 2;
- size_t index_1 = start + offset + 1;
- size_t index = start + offset;
-
- if (index_3 < UART_MAX_BUFF_SIZE) {
- // in range, no need to correct indices
- }
- else if (index >= UART_MAX_BUFF_SIZE) {
- index_3 -= UART_MAX_BUFF_SIZE;
- index_2 -= UART_MAX_BUFF_SIZE;
- index_1 -= UART_MAX_BUFF_SIZE;
- index -= UART_MAX_BUFF_SIZE;
- }
- else {
- index_3 = uart_buff_calc_index(index_3);
- index_2 = uart_buff_calc_index(index_2);
- index_1 = uart_buff_calc_index(index_1);
- index = uart_buff_calc_index(index);
- }
-
- return buff[index_3] << 24
- | buff[index_2] << 16
- | buff[index_1] << 8
- | buff[index];
-}
-
-/**
- * Retrieve a 8-bit unsigned integer from the buffer according to
- * the given offset with respect to the start of the data portion
- * of the current packet.
- *
- * This has undefined behavior if a packet is not ready.
- */
-u8 uart_buff_data_get_u8(size_t offset) {
- size_t index = start + UART_PACKET_HEADER_SIZE + offset;
-
- if (index >= UART_MAX_BUFF_SIZE) {
- index -= UART_MAX_BUFF_SIZE;
- }
-
- return buff[index];
-}
-
-/**
- * Retrieve a 16-bit unsigned integer from the buffer according to
- * the given offset with respect to the start of the data portion
- * of the current packet.
- *
- * This has undefined behavior if a packet is not ready.
- */
-u16 uart_buff_data_get_u16(size_t offset) {
- size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
- size_t index = start + UART_PACKET_HEADER_SIZE + offset;
-
- // Check common cases first
- if (index_1 < UART_MAX_BUFF_SIZE) {
- // in range, no need to correct indices
- }
- else if (index >= UART_MAX_BUFF_SIZE) {
- index_1 -= UART_MAX_BUFF_SIZE;
- index -= UART_MAX_BUFF_SIZE;
- }
- else {
- index_1 -= UART_MAX_BUFF_SIZE;
- }
-
- return buff[index_1] << 8
- | buff[index];
-}
-
-/**
- * Retrieve a 32-bit unsigned integer from the buffer according to
- * the given offset with respect to the start of the data portion
- * of the current packet.
- *
- * This has undefined behavior if a packet is not ready.
- */
-u32 uart_buff_data_get_u32(size_t offset) {
- size_t index_3 = start + UART_PACKET_HEADER_SIZE + offset + 3;
- size_t index_2 = start + UART_PACKET_HEADER_SIZE + offset + 2;
- size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
- size_t index = start + UART_PACKET_HEADER_SIZE + offset;
-
- if (index_3 < UART_MAX_BUFF_SIZE) {
- // in range, no need to correct indices
- }
- else if (index >= UART_MAX_BUFF_SIZE) {
- index_3 -= UART_MAX_BUFF_SIZE;
- index_2 -= UART_MAX_BUFF_SIZE;
- index_1 -= UART_MAX_BUFF_SIZE;
- index -= UART_MAX_BUFF_SIZE;
- }
- else {
- index_3 = uart_buff_calc_index(index_3);
- index_2 = uart_buff_calc_index(index_2);
- index_1 = uart_buff_calc_index(index_1);
- index = uart_buff_calc_index(index);
- }
-
- return buff[index_3] << 24
- | buff[index_2] << 16
- | buff[index_1] << 8
- | buff[index];
-}
-
-/**
- * Retrieve a 32-bit floating point number from the buffer according to the
- * given offset with respect to the start of the data portion of the current
- * packet.
- *
- * This has undefined behavior if a packet is not ready.
- */
-float uart_buff_data_get_float(size_t offset) {
- size_t index_3 = start + UART_PACKET_HEADER_SIZE + offset + 3;
- size_t index_2 = start + UART_PACKET_HEADER_SIZE + offset + 2;
- size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
- size_t index = start + UART_PACKET_HEADER_SIZE + offset;
-
- if (index_3 < UART_MAX_BUFF_SIZE) {
- // in range, no need to correct indices
- }
- else if (index >= UART_MAX_BUFF_SIZE) {
- index_3 -= UART_MAX_BUFF_SIZE;
- index_2 -= UART_MAX_BUFF_SIZE;
- index_1 -= UART_MAX_BUFF_SIZE;
- index -= UART_MAX_BUFF_SIZE;
- }
- else {
- index_3 = uart_buff_calc_index(index_3);
- index_2 = uart_buff_calc_index(index_2);
- index_1 = uart_buff_calc_index(index_1);
- index = uart_buff_calc_index(index);
- }
-
- union {
- float f;
- int i;
- } x;
-
- x.i = buff[index_3] << 24
- | buff[index_2] << 16
- | buff[index_1] << 8
- | buff[index];
-
- return x.f;
-}
-
-/**
- * Move the start index of the buffer to the end of the current packet.
- * If a packet is not ready, this function does nothing.
- */
-void uart_buff_consume_packet() {
- if (!packet_ready) {
- return;
- }
-
- start = uart_buff_calc_index(start + uart_buff_packet_size());
- packet_ready = 0;
- packets_processed += 1;
-}
-
-/**
- * Return the current number of bytes held in the buffer.
- */
-size_t uart_buff_size() {
- int size = end - start;
- if (size < 0) {
- size += UART_MAX_BUFF_SIZE;
- }
- return size;
-}
-
-/**
- * Return the length of the data portion of the current packet.
- */
-size_t uart_buff_data_length() {
- return packet_data_length;
-}
-
-/**
- * Return the size of the current packet.
- */
-inline
-size_t uart_buff_packet_size() {
- return UART_PACKET_HEADER_SIZE + packet_data_length + 1;
-}
-
-/**
- * Return 1 if the buffer is empty, 0 otherwise.
- */
-int uart_buff_empty() {
- return start == end;
-}
-
-/**
- * Return 1 if the buffer is full, 0 otherwise.
- */
-int uart_buff_full() {
- int effective_end = end + 1;
- if (effective_end >= UART_MAX_BUFF_SIZE) {
- effective_end -= UART_MAX_BUFF_SIZE;
- }
- return start == effective_end;
-}
-
-/**
- * Calculate the actual index from the given index, wrapping around
- * the edges of the circular buffer if necessary.
- */
-size_t uart_buff_calc_index(int given_index) {
- int actual_index = given_index;
- if (actual_index >= UART_MAX_BUFF_SIZE) {
- actual_index -= UART_MAX_BUFF_SIZE;
- }
- return actual_index;
-}
-
-/**
- * Return a pointer to the start of the packet.
- */
-u8 * uart_buff_get_raw(size_t *packet_size) {
- // Copy packet into temp buffer
- *packet_size = uart_buff_packet_size();
- int i;
- for (i = 0; i < *packet_size; i += 1) {
- packet_buff[i] = uart_buff_get_u8(i);
- }
- return packet_buff;
-}
-
-/**
- * Print the buffer with a pretty ASCII art image.
- */
-void uart_buff_print() {
- size_t packet_size = uart_buff_packet_size();
- size_t data_start = uart_buff_calc_index(start + UART_PACKET_HEADER_SIZE);
- size_t data_end = uart_buff_calc_index(start + packet_size);
- int i;
- printf("buffer size = %zu\n", uart_buff_size());
- puts("Buffer:");
- puts(".--.");
- puts("| |");
- puts("| v");
- for (i = 0; i < UART_MAX_BUFF_SIZE; i += 1) {
- printf("| %02X", buff[i]);
- if (i == start) {
- printf("<-- start");
- }
- if (packet_ready) {
- if (i == data_start) {
- printf("<-- data start");
- }
- if (i == data_end) {
- printf("<-- data end");
- }
- }
- if (i == end) {
- printf("<-- end");
- }
- printf("\n");
- }
- puts("| |");
- puts("'--'");
-}
-
-/**
- * Reset the state of the buffer.
- */
-void uart_buff_reset() {
- start = 0;
- end = 0;
- int i;
- for (i = 0; i < UART_MAX_BUFF_SIZE; i += 1) {
- buff[i] = 0;
- }
- packet_ready = 0;
-}
-
-/**
- * Return the total number of packets processed.
- */
-u32 uart_buff_packets_processed() {
- return packets_processed;
-}
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.h b/quad/sw/modular_quad_pid/src/uart_buff.h
deleted file mode 100644
index 4d6ddbe8a..000000000
--- a/quad/sw/modular_quad_pid/src/uart_buff.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef CIRC_BUFF_H
-#define CIRC_BUFF_H
-
-#include "xil_types.h"
-
-#define UART_MAX_BUFF_SIZE 2048
-#define UART_MAX_PACKET_SIZE 256
-#define UART_PACKET_HEADER_SIZE 7
-
-void uart_buff_add_u8(u8);
-int uart_buff_packet_ready();
-u8 uart_buff_get_u8(size_t);
-u16 uart_buff_get_u16(size_t);
-u32 uart_buff_get_u32(size_t);
-u8 uart_buff_data_get_u8(size_t);
-u16 uart_buff_data_get_u16(size_t);
-u32 uart_buff_data_get_u32(size_t);
-float uart_buff_data_get_float(size_t);
-void uart_buff_consume_packet();
-size_t uart_buff_size();
-size_t uart_buff_data_length();
-int uart_buff_empty();
-int uart_buff_full();
-u8 * uart_buff_get_raw(size_t *);
-void uart_buff_print();
-u32 uart_buff_packets_processed();
-void uart_buff_reset();
-
-#endif
diff --git a/quad/sw/modular_quad_pid/src/user_input.c b/quad/sw/modular_quad_pid/src/user_input.c
index b3b4331e2..bcb7478bf 100644
--- a/quad/sw/modular_quad_pid/src/user_input.c
+++ b/quad/sw/modular_quad_pid/src/user_input.c
@@ -6,27 +6,26 @@
*/
#include "user_input.h"
-#include "uart.h"
-#include "controllers.h"
-int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
+int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct)
{
- // Read in values from RC Receiver
- read_rec_all(user_input_struct->rc_commands);
+ // Read in values from RC Receiver
+ struct PWMInputDriver *pwm_inputs = &hardware_struct->pwm_inputs;
+ read_rec_all(pwm_inputs, user_input_struct->rc_commands);
- //create setpoints for manual flight
- // currently in units of radians
- user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));
- user_input_struct->roll_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[ROLL], ROLL_MAX, ROLL_CENTER, ROLL_MIN, ROLL_RAD_TARGET, -(ROLL_RAD_TARGET));
- user_input_struct->pitch_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[PITCH], PITCH_MAX, PITCH_CENTER, PITCH_MIN, PITCH_RAD_TARGET, -(PITCH_RAD_TARGET));
+ //create setpoints for manual flight
+ // currently in units of radians
+ user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));
+ user_input_struct->roll_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[ROLL], ROLL_MAX, ROLL_CENTER, ROLL_MIN, ROLL_RAD_TARGET, -(ROLL_RAD_TARGET));
+ user_input_struct->pitch_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[PITCH], PITCH_MAX, PITCH_CENTER, PITCH_MIN, PITCH_RAD_TARGET, -(PITCH_RAD_TARGET));
- // Listen on bluetooth and if there's a packet,
- // then receive the packet and set hasPacket for later processing
- // "update packet" type processing is done in sensor.c
- // "command packet" type processing is done in control_algorithm.c
- //user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);
+ // Listen on bluetooth and if there's a packet,
+ // then receive the packet and set hasPacket for later processing
+ // "update packet" type processing is done in sensor.c
+ // "command packet" type processing is done in control_algorithm.c
+ //user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);
- return 0;
+ return 0;
}
int kill_condition(user_input_t* user_input_struct)
diff --git a/quad/sw/modular_quad_pid/src/user_input.h b/quad/sw/modular_quad_pid/src/user_input.h
index c17fe91fe..c289e99c9 100644
--- a/quad/sw/modular_quad_pid/src/user_input.h
+++ b/quad/sw/modular_quad_pid/src/user_input.h
@@ -79,7 +79,7 @@
* error message
*
*/
-int get_user_input(log_t* log_struct, user_input_t* user_input_struct);
+int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct);
int kill_condition(user_input_t* user_input_struct);
float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target);
diff --git a/quad/sw/modular_quad_pid/src/util.c b/quad/sw/modular_quad_pid/src/util.c
index be07d3818..9262e49f4 100644
--- a/quad/sw/modular_quad_pid/src/util.c
+++ b/quad/sw/modular_quad_pid/src/util.c
@@ -1,10 +1,3 @@
-/*
- * util.c
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
-
#include "util.h"
#include <stdlib.h>
@@ -17,110 +10,17 @@
#include <sleep.h>
#include "controllers.h"
#include "xparameters.h"
-#include "uart.h"
extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
-//Global variable representing the current pulseW
-int pulseW = pulse_throttle_low;
-
-/**
- * Initializes the PWM output components.
- * Default pulse length = 1 ms
- * Default period length = 2.33 ms
- */
-void pwm_init() {
-
- int* pwm_0 = (int*) PWM_0_ADDR + PWM_PERIOD;
- int* pwm_1 = (int*) PWM_1_ADDR + PWM_PERIOD;
- int* pwm_2 = (int*) PWM_2_ADDR + PWM_PERIOD;
- int* pwm_3 = (int*) PWM_3_ADDR + PWM_PERIOD;
-
- // Initializes all the PWM address to have the correct period width at 450 hz
- *pwm_0 = period_width;
- *pwm_1 = period_width;
- *pwm_2 = period_width;
- *pwm_3 = period_width;
- // Initializes the PWM pulse lengths to be 1 ms
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulse_throttle_low;
-
- usleep(1000000);
-}
-
-/**
- * Writes all PWM components to be the same given pulsewidth
- */
-void pwm_write_all(int pulseWidth) {
- // Check lower and upper bounds
- if (pulseWidth > pulse_throttle_high)
- pulseWidth = pulse_throttle_high;
- if (pulseWidth < pulse_throttle_low)
- pulseWidth = pulse_throttle_low;
- // Set all the pulse widths
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulseWidth;
-}
-/**
- * Write a given pulseWidth to a channel
- */
-void pwm_write_channel(int pulseWidth, int channel){
- // Check lower and upper bounds
- if (pulseWidth > pulse_throttle_high)
- pulseWidth = pulse_throttle_high;
- if (pulseWidth < pulse_throttle_low)
- pulseWidth = pulse_throttle_low;
- switch(channel){
- case 0:
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 1:
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 2:
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 3:
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulseWidth;
- break;
- default:
- break;
- }
-}
-/**
- * Reads the registers from the PWM_Recorders, and returns the pulse width
- * of the last PWM signal to come in
- */
-int read_rec(int channel) {
- switch (channel) {
- case 0:
- return *((int*) PWM_REC_0_ADDR);
- case 1:
- return *((int*) PWM_REC_1_ADDR);
- case 2:
- return *((int*) PWM_REC_2_ADDR);
- case 3:
- return *((int*) PWM_REC_3_ADDR);
- case 4:
- return *((int*) PWM_REC_4_ADDR);
- case 5:
- return *((int*) PWM_REC_5_ADDR);
- default:
- return 0;
- }
-}
/**
- * Reads all 4 receiver channels at once
+ * Reads all 6 receiver channels at once
*/
-void read_rec_all(int* mixer){
- int i;
- for(i = 0; i < 6; i++){
- mixer[i] = read_rec(i);
- }
+void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer){
+ int i;
+ for(i = 0; i < 6; i++){
+ pwm_input->read(pwm_input, i, &mixer[i]);
+ }
}
int hexStrToInt(char *buf, int startIdx, int endIdx) {
@@ -146,131 +46,6 @@ int hexStrToInt(char *buf, int startIdx, int endIdx) {
return result;
}
-void read_bluetooth_all(int* mixer) {
- char buffer[32];
-
- int done = 0;
- int gotS = 0;
- char c = 0;
- while (!done) {
- int counter = 0;
- if (!gotS) {
- c = uart0_recvByte();
- }
- if (c == 'S') {
-
- while (1) {
- char cc = uart0_recvByte();
- if (cc == 'S') {
- counter = 0;
- gotS = 1;
- break;
- }
- printf("C=%c,\r\n",cc);
- buffer[counter++] = cc;
-
-
- if (counter == 12) {
- buffer[12] = 0;
- done = 1;
- gotS = 0;
- }
- }
- //uart0_recvBytes(buffer, 12);
- //buffer[12] = 0;
-
-
- }
- }
-
-// // data := "XX XX XX XX XX"
-// uart0_recvBytes(buffer, 12);
-// buffer[12] = 0;
-//
-//
- int i;
- for(i=0; i < 5; i++) {
- mixer[i] = 0;
- }
-
- for (i=0; i < 4; i++) {
- //mixer[i] = hexStrToInt(buffer, 3*i, 3*i + 1);
- mixer[i] = (buffer[i*3] << 8) | buffer[i*3 + 1];
- }
-
- printf("mixer: \"%s\" -> %d %d %d %d %d\r\n", buffer, mixer[0], mixer[1], mixer[2], mixer[3], mixer[4]);
-
-}
-
-/**
- * Use the buttons to drive the pulse length of the channels
- */
-void b_drive_pulse() {
- int* btns = (int *) XPAR_BTNS_BASEADDR;
-
- // Increment the pulse width by 5% throttle
- if (*btns & 0x1) {
- pulseW += 1000;
- if (pulseW > 200000)
- pulseW = 200000;
- pwm_write_all(pulseW);
- while (*btns & 0x1)
- ;
- } //Decrease the pulse width by 5% throttle
- else if (*btns & 0x2) {
- pulseW -= 1000;
- if (pulseW < 100000) {
- pulseW = 100000;
- }
- pwm_write_all(pulseW);
- while (*btns & 0x2)
- ;
- }
- // Set the pulses back to default
- else if (*btns & 0x4) {
- pulseW = MOTOR_0_PERCENT;
- pwm_write_all(pulseW);
- }
- // Read the pulse width of pwm_recorder 0
- else if (*btns & 0x8) {
- int i;
- for(i = 0; i < 4; i++){
- xil_printf("Channel %d: %d\n", i, read_rec(i));
- }
- //xil_printf("%d\n",pulseW);
- while (*btns & 0x8)
- ;
- }
-}
-
-/**
- * Creates a sine wave driving the motors from 0 to 100% throttle
- */
-void sine_example(){
-
- int* btns = (int *) XPAR_BTNS_BASEADDR;
- /* Sine Wave */
- static double time = 0;
-
- time += .0001;
- pulseW = (int)fabs(sin(time)*(100000)) + 100000;
- //pulseW = (int) (sin(time) + 1)*50000 + 100000;
- if (*btns & 0x1){
- printf("%d", pulseW);
- printf(" %d\n", *(int*) (PWM_0_ADDR + PWM_PULSE));
- }
- pwm_write_all(pulseW);
- usleep(300);
-}
-
-void print_mixer(int * mixer){
- int i;
- for(i = 0; i < 4; i++){
- xil_printf("%d : %d ", i, mixer[i]);
- }
- xil_printf("\n");
-}
-
/**
* Argument is the reading from the pwm_recorder4 which is connected to the gear pwm
* If the message from the receiver is 0 - gear, kill the system by sending a 1
@@ -295,223 +70,29 @@ int read_flap(int flap)
/**
* Turns off the motors
*/
-void pwm_kill(){
- // Initializes the PWM pulse lengths to be 1 ms
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulse_throttle_low;
+void kill_motors(struct PWMOutputDriver *pwm_outputs) {
+ pwm_outputs->write(pwm_outputs, 0, 0);
+ pwm_outputs->write(pwm_outputs, 1, 0);
+ pwm_outputs->write(pwm_outputs, 2, 0);
+ pwm_outputs->write(pwm_outputs, 3, 0);
}
-/**
- * Useful stuff in here in regards to PID tuning, and motor tuning
- * TODO : Scanf string stuff
- * TODO : Make adam do it :DDDDDDDD
- */
-void bluetoothTunePID(char instr, gam_t* gam, PID_t* CFpid, PID_t* Gpid){
- int wasX = 0;
- int wasPid = 0;
- int wasSetpoint = 0;
- int wasLog = 0;
- char buf[100];
-
- switch (instr) {
- case 'P':
- // Change this if tuning other PIDs
- CFpid->Kp += .5;
- wasPid = 1;
- break;
- case 'p':
- CFpid->Kp -= .5;
- wasPid = 1;
- break;
- case 'O':
- CFpid->Kp += .2;
- wasPid = 1;
- break;
- case 'o':
- CFpid->Kp -= .2;
- wasPid = 1;
- break;
-
- case 'I':
- CFpid->Kp += 0.1;
- wasPid = 1;
- break;
- case 'i':
- CFpid->Kp -= 0.1;
- wasPid = 1;
- break;
- case 'W':
- Gpid->Kp += 1;
- wasPid = 1;
- break;
- case 'w':
- Gpid->Kp -= 1;
- wasPid = 1;
- break;
- case 'E':
- Gpid->Kp += 2;
- wasPid = 1;
- break;
- case 'e':
- Gpid->Kp -= 2;
- wasPid = 1;
- break;
- case 'R':
- Gpid->Kp += 5;
- wasPid = 1;
- break;
- case 'r':
- Gpid->Kp -= 5;
- wasPid = 1;
- break;
- case 'D':
- CFpid->Kd += .1;
- wasPid = 1;
- break;
- case 'd':
- CFpid->Kd -= .1;
- wasPid = 1;
- break;
- case 'S':
- CFpid->setpoint += 1;
- wasSetpoint = 1;
- break;
- case 's':
- CFpid->setpoint -= 1;
- wasSetpoint = 1;
- break;
- case '0':
- motor0_bias += 100;
- wasPid = 0;
- break;
- case '1':
- motor1_bias += 100;
- wasPid = 0;
- break;
- case '2':
- motor2_bias += 100;
- wasPid = 0;
- break;
- case '3':
- motor3_bias += 100;
- wasPid = 0;
- break;
- case ')':
- motor0_bias -= 100;
- wasPid = 0;
- break;
- case '!':
- motor1_bias -= 100;
- wasPid = 0;
- break;
- case '@':
- motor2_bias -= 100;
- wasPid = 0;
- break;
- case '#':
- motor3_bias -= 100;
- wasPid = 0;
- break;
- case 'x':
- wasX = 1;
- break;
- case ' ':
- wasLog = 1;
- break;
-/* case 'm':
- pid->setpoint = -45.0;
- // Change set point
- break;
- case 'n':
- pid->setpoint = 45.0;
- */ // Change set point
- default:
- wasPid = 0;
- break;
- }
-
- if(wasX){
- return;
- }
- else if(wasSetpoint){
- sprintf(buf, "Setpoint: %4.1f\n\r", CFpid->setpoint);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else if (wasPid) {
- /*sprintf(buf,
- "PAngle: %8.3f RAngle: %8.3f PID p: %8.3f d: %8.3f\r\n",
- compY, compX, pitchPID.Kp, pitchPID.Kd);*/
-
- sprintf(buf, "CFP Coeff: %4.1f D %4.1f GP Coeff: %4.1f\n\r", CFpid->Kp, CFpid->Kd, Gpid->Kp);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else if (wasLog){
- sprintf(buf, "CX %5.2f GP GX: %5.2f\n\r", 0.0, gam->gyro_xVel_p);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else {
- sprintf(buf, "Motor bias's \t\t0: %d 1: %d 2: %d 3: %d \r\n", motor0_bias,
- motor1_bias, motor2_bias, motor3_bias);
- uart0_sendBytes(buf, strlen(buf));
-
-// sprintf(buf, "P: %3.2f I: %3.2f D: %3.2f\r\n", log_struct.ang_vel_pitch_PID_values.P, log_struct.ang_vel_pitch_PID_values.I, log_struct.ang_vel_pitch_PID_values.D);
- uart0_sendBytes(buf, strlen(buf));
- usleep(1e4);
- }
+int build_int(u8 *buff) {
+ return buff[3] << 24
+ | buff[2] << 16
+ | buff[1] << 8
+ | buff[0];
}
-/*
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
-{
- if(how_many_times <= 0)
- return;
-
- while(how_many_times)
- {
- MIO7_led_on();
-
- usleep(ms_between_flashes * 500);
- MIO7_led_off();
+float build_float(u8 *buff) {
+ union {
+ float f;
+ int i;
+ } x;
- usleep(ms_between_flashes * 500);
-
- how_many_times--;
- }
+ x.i = buff[3] << 24
+ | buff[2] << 16
+ | buff[1] << 8
+ | buff[0];
+ return x.f;
}
-
-void MIO7_led_off()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Disable LED
- XGpioPs_WritePin(&Gpio, 7, 0x00);
-}
-
-void MIO7_led_on()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Enable LED
- XGpioPs_WritePin(&Gpio, 7, 0x01);
-}
-*/
-void msleep(int msecs)
-{
- usleep(msecs * 1000);
-}
-
diff --git a/quad/sw/modular_quad_pid/src/util.h b/quad/sw/modular_quad_pid/src/util.h
index 7c864f4d6..003987759 100644
--- a/quad/sw/modular_quad_pid/src/util.h
+++ b/quad/sw/modular_quad_pid/src/util.h
@@ -1,92 +1,23 @@
-/*
- * util.h
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
#ifndef _UTIL_H
#define _UTIL_H
#include "type_def.h"
-
-
-#define clock_rate 100000000
-#define frequency 450
-#define period_width clock_rate/frequency
-#define pulse_throttle_low clock_rate / 1000 // 1ms
-#define pulse_throttle_high clock_rate / 500 // 2ms
-#define MOTOR_0_PERCENT 115000
-
-
-#define XPAR_BTNS_BASEADDR 0x41200000
-
-/**
- * Various Addresses of custom IP components
- */
-#define PWM_0_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR
-#define PWM_1_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR
-#define PWM_2_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR
-#define PWM_3_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR
-#define PWM_REC_0_ADDR XPAR_PWM_RECORDER_0_BASEADDR
-#define PWM_REC_1_ADDR XPAR_PWM_RECORDER_1_BASEADDR
-#define PWM_REC_2_ADDR XPAR_PWM_RECORDER_2_BASEADDR
-#define PWM_REC_3_ADDR XPAR_PWM_RECORDER_3_BASEADDR
-#define PWM_REC_4_ADDR XPAR_PWM_RECORDER_4_BASEADDR
-#define PWM_REC_5_ADDR XPAR_PWM_RECORDER_5_BASEADDR
-
-/**
- * Register offsets within the custom IP
- */
-#define PWM_PERIOD 0
-#define PWM_PULSE 4
-
-void pwm_init();
-void pwm_write_all(int pulseWidth);
-void pwm_write_channel(int pulseWidth, int channel);
-
-int read_rec(int channel);
-void read_rec_all(int* mixer);
-
-void read_bluetooth_all(int* mixer);
-
-void b_drive_pulse();
-
-void sine_example();
-
-void print_mixer(int* mixer);
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include "PID.h"
+#include "log_data.h"
+#include <sleep.h>
+#include "controllers.h"
+#include "hardware/hw_iface.h"
+
+void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer);
int read_kill(int gear);
int read_flap(int flap);
-void pwm_kill();
-
-void printLogging();
-void bluetoothTunePID(char instr, gam_t* gam, PID_t* CFpid, PID_t* Gpid);
-void msleep(int msecs);
-
-/**
- * @brief
- * Flashes the MIO7 LED how_many_times times and with ms_between_flashes between the flashes.
- *
- * @param how_many_times
- * times the LED should be flashed
- *
- * @param ms_between_flashes
- * time between flashes in milliseconds
- *
- */
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
-
-/**
- * @brief
- * Turns off MIO7 LED.
- *
- */
-void MIO7_led_off();
+void kill_motors();
-/**
- * @brief
- * Turns on MIO7 LED.
- *
- */
-void MIO7_led_on();
+int build_int(u8 *buff);
+float build_float(u8 *buff);
#endif //_UTIL_H
--
GitLab