From b767a3a1791e4b126934b724e7fa3c0f6d8a4246 Mon Sep 17 00:00:00 2001
From: ucart <ucart_groundstation@iastate.edu>
Date: Sun, 28 Apr 2019 14:31:31 -0500
Subject: [PATCH] continuing work on RT data logging feature
---
quad/src/quad_app/callbacks.c | 5 +
quad/src/quad_app/debug_rt.c | 169 +++++++++++++++-------------------
quad/src/quad_app/debug_rt.h | 81 ++++++----------
quad/src/quad_app/log_data.c | 33 ++-----
quad/src/quad_app/log_data.h | 2 +-
quad/src/quad_app/quad_app.c | 3 -
quad/src/quad_app/type_def.h | 58 ++++++++++++
7 files changed, 172 insertions(+), 179 deletions(-)
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index d4c839bef..13d3a72e1 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -409,3 +409,8 @@ int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned
send_data(structs->hardware_struct.comm.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
return 0;
}
+
+int cb_sendrtdata(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+
+ return 0;
+}
diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index f4bd83f24..4ccee0f6d 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/quad/src/quad_app/debug_rt.c
@@ -2,12 +2,8 @@
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
-#include "PID.h"
-#include "type_def.h"
+
#include "debug_rt.h"
-#include "communication.h"
-#include "computation_graph.h"
-#include "timer.h"
/*
Initially sets it so that all debug values are zero
@@ -40,13 +36,16 @@ u32 read_bit(u32 data, int position){
}
/*
-Uses the payload from the configuration packet (it's in the flag parameter) to set what should be sent
-Size of float = 32 bits
+ This method is used to configure the bits within the flags struct by reading in the data sent from the user via the groundstation.
-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
+ Params:
+ configData: Payload from configuration packet sent from the groundstation. Each bit (starting at 31, downto 10) represents a flag either being disabled (0) or enabled (1)
-It returns the size of the packet that will be sent, so the payload is correctly determined
+ SensorRTFlags_t * flags: This contains a pointer to the flags struct. This struct is held within the quad's memory and is used to determine which information within the quad's sensors
+ will be sent back during each control loop. Due to the limited bandwidth capacity of the UART, it is undesirable to have a large amount of these flags enabled
+ when sending data.
+ Returns:
+ int size: Total size of payload assuming all data enabled by flags are transmitted.
*/
int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
{
@@ -77,222 +76,202 @@ int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
return size;
}
+/*
+ This method populates a payload full of sensor data and transmits it to the groundstation (for real-time data logging feature)
+
+ Params:
+ CommDriver *comm: This contains a pointer to the communication medium currently being used to communicate with the groundstation. In its current state, this will be
+ the UART capabilities located on the quad.
+
+ raw_sensor_t * data: This contains a pointer to the struct containing all of the current sensor data that has been collected by the various peripherals on the quad. This
+ will be used to populate the majority of the information within the payload packet.
+
+ SensorRTFlags_t * flags: This contains a pointer to the flags struct. This struct is held within the quad's memory and is used to determine which information within the sensors
+ will be sent back during each control loop. Due to the limited bandwidth capacity of the UART, it is undesirable to have a large amount of these flags
+ enabled when sending data. The exact contents of the flags struct are detailed in the process_configuration_packet method as well as the struct definition
+ within this file's header.
+
+ Returns:
+ bytes_sent: Total number of bytes that were transferred via the communication channel (UART). This number can be used to determine the succesful transfer of data.
+*/
int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags)
{
- char * payload = malloc(sizeof(u32) * (flags -> flag_count));
+ //include 4 bytes to carry flags info
+ u32 * payload = malloc(sizeof(u32) * (flags -> flag_count) + 4);
int currentPosition = 0;
+ u32 flag_info = 0x00000000;
/*
- Since it's calloced (all zeros), a simple & operation should set this properly
+ Since payload is calloced (all zeros), a simple | operation should set this properly
*/
- if(flags -> lidarflags -> quadHeight){
-
- //(*payload) |= ((u32)(data -> lidar.distance_m) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> lidar.distance_m) / 8;
+ if(flags -> lidarflags -> quadHeight)
+ {
payload[currentPosition] = (u32)(data -> lidar.distance_m);
currentPosition++;
+ //populate correct bit within flag_info. Due to the fact that enabled flags are stored only on the quad's memory, this information will need to be sent to the groundstation with every
+ //packet to ensure the correct formatting of data before being graphed.
+ flag_info |= QUAD_HEIGHT_FLAG;
}
-
+ //64-bit data must be sent in two concurrent 32 bit data blocks within the payload. The MSB's of the data are stored first, then the LSB's.
if(flags -> optflowflags -> x_flow)
{
- //(*payload) |= ((u64)(data -> optical_flow.flow_x_rad) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.flow_x_rad) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.flow_x_rad)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.flow_x_rad)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= X_FLOW_FLAG;
}
if(flags -> optflowflags -> y_flow)
{
- //(*payload) |= ((u64)(data -> optical_flow.flow_y_rad) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.flow_y_rad) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.flow_y_rad)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.flow_y_rad)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= Y_FLOW_FLAG;
}
if(flags -> optflowflags -> x_filter)
{
- //(*payload) |= ((u64)(data -> optical_flow.xVelFilt) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.xVelFilt) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.xVelFilt)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.xVelFilt)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= X_FILTER_FLAG;
}
if(flags -> optflowflags -> y_filter)
{
- //(*payload) |= ((u64)(data -> optical_flow.yVelFilt) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.yVelFilt) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.yVelFilt)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.yVelFilt)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= Y_FILTER_FLAG;
}
if(flags -> optflowflags -> x_velocity)
{
- //(*payload) |= ((u64)(data -> optical_flow.xVel) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.xVel) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.xVel)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.xVel)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= X_VELOCITY_FLAG;
}
if(flags -> optflowflags -> y_velocity)
{
- //(*payload) |= ((u64)(data -> optical_flow.yVel) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.xVel) / 8;
-
payload[currentPosition] = ((u64)(data -> optical_flow.yVel)) >> 32; //MSB
currentPosition++;
payload[currentPosition] = ((u64)(data -> optical_flow.yVel)) & 0x00000000FFFFFFFF; //LSB
currentPosition++;
+ flag_info |= Y_VELOCITY_FLAG;
}
if(flags -> imuflags -> gyro_x)
{
- //(*payload) |= ((u32)(data -> gam.gyro_xVel_p) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.gyro_xVel_p) / 8;
-
payload[currentPosition] = (u32)(data -> gam.gyro_xVel_p);
- currentPosition++;
+ currentPosition++;
+ flag_info |= X_GYRO_FLAG;
}
if(flags -> imuflags -> gyro_y)
{
- //(*payload) |= ((u32)(data -> gam.gyro_yVel_q) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.gyro_yVel_q) / 8;
-
payload[currentPosition] = (u32)(data -> gam.gyro_yVel_q);
currentPosition++;
+ flag_info |= Y_GYRO_FLAG;
}
if(flags -> imuflags -> gyro_z)
{
- //(*payload) |= ((u32)(data -> gam.gyro_zVel_r) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.gyro_zVel_r) / 8;
-
payload[currentPosition] = (u32)(data -> gam.gyro_zVel_r);
- currentPosition++;
+ currentPosition++;
+ flag_info |= Z_GYRO_FLAG;
}
if(flags -> imuflags -> acc_x)
{
- //(*payload) |= ((u32)(data -> gam.accel_x) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data->gam.accel_x) / 8;
-
payload[currentPosition] = (u32)(data -> gam.accel_x);
- currentPosition++;
+ currentPosition++;
+ flag_info |= X_ACCEL_FLAG;
}
if(flags -> imuflags -> acc_y)
{
- //(*payload) |= ((u32)(data -> gam.accel_y) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.accel_y) / 8;
-
payload[currentPosition] = (u32)(data -> gam.accel_y);
- currentPosition++;
+ currentPosition++;
+ flag_info |= Y_ACCEL_FLAG;
}
if(flags -> imuflags -> acc_z)
{
- //(*payload) |= ((u32)(data -> gam.accel_z) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.accel_z) / 8;
-
payload[currentPosition] = (u32)(data -> gam.accel_z);
- currentPosition++;
+ currentPosition++;
+ flag_info |= Z_ACCEL_FLAG;
}
if(flags -> imuflags -> mag_x)
{
- //(*payload) |= ((u32)(data -> gam.mag_x) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.mag_x) / 8;
-
payload[currentPosition] = (u32)(data -> gam.mag_x);
- currentPosition++;
+ currentPosition++;
+ flag_info |= X_MAG_FLAG;
}
if(flags -> imuflags -> mag_y)
{
- //(*payload) |= ((u32)(data -> gam.mag_y) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data->gam.mag_y) / 8;
-
payload[currentPosition] = (u32)(data -> gam.mag_y);
- currentPosition++;
+ currentPosition++;
+ flag_info |= Y_MAG_FLAG;
}
if(flags -> imuflags -> mag_z)
{
- //(*payload) |= ((u32)(data -> gam.mag_z) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.mag_z) / 8;
-
payload[currentPosition] = (u32)(data -> gam.mag_z);
- currentPosition++;
+ currentPosition++;
+ flag_info |= Z_MAG_FLAG;
}
if(flags -> errorflags -> lidar)
{
- //(*payload) |= ((u32)(data -> lidar.error.errorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> lidar.error.errorCount) / 8;
-
payload[currentPosition] = (u32)(data -> lidar.error.errorCount);
- currentPosition++;
+ currentPosition++;
+ flag_info |= LIDAR_ERR_FLAG;
}
if(flags -> errorflags -> consec_lidar)
{
- //(*payload) |= ((u32)(data -> lidar.error.consErrorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> lidar.error.consErrorCount) / 8;
-
payload[currentPosition] = (u32)(data -> lidar.error.consErrorCount);
- currentPosition++;
+ currentPosition++;
+ flag_info |= LIDAR_CONSEC_ERR_FLAG;
}
if(flags -> errorflags -> optFlow)
{
- //(*payload) |= ((u32)(data -> optical_flow.error.errorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.error.errorCount) / 8;
-
payload[currentPosition] = (u32)(data -> optical_flow.error.errorCount);
- currentPosition++;
+ currentPosition++;
+ flag_info |= OPTFL_ERR_FLAG;
}
if(flags -> errorflags -> consec_optFlow)
{
- //(*payload) |= ((u32)(data -> optical_flow.error.consErrorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> optical_flow.error.consErrorCount) / 8;
-
payload[currentPosition] = (u32)(data -> optical_flow.error.consErrorCount);
- currentPosition++;
+ currentPosition++;
+ flag_info |= OPTFL_CONSEC_ERR_FLAG;
}
if(flags -> errorflags -> imu)
{
- //(*payload) |= ((u32)(data -> gam.error.errorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.error.errorCount) / 8;
-
payload[currentPosition] = (u32)(data -> gam.error.errorCount);
currentPosition++;
+ flag_info |= IMU_ERR_FLAG;
}
if(flags -> errorflags -> consec_imu)
{
- //(*payload) |= ((u32)(data -> gam.error.consErrorCount) << (currentBytePosition * 8));
- //currentBytePosition += sizeof(data -> gam.error.consErrorCount) / 8;
-
payload[currentPosition] = (u32)(data -> gam.error.consErrorCount);
- currentPosition++;
+ currentPosition++;
+ flag_info |= IMU_CONSEC_ERR_FLAG;
}
-
- int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
+ //package the last 4 bytes of info as binary representation of the enabled flags
+ payload[currentPosition] = flag_info;
+ int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, sizeof(payload));
free(payload);
return bytes_sent;
}
diff --git a/quad/src/quad_app/debug_rt.h b/quad/src/quad_app/debug_rt.h
index 2393c9f50..22afae72e 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/quad/src/quad_app/debug_rt.h
@@ -1,67 +1,42 @@
#include <stdio.h>
+#include "PID.h"
+#include "type_def.h"
+#include "communication.h"
+#include "computation_graph.h"
+#include "timer.h"
+
#define ON 1
#define OFF 0
#define ERROR -1
#define SEND_RT_ID 20
-typedef struct lidarFlags
-{
- char quadHeight;
-
-}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 imu;
- char optFlow;
-
-}SensorErrorFlags_t;
-
-typedef struct SensorRTFlags
-{
- IMUFlags_t * imuflags;
- OptFlowFlags_t * optflowflags;
- lidarFlags_t * lidarflags;
- SensorErrorFlags_t * errorflags;
- int flag_count;
-
-}SensorRTFlags_t;
+#define QUAD_HEIGHT_FLAG 0x80000000
+#define X_FLOW_FLAG 0x40000000
+#define Y_FLOW_FLAG 0x20000000
+#define X_FILTER_FLAG 0x10000000
+#define Y_FILTER_FLAG 0x08000000
+#define X_VELOCITY_FLAG 0x04000000
+#define Y_VELOCITY_FLAG 0x02000000
+#define X_GYRO_FLAG 0x01000000
+#define Y_GYRO_FLAG 0x00800000
+#define Z_GYRO_FLAG 0x00400000
+#define X_ACCEL_FLAG 0x00200000
+#define Y_ACCEL_FLAG 0x00100000
+#define Z_ACCEL_FLAG 0x00080000
+#define X_MAG_FLAG 0x00040000
+#define Y_MAG_FLAG 0x00020000
+#define Z_MAG_FLAG 0x00010000
+#define LIDAR_ERR_FLAG 0x00008000
+#define LIDAR_CONSEC_ERR_FLAG 0x00004000
+#define OPTFL_ERR_FLAG 0x00002000
+#define OPTFL_CONSEC_ERR_FLAG 0x00001000
+#define IMU_ERR_FLAG 0x00000800
+#define IMU_CONSEC_ERR_FLAG 0x00000400
void initializeFlags( SensorRTFlags_t * flag_pointer);
-//int setDebugLevel(SensorRTFlags_t * flags, float flags);
-
void freeFlags(SensorRTFlags_t * flags);
int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 9ee2c975d..6310f1c1f 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -186,30 +186,9 @@ int log_data(log_t* log_struct, parameter_t* ps)
}
/**
- * Print sensor info
+ * Print current sensor info to log file
*/
-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)
- {
-
- }
-
- if(level == debug_level.LEVEL_ONE)
- {
-
- }
-
- if(level == debug_level.LEVEL_ONE)
- {
-
- }
-
- if(level == debug_level.LEVEL_ONE)
- {
-
- }
-#endif
+void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags){
if (arrayIndex == 0) {
// Don't send any logs if nothing was logged
return;
@@ -226,10 +205,10 @@ void printLoggingRT(struct CommDriver *comm, log_t* log_struct, parameter_t* ps,
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);
+ 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
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index 76aabb600..f74e4cc69 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -49,7 +49,7 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
/**
* Prints the latest log entry
*/
-void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors);
+void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags);
/**
* Prints all the log information.
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 18c73685d..4f62549cc 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -73,9 +73,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_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);
if (!this_kill_condition) {
// Run the control algorithm
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 4fee4d226..d8ad249f2 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -494,6 +494,64 @@ typedef struct modular_structs {
hardware_t hardware_struct;
} modular_structs_t;
+/* BEGIN FLAG STRUCTS CONTAINING ENABLED/DISABLED RT DATA TRANSFERS */
+typedef struct lidarFlags
+{
+ int quadHeight;
+
+}lidarFlags_t;
+
+typedef struct IMUFlags
+{
+ int gyro_x;
+ int gyro_y;
+ int gyro_z;
+ int acc_x;
+ int acc_y;
+ int acc_z;
+ int mag_x;
+ int mag_y;
+ int mag_z;
+
+}IMUFlags_t;
+
+typedef struct OptFlowFlags
+{
+ int x_flow;
+ int y_flow;
+ int x_filter;
+ int y_filter;
+ int x_velocity;
+ int y_velocity;
+
+}OptFlowFlags_t;
+
+typedef struct SensorErrorFlags
+{
+ int consec_lidar;
+ int consec_imu;
+ int consec_optFlow;
+ int lidar;
+ int imu;
+ int optFlow;
+
+}SensorErrorFlags_t;
+
+/*
+ This struct contains all of the above pre-defined flag structs. For use in RT data logging configuration
+*/
+typedef struct SensorRTFlags
+{
+ IMUFlags_t * imuflags;
+ OptFlowFlags_t * optflowflags;
+ lidarFlags_t * lidarflags;
+ SensorErrorFlags_t * errorflags;
+ int flag_count;
+
+}SensorRTFlags_t;
+
+/* END FLAG STRUCTS */
+
//////// END MAIN MODULAR STRUCTS
#endif /* TYPE_DEF_H_ */
--
GitLab