diff --git a/MicroCART b/MicroCART
index 5600e197a414c92426b3535b6c0b69c73e8c6ee3..d01645c0571a335594396942e6ffa1b53bc710f0 160000
--- a/MicroCART
+++ b/MicroCART
@@ -1 +1 @@
-Subproject commit 5600e197a414c92426b3535b6c0b69c73e8c6ee3
+Subproject commit d01645c0571a335594396942e6ffa1b53bc710f0
diff --git a/quad/src/quad_app/RTSensorPacketDocumentation.md b/quad/src/quad_app/RTSensorPacketDocumentation.md
new file mode 100644
index 0000000000000000000000000000000000000000..e2e534320e0c26cc4abd0f4484800da0af06c5f2
--- /dev/null
+++ b/quad/src/quad_app/RTSensorPacketDocumentation.md
@@ -0,0 +1,151 @@
+#Sensor Data - Real Time
+
+### Sends data in "raw sensor struct"
+
+**NOTE**
+It is important to keep the payload as small as possible. So if the code seems strange, keep in mind NOT to change the payload size to be fixed, even if it would be cleaner or easier to understand
+
+* In the main quad loop, a function is called that is "get sensors". It places stuff in the hardware struct into the raw sensor struct.
+* The hardware struct includes all the drivers for the various sensors on the quadcopter. The get sensors function essentially calls "read" on the drivers and places that info into the raw sensor struct
+* Each driver has these things:
+ * reset function pointer
+ * read function pointer
+ * state void pointer
+
+* The raw sensor struct holds these values
+ * optical flow sensor data (px4flow)
+ * lidar data
+ * imu data (gam struct)
+ * UNUSED gps struct
+ * All these structs have an error variable
+ * called SensorError_t struct
+ * has an error count variable
+
+### Structs for sensors
+* lidar struct
+ * distance the quad is from the ground in meters
+ * float (32 bits)
+
+
+* optical flow struct
+ * All the below are doubles (64 bits)
+ * xy flow
+ * xy filter
+ * xy velocity
+
+* imu data
+ * All the below values are floats (32 bits)
+ * gyroscope data
+ * xyz velocity (angular)
+ * accelerometer data
+ * xyz acceleration
+ * magnetometer data
+ * xyz direction
+
+* actuator data
+ *
+
+* SensorError struct (all the above have this)
+ * Errors
+ * Consecutive Errors
+ * all the above is 32 bits wide
+
+
+
+### Packet structure
+
+#### Groundstation to Quad: Sensor RT Data Configuration Packet
+
+| index | Description | Size |
+|---|---|---|
+| 0 | start char | 1 |
+| 1 + 2 | message type - 20 | 2 |
+| 3 + 4 | message id - 0 | 2 |
+| 5 + 6 | # bytes - 14 | 2 |
+| 7 | first 8 variables (see table) | 1 |
+| 8 | second 8 varibales (see table) | 1 |
+| 9 | last 6 variables (see table) | 1 |
+| 10 | reserved for future use | 1 |
+| end| checksum | 1 |
+
+Bit map:
+
+Payload of configuration packet
+
+MSB (Bit 31) -------- LSB (bit 0)
+|Byte 4| Byte 3| Byte 2| Byte 1|
+
+** Byte 4 **
+
+| bit | Description |
+|---|---|
+| 31 | lidar - distance to ground |
+| 30 | optical flow - x flow|
+| 29 | optical flow - y flow|
+| 28 | optical flow - x filter|
+| 27 | optical flow - y filter|
+| 26 | optical flow - x velocity |
+| 25 | optical flow - y velocity |
+| 24 | gyroscope - x velocity|
+
+
+
+** byte 3 **
+
+| bit | Description |
+|---|---|
+| 23 | gyroscope - y velocity|
+| 22 | gyroscope - z velocity|
+| 21 | accelerometer - x acceleration|
+| 20 | accelerometer - y acceleration|
+| 19 | accelerometer - z acceleration|
+| 18 | magnetometer - x |
+| 17 | magnetometer - y |
+| 16 | magnetometer - z |
+
+
+** byte 2**
+
+| bit | Description |
+|---|---|
+| 15 | lidar err total|
+| 14 | consecutive lidar err|
+| 13 | opt flow error total|
+| 12 | consecutive opt flow error|
+| 11 | imu total errors|
+| 10 | consecutive imu errors|
+| 9 | EMPTY |
+| 8 | EMPTY |
+
+
+** byte 1**
+
+| bit | Description |
+|---|---|
+| 7 | EMPTY |
+| 6 | EMPTY |
+| 5 | EMPTY |
+| 4 | EMPTY |
+| 3 | EMPTY |
+| 2 | EMPTY |
+| 1 | EMPTY |
+| 0 | EMPTY |
+
+
+#### Quad to Groundstation Packet - Contains payload data
+
+| index | 0 | 1 + 2 | 3+4 | 5&6 | ? | end |
+|---|---|---|---|---|---|---|
+| description | start char | msg type - SEND_RT_ID | msg id - 0 | # bytes| payload | checksum |
+| bytes | 1 | 2 | 2 | 2 | variable | 1 |
+
+
+* The packet has a msg type of send_rt_id, which has a value of 20
+* The structure of the payload depends on what variables are selected
+ * See "Checklist" below
+ * There are 22 available things to send, but depending on how much data the user wants sent, the payload will vary
+ * They will be sent in the order listed by the configuration packet
+
+
+
+
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index 2f0183c984d3ad1d38ff8a07d3f72a8427675481..ad39cdc081ddcb61e8efb07f967fa712e3b7bb25 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -20,7 +20,7 @@ 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_level_readme.txt b/quad/src/quad_app/debug_level_readme.txt
new file mode 100644
index 0000000000000000000000000000000000000000..49e181c720a7ee1b6df3ea750f345bfe334c83b8
--- /dev/null
+++ b/quad/src/quad_app/debug_level_readme.txt
@@ -0,0 +1,101 @@
+
+DEBUG LEVEL ONE
+
+// Sensor blocks
+ int cur_pitch;
+ int cur_roll;
+ int cur_yaw;
+ int gyro_y;
+ int gyro_x;
+ int gyro_z;
+ int lidar;
+ int flow_vel_x; // optical flow
+ int flow_vel_y;
+ int flow_vel_x_filt;
+ int flow_vel_y_filt;
+ int flow_quality; // Quality value returned by optical flow sensor
+ int flow_distance;
+
+// Sensor processing
+ int yaw_correction;
+ int of_angle_corr; // Corrects for the optical flow mounting angle
+ int of_integ_x; // Integrates the optical flow data
+ int of_integ_y;
+ int of_trim_x; // Trim value for optical flow integrated value
+ int of_trim_y;
+ int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
+ int of_trimmed_y;
+
+
+DEBUG LEVEL TWO
+
+// PID blocks
+ int roll_pid;
+ int pitch_pid;
+ int yaw_pid;
+ int roll_r_pid;
+ int pitch_r_pid;
+ int yaw_r_pid;
+ int x_pos_pid;
+ int y_pos_pid;
+ int alt_pid;
+
+DEBUG LEVEL THREE
+
+// Velocity nodes
+ int x_vel_pid;
+ int y_vel_pid;
+ int x_vel;
+ int y_vel;
+ int x_vel_clamp;
+ int y_vel_clamp;
+ int vel_x_gain;
+ int vel_y_gain;
+
+ // VRPN blocks
+ int vrpn_x;
+ int vrpn_y;
+ int vrpn_alt;
+ int vrpn_pitch, vrpn_roll;
+
+ // RC blocks
+ int rc_pitch;
+ int rc_roll;
+ int rc_yaw;
+ int rc_throttle;
+
+ // Desired positions
+ int x_set;
+ int y_set;
+ int alt_set;
+ int yaw_set;
+
+DEBUG LEVEL FOUR
+
+
+ // Clamps
+ int clamp_d_pwmP;
+ int clamp_d_pwmR;
+ int clamp_d_pwmY;
+ int yaw_clamp;
+ // Loop times
+ int angle_time;
+ int pos_time;
+
+ // Signal mixer
+ int mixer;
+ int throttle_trim;
+ int throttle_trim_add;
+ int pitch_trim;
+ int pitch_trim_add;
+ int yaw_trim;
+ int yaw_trim_add;
+
+ //psi dot integration chain
+ int psi_dot;
+ int psi_dot_offset;
+ int psi_dot_sum;
+ int psi;
+ int psi_offset;
+ int psi_sum;
+ int mag_yaw; //Complementary filtered magnetometer/gyro yaw
diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index d4a61a7430f7e09a943307f5e25269a569e311ab..cd9fc7204d9a55d383965e81671456cf5e218157 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/quad/src/quad_app/debug_rt.c
@@ -9,189 +9,235 @@
#include "computation_graph.h"
#include "timer.h"
-//use int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t size) to send information to the via uart to the wifi chip
-//the uart driver is in the comm driver
-//send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
-//where is LOG ID? Maybe it will compile any if I just write it as is?
+
+#define ReadBit(data, position) ( (data>>y) & 1)
+#define SetBit(data,position) data |= (1 << y)
+#define ClearBit(data,y) data &= ~(1 << y)
-//typedef struct hardware_t : this is where you access the comm driver
/*
-
- typedef struct hardware_t {
- struct I2CDriver i2c_0;
- struct I2CDriver i2c_1;
- struct RCReceiverDriver rc_receiver;
- struct MotorDriver motors;
- struct UARTDriver uart_0;
- struct UARTDriver uart_1;
- struct GPSDriver gps;
- struct CommDriver comm;
- struct TimerDriver global_timer;
- struct TimerDriver axi_timer;
- struct LEDDriver mio7_led;
- struct SystemDriver sys;
- struct IMUDriver imu;
- struct LidarDriver lidar;
- struct OpticalFlowDriver of;
- } hardware_t;
-
- struct CommDriver {
- struct UARTDriver *uart; <-- ???
- };
-
- */
+ Initially sets it so that all debug values are zero
+*/
+SensorRTFlags_t * initializeFlags(void){
+
+ SensorRTFlags_t * flags = malloc(sizeof(SensorRTFlags_t));
+ flags -> imuflags = calloc(sizeof(IMUFlags_t));
+ flags -> optflowflags = calloc(sizeof(OptFlowFlags_t));
+ flags -> lidarflags = calloc(sizeof(lidarFlags_t));
+ flags -> errorFlags = calloc(sizeof(SensorErrorFlags_t));
+
+}
+
+void freeFlags(SensorRTFlags_t * flags){
+ free(flags -> imuflags);
+ free(flags -> optflowflags);
+ free(flags -> lidarflags);
+ free(flags -> errorFlags);
+ free(flags);
+}
/*
-typedef struct modular_structs {
- 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;
- hardware_t hardware_struct;
-} modular_structs_t;
-
- */
+Uses the payload from the configuration packet (it's in the flag parameter) to set what should be sent
+Size of float = 32 bits
+
+As of now many bits are empty and wont be processed
+See RTSensorPacketDocumentation.md for which flags each bit of the config data corresponds to
+
+It returns the size of the packet that will be sent, so the payload is correctly determined
+*/
+
+int process_configuration_packet(float configData, SensorRTFlags_t * flags)
+{
+
+ int size = 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);
+ flags -> optflowflags -> x_filter = ReadBit(28); size += 2 * ReadBit(28);
+ flags -> optflowflags -> y_filter = ReadBit(27); size += 2 * ReadBit(27);
+ flags -> optflowflags -> x_velocity = ReadBit(26); size += 2 * ReadBit(26);
+ flags -> optflowflags -> y_velocity = ReadBit(25); size += 2 * ReadBit(25);
+ flags -> imuflags -> gyro_x = ReadBit(24); size += ReadBit(24);
+ flags -> imuflags -> gyro_y = ReadBit(23); size += ReadBit(23);
+ flags -> imuflags -> gyro_z = ReadBit(22); size += ReadBit(22);
+ flags -> imuflags -> acc_x = ReadBit(21); size += ReadBit(21);
+ flags -> imuflags -> acc_y = ReadBit(20); size += ReadBit(20);
+ flags -> imuflags -> acc_z = ReadBit(19); size += ReadBit(19);
+ flags -> imuflags -> mag_x = ReadBit(18); size += ReadBit(18);
+ flags -> imuflags -> mag_y = ReadBit(17); size += ReadBit(17);
+ flags -> imuflags -> mag_z = ReadBit(16); size += ReadBit(16);
+ flags -> errorflags -> lidar = ReadBit(15); size += ReadBit(15);
+ flags -> errorflags -> consec_lidar = ReadBit(14); size += ReadBit(14);
+ flags -> errorflags -> optFlow = ReadBit(13); size += ReadBit(13);
+ 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);
+
+ return size;
+}
/*
-
- int send_actuator_commands(struct MotorDriver *motors, log_t* log_struct, actuator_command_t* actuator_command_struct) {
- int i;
- // write the PWMs to the motors
-
- for (i = 0; i < 4; i++) {
- motors->write(motors, i, actuator_command_struct->motor_magnitudes[i]);
- }
-
- return 0;
- }
-
- */
-
-int set_debug_level(enum debug_data_types type, int state){
- switch(type)
- {
- case CAMERA:
- flags->camera = state;
- break;
- case GYRO:
- flags->gyro = state;
- break;
- case ACCELEROMETER:
- flags->accelerometer = state;
- break;
- case LIDAR:
- flags->lidar = state;
- break;
- case OPT_FLOW:
- flags->optical_flow = state;
- break;
- case GPS:
- flags->GPS = state;
- break;
- case PID:
- flags->PID = state;
- break;
- case RC_TRANS:
- flags->RC_transmitter = state;
- break;
- case ACTUATOR:
- flags->actuator = state;
- break;
- case default:
- break;
+
+typedef struct raw_sensor {
+
+ gam_t gam;
+ lidar_t lidar;
+ px4flow_t optical_flow;
+
+ // Structures to hold the current quad position & orientation
+ // This is mostly unused?
+ quadPosition_t currentQuadPosition;
+
+} raw_sensor_t;
+
+
+*/
+
+int send_RT_data(struct CommDriver *comm, struct raw_sensor_t * data, SensorRTFlags_t * flags, int size)
+{
+ char * payload = calloc(sizeof(char) * size);
+ int currentBytePosition = 0;
+
+ /*
+ Since it's calloced (all zeros), a simple & operation should set this properly
+ */
+ if(flags -> lidarflags -> quadHeight){
+
+ (*payload) &= ((data -> lidar -> distance_m) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> optflowflags -> x_flow)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
+ }
+
+ if(flags -> optflowflags -> y_flow)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
}
-}
+ if(flags -> optflowflags -> x_filter)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
+ }
+ if(flags -> optflowflags -> y_filter)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
+ }
+ if(flags -> optflowflags -> x_velocity)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
+ }
-char get_debug_level(enum debug_data_types type){
+ if(flags -> optflowflags -> y_velocity)
+ {
+ (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+ currentBytePosition += 2;
+ }
- switch(type)
- {
- case CAMERA:
- return flags->camera;
- break;
- case GYRO:
- return flags->gyro;
- break;
- case ACCELEROMETER:
- return flags->accelerometer;
- break;
- case LIDAR:
- return flags->lidar;
- break;
- case OPT_FLOW:
- return flags->optical_flow;
- break;
- case GPS:
- return flags->GPS;
- break;
- case PID:
- return flags->PID;
- break;
- case RC_TRANS:
- return flags->RC_transmitter;
- break;
- case ACTUATOR:
- return flags->actuator;
- break;
- case default:
- break;
+ if(flags -> imuflags -> gyro_x)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> gyro_y)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
}
-}
-int send_debug_data(enum debug_data_types type){
+ if(flags -> imuflags -> gyro_z)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> acc_x)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> acc_y)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> acc_z)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> mag_x)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> mag_y)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> imuflags -> mag_z)
+ {
+ (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
- if(!get_debug_level(type))
+ if(flags -> errorflags -> lidar)
{
- return ERROR;
+ (*payload) &= ((data -> lidar -> error -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> errorflags -> consec_lidar)
+ {
+ (*payload) &= ((data -> lidar -> consErrorCount -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
}
- switch(type)
- {
- case CAMERA:
-
- break;
- case GYRO:
-
- break;
- case ACCELEROMETER:
-
- break;
- case LIDAR:
-
- break;
- case OPT_FLOW:
-
- break;
- case GPS:
-
- break;
- case PID:
-
- break;
- case RC_TRANS:
-
- break;
- case ACTUATOR:
- for (i = 0; i < 4; i++) {
- actuator_command_struct->motor_magnitudes[i]
- }
- break;
- case default:
- break;
+ if(flags -> errorflags -> optFlow)
+ {
+ (*payload) &= ((data -> optical_flow -> error -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
}
-}
+ if(flags -> errorflags -> consec_optFlow)
+ {
+ (*payload) &= ((data -> optical_flow -> consErrorCount -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+ if(flags -> errorflags -> imu)
+ {
+ (*payload) &= ((data -> gam -> error -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+
+ if(flags -> errorflags -> consec_imu)
+ {
+ (*payload) &= ((data -> gam -> consErrorCount -> errorCount) << (currentBytePosition * 8));
+ currentBytePosition++;
+ }
+ 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 3e951ea0ae28394a0b79cfce278eceb6f7502d26..9121a7ed665db77768b0767008199fe101a96659 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/quad/src/quad_app/debug_rt.h
@@ -4,30 +4,96 @@
#define OFF 0
#define ERROR -1
-typedef enum debug_data_types
+#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
{
- CAMERA, GYRO, ACCELEROMETER, LIDAR, OPT_FLOW, GPS, PID, RC_TRANS, ACTUATOR
-}
+ char consec_lidar;
+ char consec_imu;
+ char consec_optFlow;
+ char lidar;
+ char imu;
+ char optFlow;
-typedef struct debug_level_flags{
+typedef struct lidarFlags
+{
+ char quadHeight;
- char camera;
- char gyro;
- char accelerometer;
+}lidarFlags_t;
+
+typedef struct IMUFlags
+{
+ 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 optical_flow;
- char GPS;
- char PID;
- char RC_transmitter;
- char actuator;
+ char imu;
+ char optFlow;
+
+}SensorErrorFlags_t;
+
+typedef struct SensorRTFlags
+{
+ IMUFlags_t * imuflags;
+ OptFlowFlags_t * optflowflags;
+ lidarFlags_t * lidarflags;
+ SensorErrorFlags_t * errorflags;
-}debug_flags;
+}SensorRTFlags_t;
-debug_flags flags;
+SensorRTFlags_t * initializeFlags(void);
-int set_debug_level(enum debug_data_types type, int state);
+int setDebugLevel(SensorRTFlags_t * flags, float flags);
-char get_debug_level(enum debug_data_types type);
+void freeFlags(SensorRTFlags_t * flags);
-int send_debug_data( struct CommDriver *comm, );
+int send_RT_data(struct CommDriver *comm, struct raw_sensor_t * data);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index e3de87aadcb8bdd0827cd3ea17850406761ca26d..201953fa442be7361b8b26c8233d8fd4aec24d8a 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -54,6 +54,12 @@ static char units_param_str[512] = {};
static struct str units_output = { .str = units_output_str, .size = 0, .capacity = sizeof(units_output_str)};
static struct str units_param = { .str = units_param_str, .size = 0, .capacity = sizeof(units_output_str)};
+static enum debug_level level;
+
+int set_debug_level(enum debug_level newlevel)
+{
+ level = newlevel;
+}
/*
* Does an sprintf and concatenation. Used just like sprintf, but pass in a pointer to a struct str instead.
* Returns the number of bytes that would have been written (just like snprintf)
@@ -179,6 +185,107 @@ int log_data(log_t* log_struct, parameter_t* ps)
return 0;
}
+/**
+ * Print sensor info
+ */
+void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors){
+
+ if(level == debug_level.LEVEL_ONE)
+ {
+
+ }
+
+ if(level == debug_level.LEVEL_ONE)
+ {
+
+ }
+
+ if(level == debug_level.LEVEL_ONE)
+ {
+
+ }
+
+ if(level == debug_level.LEVEL_ONE)
+ {
+
+ }
+
+ if (arrayIndex == 0) {
+ // Don't send any logs if nothing was logged
+ return;
+ }
+ char buf_arr[2560] = {};
+ struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};
+
+ // Let user know that allocation failed
+ if (arraySize != LOG_STARTING_SIZE) {
+ safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
+ send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+ return;
+ }
+
+ int i;
+ // Comment header
+ //safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+ //safe_sprintf_cat(&buf, "# IMU IIC failures: %d\n", raw_sensors->gam.error.consErrorCount);
+ //safe_sprintf_cat(&buf, "# LiDAR IIC failures: %d\n", raw_sensors->lidar.error.consErrorCount);
+ //safe_sprintf_cat(&buf, "# Optical Flow IIC failures: %d\n", raw_sensors->optical_flow.error.consErrorCount);
+
+ // List Pid Constants
+
+ 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);
+ ki = graph_get_param_val(ps->graph, n_nodes, 1);
+ kd = graph_get_param_val(ps->graph, n_nodes, 2);
+ alpha = graph_get_param_val(ps->graph, n_nodes, 3);
+ 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");
+
+ // Print all the recorded block parameters
+
+ const char* block_name = ps->graph->nodes[log_params[n_params].block_id].name;
+ const char* output_name = ps->graph->nodes[log_params[n_params].block_id].type->param_names[log_params[n_params].sub_id];
+ safe_sprintf_cat(&buf, "\t%s_%s", block_name, output_name);
+
+ // Print all the recorded block outputs
+
+ const char* 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);
+
+ safe_sprintf_cat(&buf, "\n");
+
+ // Send header names
+ send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+
+ // Send units header
+ buf.size = 0;
+ safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s\tuT\tuT\tuT"); // The pre-defined ones
+ safe_sprintf_cat(&buf, units_output.str);
+ safe_sprintf_cat(&buf, units_param.str);
+ safe_sprintf_cat(&buf, "\n");
+ send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+
+ /*************************/
+ /* print & send log data */
+
+ format_log(arrayIndex, log_struct, &buf);
+ send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+ // This is a stupid hack because if the axi timer is not reset in awhile, it always returns 0, and the timer_end_loop() call hangs forever after a long log
+ // Not 100% certain this works
+ timer_axi_reset();
+
+
+ // Empty message of type LOG_END to indicate end of log
+ send_data(comm->uart, LOG_END_ID, 0, (u8*)buf.str, 0);
+}
/**
* Prints all the log information.
@@ -203,7 +310,7 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
int i;
// Comment header
- safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+ safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample number: %d\n", arrayIndex);
safe_sprintf_cat(&buf, "# IMU IIC failures: %d\n", raw_sensors->gam.error.consErrorCount);
safe_sprintf_cat(&buf, "# LiDAR IIC failures: %d\n", raw_sensors->lidar.error.consErrorCount);
safe_sprintf_cat(&buf, "# Optical Flow IIC failures: %d\n", raw_sensors->optical_flow.error.consErrorCount);
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index 9c4b8b7d9cf61f9d86bca21543100b5c48fdb38f..76aabb6005d8ef88d514dcba5e41231758ed7282 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -13,6 +13,8 @@
// Maximum number of block outputs you can record, and maximum number of block parameters to record
#define MAX_LOG_NUM 50
+enum debug_level{LEVEL_ONE, LEVEL_TWO, LEVEL_THREE, LEVEL_FOUR};
+
void initialize_logging(log_t* log_struct, parameter_t* ps);
/**
@@ -44,6 +46,11 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
*/
void updateLog(log_t log_struct);
+/**
+ * Prints the latest log entry
+ */
+void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors);
+
/**
* Prints all the log information.
*/
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 13685c398be72be46a6cfa01c6621c1ff7d4d1c6..726474478d1953bfeea178e5297f1d51a9425c3b 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -9,18 +9,27 @@
#include "log_data.h"
#include "initialize_components.h"
#include "user_input.h"
+#include "log_data.h"
#include "sensor.h"
#include "sensor_processing.h"
#include "control_algorithm.h"
#include "send_actuator_commands.h"
#include "communication.h"
#include "mio7_led.h"
+#include "debug_rt.h"
int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
{
// Structures to be used throughout
modular_structs_t structs = { };
-
+
+ //flags that indicate which rt data to send
+ debug_flags_t flags;
+
+ // TEST : only send accelerometer data for now
+ memset(&flags, 0, sizeof(flags));
+ flags.accelerometer = 1;
+
// Wire up hardware
setup_hardware(&structs.hardware_struct);
@@ -37,9 +46,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
protection_loops(&structs);
int last_kill_condition = kill_condition(&(structs.user_input_struct));
-
-
- //find debug level
// Main control loop
@@ -59,21 +65,40 @@ 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));
// 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));
- //Send the sensor data in RT
+
+ printLoggingRT(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+
if (!this_kill_condition) {
// Run the control algorithm
control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
-
+
+ //print the actuator commands
+ //send actuator_command_t
+
+
// send the actuator commands
send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
- // send actuator to
+ //send PID stuff
+ if(flags -> PID)
+ {
+ &(structs.parameter_struct)
+ }
+ // send actuator data in real time
+ if(flags->actuator)
+ {
+ send_actuator_data(&(structs.actuator_command_struct), &structs.hardware_struct.comm);
+ }
+
}
else {
kill_motors(&(structs.hardware_struct.motors));
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index 6ae875ce931c2ac931de5c18f427acdf85d0fa1a..eaf9e60ef32b74abb5b51ce3146d84ffde61e128 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 e91da27f5910fa10ad241d64f6f904735aaa9806..3249c115d8eddfa09cff4405ef19695803aa1844 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -230,7 +230,7 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
+ (1. - ALPHA) * accel_roll;
// Z-axis points upward, so negate distance
- //sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
+ //sensor_struct->lidar_altitude = -raw_sensor_struct->lidar->distance_m;
//-------- Optical flow -----------//
@@ -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;
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 4d61cb1f088c3947daf2490a65e1376938850db2..c520904cad57e5046365fc597459bb7b476aec25 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -52,6 +52,7 @@ typedef struct commands{
typedef struct raw{
int x,y,z;
}raw;
+
typedef struct PID_Consts{
float P, I, D;
}PID_Consts;
@@ -140,6 +141,7 @@ typedef struct px4flow {
SensorError_t error;
} px4flow_t;
+/** is this used??? */
typedef struct gps {
double lat, lon;
double vel, course;
@@ -245,41 +247,16 @@ typedef struct log_t {
*
*/
typedef struct raw_sensor {
- int acc_x; // accelerometer x data
- int acc_x_t; // time of accelerometer x data
-
- int acc_y; // accelerometer y data
- int acc_y_t; // time of accelerometer y data
-
- int acc_z; // accelerometer z data
- int acc_z_t; // time of accelerometer z data
-
-
- int gyr_x; // gyroscope x data
- int gyr_x_t; // time of gyroscope x data
-
- int gyr_y; // gyroscope y data
- int gyr_y_t; // time of gyroscope y data
-
- int gyr_z; // gyroscope z data
- int gyr_z_t; // time of gyroscope z data
-
- int ldr_z; //lidar z data (altitude)
- int ldr_z_t; //time of lidar z data
gam_t gam;
-
lidar_t lidar;
+ px4flow_t optical_flow;
// Structures to hold the current quad position & orientation
+ // This is mostly unused?
quadPosition_t currentQuadPosition;
- // Distance from the ground, in meters, that the quadcopter is.
- // Ideally equals 0 when landed
- float lidar_distance_m;
-
- // Information obtained from optical flow sensor
- px4flow_t optical_flow;
+
} raw_sensor_t;