Skip to content
Snippets Groups Projects
Commit b767a3a1 authored by ucart's avatar ucart
Browse files

continuing work on RT data logging feature

parent dae1711f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/quad_app/callbacks.c
+ 5
− 0
View file @ b767a3a1
......@@ -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;
}
quad/src/quad_app/debug_rt.c
+ 74
− 95
View file @ b767a3a1
......@@ -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;
}
quad/src/quad_app/debug_rt.h
+ 28
− 53
View file @ b767a3a1
#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);
......
quad/src/quad_app/log_data.c
+ 6
− 27
View file @ b767a3a1
......@@ -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
......
quad/src/quad_app/log_data.h
+ 1
− 1
View file @ b767a3a1
......@@ -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.
......
quad/src/quad_app/quad_app.c
+ 0
− 3
View file @ b767a3a1
......@@ -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
......
quad/src/quad_app/type_def.h
+ 58
− 0
View file @ b767a3a1
......@@ -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_ */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment