From bfb98aa41b56b9efa328850a3f0b9b043921c031 Mon Sep 17 00:00:00 2001
From: bertucci <bertucci@iastate.edu>
Date: Sat, 23 Feb 2019 20:36:02 -0600
Subject: [PATCH] Merging quad send_rt_data branch with real-time data transfer
gui branch
---
MicroCART | 2 +-
.../quad_app/RTSensorPacketDocumentation.md | 151 ++++++++
quad/src/quad_app/communication.c | 2 +-
quad/src/quad_app/debug_level_readme.txt | 101 +++++
quad/src/quad_app/debug_rt.c | 366 ++++++++++--------
quad/src/quad_app/debug_rt.h | 100 ++++-
quad/src/quad_app/log_data.c | 109 +++++-
quad/src/quad_app/log_data.h | 7 +
quad/src/quad_app/quad_app.c | 39 +-
quad/src/quad_app/sensor.c | 2 +-
quad/src/quad_app/sensor_processing.c | 6 +-
quad/src/quad_app/type_def.h | 33 +-
12 files changed, 699 insertions(+), 219 deletions(-)
create mode 100644 quad/src/quad_app/RTSensorPacketDocumentation.md
create mode 100644 quad/src/quad_app/debug_level_readme.txt
diff --git a/MicroCART b/MicroCART
index 5600e197a..d01645c05 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 000000000..e2e534320
--- /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 2f0183c98..ad39cdc08 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 000000000..49e181c72
--- /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 d4a61a743..cd9fc7204 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 3e951ea0a..9121a7ed6 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 e3de87aad..201953fa4 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 9c4b8b7d9..76aabb600 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 13685c398..726474478 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 6ae875ce9..eaf9e60ef 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 e91da27f5..3249c115d 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 4d61cb1f0..c520904ca 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;
--
GitLab