From b91a50d66f29e325ab6fb5d3c3ab673c348cef24 Mon Sep 17 00:00:00 2001
From: ucart <ucart_groundstation@iastate.edu>
Date: Mon, 4 Mar 2019 14:44:50 -0600
Subject: [PATCH] beggining to make necessary fixes to Tina's addition to allow
building of functional project
---
groundStation/Makefile | 2 +-
groundStation/src/backend/backend.c | 12 ++++-----
quad/src/quad_app/communication.c | 1 -
quad/src/quad_app/debug_rt.c | 20 +++++++++------
quad/src/quad_app/debug_rt.h | 36 ++-------------------------
quad/src/quad_app/log_data.c | 10 ++++----
quad/src/quad_app/quad_app.c | 18 ++++++++------
quad/src/quad_app/sensor.c | 2 +-
quad/src/quad_app/sensor_processing.c | 4 +--
9 files changed, 39 insertions(+), 66 deletions(-)
diff --git a/groundStation/Makefile b/groundStation/Makefile
index 2ca5036b5..e4eafdd3e 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -7,7 +7,7 @@ CFLAGS= -Wall -pedantic -Wextra -std=gnu99 -g -Wno-unused-parameter -Wno-unused-
CXXFLAGS= -Wall -pedantic -Wextra -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
INCDIR= src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR) $(MASRCDIR) ../quad/inc
-LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat -L../quad/lib -lquad_app -lcommands -lgraph_blocks -lcomputation_graph -lm
+LIBS= -lpthread -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat -L../quad/lib -lquad_app -lcommands -lgraph_blocks -lcomputation_graph -lm
OBJDIR=obj
# Manual Assist Specific Variables
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 26ca928ca..b442db5f3 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -22,8 +22,8 @@
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/stat.h>
-#include <bluetooth/bluetooth.h>
-#include <bluetooth/rfcomm.h>
+//#include <bluetooth/bluetooth.h> Bluetooth not currently used
+//#include <bluetooth/rfcomm.h> Bluetooth not currently used
#include <pthread.h>
#include <assert.h>
#include <errno.h>
@@ -471,7 +471,7 @@ int connectToZybo(int index) {
status = 0;
sock = trackables[index].fifo_rx;
}
- else if (trackables[index].isBluetooth == 1) {
+ /*else if (trackables[index].isBluetooth == 1) { //Bluetooth functionality currently disabled
printf("Using BT Settings\n");
struct sockaddr_rc addr;
@@ -486,7 +486,7 @@ int connectToZybo(int index) {
printf("Attempting to connect to zybo. Please be patient...\n");
// blocking call to connect to socket sock ie. zybo board
status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
- }
+ }*/
else {
printf("Using WIFI settings\n");
struct sockaddr_in addr;
@@ -1005,11 +1005,11 @@ static void quad_recv(int index) {
}
fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
break;
- case SEND_RT_ID:
+ /*case SEND_RT_ID:
quadlog_file = fopen("quad_log_data.txt", "w");
fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
fclose(quadlog_file);
- break;
+ break;*/
case RESPPARAM_ID:
case RESPSOURCE_ID:
case RESPOUTPUT_ID:
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index ad39cdc08..891c4f94e 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -20,7 +20,6 @@ void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
u8 packet[MAX_PACKET_SIZE];
int bytes_recv = 0;
-/Users/tina/Desktop/pythonlibrary/matplotlib
int try_receive_packet(struct UARTDriver *uart) {
int attempts = 0;
while (attempts++ < MAX_PACKET_SIZE) {
diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index cd9fc7204..857156227 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/quad/src/quad_app/debug_rt.c
@@ -24,19 +24,22 @@
SensorRTFlags_t * initializeFlags(void){
SensorRTFlags_t * flags = malloc(sizeof(SensorRTFlags_t));
+#if 0
flags -> imuflags = calloc(sizeof(IMUFlags_t));
flags -> optflowflags = calloc(sizeof(OptFlowFlags_t));
flags -> lidarflags = calloc(sizeof(lidarFlags_t));
flags -> errorFlags = calloc(sizeof(SensorErrorFlags_t));
-
+#endif
}
void freeFlags(SensorRTFlags_t * flags){
+#if 0
free(flags -> imuflags);
free(flags -> optflowflags);
free(flags -> lidarflags);
free(flags -> errorFlags);
free(flags);
+#endif
}
/*
@@ -53,7 +56,7 @@ int process_configuration_packet(float configData, SensorRTFlags_t * flags)
{
int size = 0;
-
+#if 0
flags -> lidarflags -> quadHeight = ReadBit(31); size += ReadBit(31);
flags -> optflowflags -> x_flow = ReadBit(30); size += 2 * ReadBit(30);
flags -> optflowflags -> y_flow = ReadBit(29); size += 2 * ReadBit(29);
@@ -76,7 +79,7 @@ int process_configuration_packet(float configData, SensorRTFlags_t * flags)
flags -> errorflags -> consec_optFlow = ReadBit(12); size += ReadBit(12);
flags -> errorflags -> imu = ReadBit(11); size += ReadBit(11);
flags -> errorflags -> consec_imu = ReadBit(10); size += ReadBit(10);
-
+#endif
return size;
}
@@ -97,11 +100,11 @@ typedef struct raw_sensor {
*/
-int send_RT_data(struct CommDriver *comm, struct raw_sensor_t * data, SensorRTFlags_t * flags, int size)
+int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags, int size)
{
- char * payload = calloc(sizeof(char) * size);
+ char * payload = malloc(sizeof(char) * size);
int currentBytePosition = 0;
-
+#if 0
/*
Since it's calloced (all zeros), a simple & operation should set this properly
*/
@@ -236,8 +239,9 @@ int send_RT_data(struct CommDriver *comm, struct raw_sensor_t * data, SensorRTFl
currentBytePosition++;
}
- int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
+#endif
+ int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
free(payload);
return bytes_sent;
-}
\ No newline at end of file
+}
diff --git a/quad/src/quad_app/debug_rt.h b/quad/src/quad_app/debug_rt.h
index 9121a7ed6..8c685ac24 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/quad/src/quad_app/debug_rt.h
@@ -6,38 +6,6 @@
#define SEND_RT_ID 20
- char gyro_x;
- char gyro_y;
- char gyro_z;
- char acc_x;
- char acc_y;
- char acc_z;
- char mag_x;
- char mag_y;
- char mag_z;
-
-}IMUFlags_t;
-
-typedef struct OptFlowFlags
-{
- char x_flow;
- char y_flow;
- char x_filter;
- char y_filter;
- char x_velocity;
- char y_velocity;
-
-}OptFlowFlags_t;
-
-typedef struct SensorErrorFlags
-{
- char consec_lidar;
- char consec_imu;
- char consec_optFlow;
- char lidar;
- char imu;
- char optFlow;
-
typedef struct lidarFlags
{
char quadHeight;
@@ -91,9 +59,9 @@ typedef struct SensorRTFlags
SensorRTFlags_t * initializeFlags(void);
-int setDebugLevel(SensorRTFlags_t * flags, float flags);
+//int setDebugLevel(SensorRTFlags_t * flags, float flags);
void freeFlags(SensorRTFlags_t * flags);
-int send_RT_data(struct CommDriver *comm, struct raw_sensor_t * data);
+int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags, int size);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 201953fa4..9ee2c975d 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -189,7 +189,7 @@ int log_data(log_t* log_struct, parameter_t* ps)
* Print sensor info
*/
void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors){
-
+#if 0
if(level == debug_level.LEVEL_ONE)
{
@@ -209,7 +209,7 @@ void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
{
}
-
+#endif
if (arrayIndex == 0) {
// Don't send any logs if nothing was logged
return;
@@ -233,7 +233,7 @@ void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
// List Pid Constants
- struct graph_node* node = &ps->graph->nodes[n_nodes];
+ /*struct graph_node* node = &ps->graph->nodes[n_nodes];
if (node->type->type_id == BLOCK_PID) {
double kp, ki, kd, alpha;
kp = graph_get_param_val(ps->graph, n_nodes, 0);
@@ -243,7 +243,7 @@ void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf Alpha = %lf\n", node->name, kp, ki, kd, alpha);
}
-
+ */
// Header names for the pre-defined values
safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z\tmag_x\tmag_y\tmag_z");
@@ -256,7 +256,7 @@ void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
// Print all the recorded block outputs
- const char* block_name = ps->graph->nodes[log_outputs[n_outputs].block_id].name;
+ block_name = ps->graph->nodes[log_outputs[n_outputs].block_id].name;
const char* param_name = ps->graph->nodes[log_outputs[n_outputs].block_id].type->output_names[log_outputs[n_outputs].sub_id];
safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 726474478..d4374ddfa 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -24,11 +24,11 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
modular_structs_t structs = { };
//flags that indicate which rt data to send
- debug_flags_t flags;
+ SensorRTFlags_t flags;
// TEST : only send accelerometer data for now
memset(&flags, 0, sizeof(flags));
- flags.accelerometer = 1;
+ flags.imuflags->acc_x = 1;
// Wire up hardware
setup_hardware(&structs.hardware_struct);
@@ -66,15 +66,15 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// Get data from the sensors and put it into raw_sensor_struct
get_sensors(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
- //Send the sensor data in RT
- send_sensor_data(&flags, &(structs.hardware_struct.comm), &(structs.raw_sensor_struct));
+ //Send the sensor data in RT (TODO WILL NEED TO BE EDITED FROM CURRENT CONDITION DUE TO TINA)
+ //send_sensor_data(&flags, &(structs.hardware_struct.comm), &(structs.raw_sensor_struct));
// Process the sensor data and put it into sensor_struct
sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
- printLoggingRT(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+ //printLoggingRT(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
if (!this_kill_condition) {
// Run the control algorithm
@@ -87,7 +87,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// send the actuator commands
send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
-
+#if 0
//send PID stuff
if(flags -> PID)
{
@@ -98,12 +98,14 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
{
send_actuator_data(&(structs.actuator_command_struct), &structs.hardware_struct.comm);
}
-
- }
+#endif
+ }
+
else {
kill_motors(&(structs.hardware_struct.motors));
}
+
if (!this_kill_condition) {
// Log the data collected in this loop
log_data(&(structs.log_struct), &(structs.parameter_struct));
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index eaf9e60ef..3c0dd0365 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -52,7 +52,7 @@ int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* us
status = lidar->read(lidar, &lidar_val);
updateError(&(raw_sensor_struct->lidar.error), status);
if (status == 0) {
- raw_sensor_struct->lidar -> distance_m = lidar_val.distance_m;
+ raw_sensor_struct->lidar.distance_m = lidar_val.distance_m;
}
status = of->read(of, &raw_sensor_struct->optical_flow);
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 3249c115d..29bccdee9 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -238,7 +238,7 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
flow_gyro_compensation(sensor_struct,
- raw_sensor_struct->lidar ->distance_m,
+ raw_sensor_struct->lidar.distance_m,
sensor_struct->roll_angle_filtered,
sensor_struct->pitch_angle_filtered,
sensor_struct->psi_dot);
@@ -258,7 +258,7 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
static float filtered_alt = 0;
static float last_lidar = 0;
- float this_lidar = -raw_sensor_struct->lidar -> distance_m;
+ float this_lidar = -raw_sensor_struct->lidar.distance_m;
if(this_lidar < (-MAX_VALID_LIDAR)) {
this_lidar = filtered_alt;
}
--
GitLab