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

Continuing to make changes to debug_rt.c

parent 316d933a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/quad_app/debug_rt.c
+ 127
− 49
View file @ 3ba012ef
...@@ -12,20 +12,21 @@ ...@@ -12,20 +12,21 @@
/* /*
Initially sets it so that all debug values are zero Initially sets it so that all debug values are zero
*/ */
SensorRTFlags_t * initializeFlags(sensorRTF_flags_t * flags) { void initializeFlags(SensorRTFlags_t * flags) {
flags = calloc(1, sizeof(SensorRTFlags_t)); flags = calloc(1, sizeof(SensorRTFlags_t));
flags -> imuflags = calloc(1, sizeof(IMUFlags_t)); flags -> imuflags = calloc(1, sizeof(IMUFlags_t));
flags -> optflowflags = calloc(1, sizeof(OptFlowFlags_t)); flags -> optflowflags = calloc(1, sizeof(OptFlowFlags_t));
flags -> lidarflags = calloc(1, sizeof(lidarFlags_t)); flags -> lidarflags = calloc(1, sizeof(lidarFlags_t));
flags -> errorFlags = calloc(1, sizeof(SensorErrorFlags_t)); flags -> errorflags = calloc(1, sizeof(SensorErrorFlags_t));
} }
void freeFlags(SensorRTFlags_t * flags){ void freeFlags(SensorRTFlags_t * flags){
free(flags -> imuflags); free(flags -> imuflags);
free(flags -> optflowflags); free(flags -> optflowflags);
free(flags -> lidarflags); free(flags -> lidarflags);
free(flags -> errorFlags); free(flags -> errorflags);
free(flags); free(flags);
} }
...@@ -76,140 +77,217 @@ int process_configuration_packet(u32 configData, SensorRTFlags_t * flags) ...@@ -76,140 +77,217 @@ int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags, int size) int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags, int size)
{ {
char * payload = malloc(sizeof(char) * size); char * payload = malloc(sizeof(u32) * size);
int currentBytePosition = 0; int currentPosition = 0;
/* /*
Since it's calloced (all zeros), a simple & operation should set this properly Since it's calloced (all zeros), a simple & operation should set this properly
*/ */
if(flags -> lidarflags -> quadHeight){ if(flags -> lidarflags -> quadHeight){
(*payload) &= ((data -> lidar -> distance_m) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> lidar.distance_m) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> lidar.distance_m) / 8;
payload[currentPosition] = (u32)(data -> lidar.distance_m);
currentPosition++;
} }
if(flags -> optflowflags -> x_flow) if(flags -> optflowflags -> x_flow)
{ {
(*payload) &= ((data -> optical_flow -> flow_x_rad) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.flow_x_rad) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> optflowflags -> y_flow) if(flags -> optflowflags -> y_flow)
{ {
(*payload) &= ((data -> optical_flow -> flow_y_rad) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.flow_y_rad) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> optflowflags -> x_filter) if(flags -> optflowflags -> x_filter)
{ {
(*payload) &= ((data -> optical_flow -> xVelFilt) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.xVelFilt) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> optflowflags -> y_filter) if(flags -> optflowflags -> y_filter)
{ {
(*payload) &= ((data -> optical_flow -> yVelFilt) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.yVelFilt) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> optflowflags -> x_velocity) if(flags -> optflowflags -> x_velocity)
{ {
(*payload) &= ((data -> optical_flow -> xVel) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.xVel) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> optflowflags -> y_velocity) if(flags -> optflowflags -> y_velocity)
{ {
(*payload) &= ((data -> optical_flow -> yVel) << (currentBytePosition * 8)); //(*payload) |= ((u64)(data -> optical_flow.yVel) << (currentBytePosition * 8));
currentBytePosition += 2; //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++;
} }
if(flags -> imuflags -> gyro_x) if(flags -> imuflags -> gyro_x)
{ {
(*payload) &= ((data -> gam -> gyro_xVel_p) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.gyro_xVel_p) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.gyro_xVel_p) / 8;
payload[currentPosition] = (u32)(data -> gam.gyro_xVel_p);
currentPosition++;
} }
if(flags -> imuflags -> gyro_y) if(flags -> imuflags -> gyro_y)
{ {
(*payload) &= ((data -> gam -> gyro_yVel_q) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.gyro_yVel_q) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.gyro_yVel_q) / 8;
payload[currentPosition] = (u32)(data -> gam.gyro_yVel_q);
currentPosition++;
} }
if(flags -> imuflags -> gyro_z) if(flags -> imuflags -> gyro_z)
{ {
(*payload) &= ((data -> gam -> gyro_zVel_r) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.gyro_zVel_r) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.gyro_zVel_r) / 8;
payload[currentPosition] = (u32)(data -> gam.gyro_zVel_r);
currentPosition++;
} }
if(flags -> imuflags -> acc_x) if(flags -> imuflags -> acc_x)
{ {
(*payload) &= ((data -> gam -> accel_x) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.accel_x) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data->gam.accel_x) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_x);
currentPosition++;
} }
if(flags -> imuflags -> acc_y) if(flags -> imuflags -> acc_y)
{ {
(*payload) &= ((data -> gam -> accel_y) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.accel_y) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.accel_y) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_y);
currentPosition++;
} }
if(flags -> imuflags -> acc_z) if(flags -> imuflags -> acc_z)
{ {
(*payload) &= ((data -> gam -> accel_z) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.accel_z) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.accel_z) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_z);
currentPosition++;
} }
if(flags -> imuflags -> mag_x) if(flags -> imuflags -> mag_x)
{ {
(*payload) &= ((data -> gam -> mag_x) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.mag_x) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.mag_x) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_x);
currentPosition++;
} }
if(flags -> imuflags -> mag_y) if(flags -> imuflags -> mag_y)
{ {
(*payload) &= ((data -> gam -> mag_y) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.mag_y) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data->gam.mag_y) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_y);
currentPosition++;
} }
if(flags -> imuflags -> mag_z) if(flags -> imuflags -> mag_z)
{ {
(*payload) &= ((data -> gam -> mag_z) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.mag_z) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.mag_z) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_z);
currentPosition++;
} }
if(flags -> errorflags -> lidar) if(flags -> errorflags -> lidar)
{ {
(*payload) &= ((data -> lidar -> error -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> lidar.error.errorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> lidar.error.errorCount) / 8;
payload[currentPosition] = (u32)(data -> lidar.error.errorCount);
currentPosition++;
} }
if(flags -> errorflags -> consec_lidar) if(flags -> errorflags -> consec_lidar)
{ {
(*payload) &= ((data -> lidar -> consErrorCount -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> lidar.error.consErrorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> lidar.error.consErrorCount) / 8;
payload[currentPosition] = (u32)(data -> lidar.error.consErrorCount);
currentPosition++;
} }
if(flags -> errorflags -> optFlow) if(flags -> errorflags -> optFlow)
{ {
(*payload) &= ((data -> optical_flow -> error -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> optical_flow.error.errorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> optical_flow.error.errorCount) / 8;
payload[currentPosition] = (u32)(data -> optical_flow.error.errorCount);
currentPosition++;
} }
if(flags -> errorflags -> consec_optFlow) if(flags -> errorflags -> consec_optFlow)
{ {
(*payload) &= ((data -> optical_flow -> consErrorCount -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> optical_flow.error.consErrorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> optical_flow.error.consErrorCount) / 8;
payload[currentPosition] = (u32)(data -> optical_flow.error.consErrorCount);
currentPosition++;
} }
if(flags -> errorflags -> imu) if(flags -> errorflags -> imu)
{ {
(*payload) &= ((data -> gam -> error -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.error.errorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.error.errorCount) / 8;
payload[currentPosition] = (u32)(data -> gam.error.errorCount);
currentPosition++;
} }
if(flags -> errorflags -> consec_imu) if(flags -> errorflags -> consec_imu)
{ {
(*payload) &= ((data -> gam -> consErrorCount -> errorCount) << (currentBytePosition * 8)); //(*payload) |= ((u32)(data -> gam.error.consErrorCount) << (currentBytePosition * 8));
currentBytePosition++; //currentBytePosition += sizeof(data -> gam.error.consErrorCount) / 8;
payload[currentPosition] = (u32)(data -> gam.error.consErrorCount);
currentPosition++;
} }
int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload)); int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
......
quad/src/quad_app/debug_rt.h
+ 1
− 1
View file @ 3ba012ef
...@@ -57,7 +57,7 @@ typedef struct SensorRTFlags ...@@ -57,7 +57,7 @@ typedef struct SensorRTFlags
}SensorRTFlags_t; }SensorRTFlags_t;
SensorRTFlags_t * initializeFlags(void); void initializeFlags( SensorRTFlags_t * flag_pointer);
//int setDebugLevel(SensorRTFlags_t * flags, float flags); //int setDebugLevel(SensorRTFlags_t * flags, float flags);
......
quad/src/quad_app/type_def.h
+ 1
− 2
View file @ 3ba012ef
...@@ -250,8 +250,7 @@ typedef struct raw_sensor { ...@@ -250,8 +250,7 @@ typedef struct raw_sensor {
gam_t gam; gam_t gam;
lidar_t lidar; lidar_t lidar;
px4flow_t optical_flow; px4flow_t optical_flow;
// Structures to hold the current quad position & orientation // Structures to hold the current quad position & orientation
// This is mostly unused? // This is mostly unused?
quadPosition_t currentQuadPosition; quadPosition_t currentQuadPosition;
......
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