Skip to content
Snippets Groups Projects
Commit 9e8db90b authored by bertucci's avatar bertucci
Browse files

The work continues

parent 3ba012ef
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
+ 62
− 60
View file @ 9e8db90b
......@@ -19,7 +19,7 @@ void initializeFlags(SensorRTFlags_t * flags) {
flags -> optflowflags = calloc(1, sizeof(OptFlowFlags_t));
flags -> lidarflags = calloc(1, sizeof(lidarFlags_t));
flags -> errorflags = calloc(1, sizeof(SensorErrorFlags_t));
flags -> flag_count = 0;
}
void freeFlags(SensorRTFlags_t * flags){
......@@ -27,6 +27,7 @@ void freeFlags(SensorRTFlags_t * flags){
free(flags -> optflowflags);
free(flags -> lidarflags);
free(flags -> errorflags);
free(flags -> flag_count);
free(flags);
}
......@@ -71,13 +72,14 @@ int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
flags -> errorflags -> optFlow = read_bit(configData, 13); size += read_bit(configData, 13);
flags -> errorflags -> consec_optFlow = read_bit(configData, 12); size += read_bit(configData, 12);
flags -> errorflags -> imu = read_bit(configData, 11); size += read_bit(configData, 11);
flags -> errorflags -> consec_imu = read_bit(configData, 10); size += read_bit(configData, 10);
flags -> errorflags -> consec_imu = read_bit(configData, 10); size += read_bit(configData, 10);
flags -> flag_count = size;
return size;
}
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)
{
char * payload = malloc(sizeof(u32) * size);
char * payload = malloc(sizeof(u32) * (flags -> flag_count));
int currentPosition = 0;
/*
Since it's calloced (all zeros), a simple & operation should set this properly
......@@ -86,8 +88,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> lidar.distance_m) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> lidar.distance_m) / 8;
payload[currentPosition] = (u32)(data -> lidar.distance_m);
currentPosition++;
payload[currentPosition] = (u32)(data -> lidar.distance_m);
currentPosition++;
}
if(flags -> optflowflags -> x_flow)
......@@ -95,10 +97,10 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
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)
......@@ -106,10 +108,10 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
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)
......@@ -117,10 +119,10 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
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)
......@@ -128,20 +130,20 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
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)
{
//(*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++;
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)
......@@ -149,10 +151,10 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
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)
......@@ -160,8 +162,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
payload[currentPosition] = (u32)(data -> gam.gyro_xVel_p);
currentPosition++;
}
if(flags -> imuflags -> gyro_y)
......@@ -169,8 +171,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
payload[currentPosition] = (u32)(data -> gam.gyro_yVel_q);
currentPosition++;
}
if(flags -> imuflags -> gyro_z)
......@@ -178,8 +180,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
payload[currentPosition] = (u32)(data -> gam.gyro_zVel_r);
currentPosition++;
}
if(flags -> imuflags -> acc_x)
......@@ -187,8 +189,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.accel_x) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data->gam.accel_x) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_x);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.accel_x);
currentPosition++;
}
if(flags -> imuflags -> acc_y)
......@@ -196,8 +198,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.accel_y) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.accel_y) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_y);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.accel_y);
currentPosition++;
}
if(flags -> imuflags -> acc_z)
......@@ -205,8 +207,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.accel_z) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.accel_z) / 8;
payload[currentPosition] = (u32)(data -> gam.accel_z);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.accel_z);
currentPosition++;
}
if(flags -> imuflags -> mag_x)
......@@ -214,8 +216,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.mag_x) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.mag_x) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_x);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.mag_x);
currentPosition++;
}
if(flags -> imuflags -> mag_y)
......@@ -223,8 +225,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.mag_y) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data->gam.mag_y) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_y);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.mag_y);
currentPosition++;
}
if(flags -> imuflags -> mag_z)
......@@ -232,8 +234,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.mag_z) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.mag_z) / 8;
payload[currentPosition] = (u32)(data -> gam.mag_z);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.mag_z);
currentPosition++;
}
if(flags -> errorflags -> lidar)
......@@ -241,8 +243,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> lidar.error.errorCount) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> lidar.error.errorCount) / 8;
payload[currentPosition] = (u32)(data -> lidar.error.errorCount);
currentPosition++;
payload[currentPosition] = (u32)(data -> lidar.error.errorCount);
currentPosition++;
}
if(flags -> errorflags -> consec_lidar)
......@@ -250,8 +252,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> lidar.error.consErrorCount) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> lidar.error.consErrorCount) / 8;
payload[currentPosition] = (u32)(data -> lidar.error.consErrorCount);
currentPosition++;
payload[currentPosition] = (u32)(data -> lidar.error.consErrorCount);
currentPosition++;
}
if(flags -> errorflags -> optFlow)
......@@ -259,8 +261,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
payload[currentPosition] = (u32)(data -> optical_flow.error.errorCount);
currentPosition++;
}
if(flags -> errorflags -> consec_optFlow)
......@@ -268,8 +270,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*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++;
payload[currentPosition] = (u32)(data -> optical_flow.error.consErrorCount);
currentPosition++;
}
if(flags -> errorflags -> imu)
......@@ -277,8 +279,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.error.errorCount) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.error.errorCount) / 8;
payload[currentPosition] = (u32)(data -> gam.error.errorCount);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.error.errorCount);
currentPosition++;
}
if(flags -> errorflags -> consec_imu)
......@@ -286,8 +288,8 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
//(*payload) |= ((u32)(data -> gam.error.consErrorCount) << (currentBytePosition * 8));
//currentBytePosition += sizeof(data -> gam.error.consErrorCount) / 8;
payload[currentPosition] = (u32)(data -> gam.error.consErrorCount);
currentPosition++;
payload[currentPosition] = (u32)(data -> gam.error.consErrorCount);
currentPosition++;
}
int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
......
quad/src/quad_app/debug_rt.h
+ 2
− 1
View file @ 9e8db90b
......@@ -54,6 +54,7 @@ typedef struct SensorRTFlags
OptFlowFlags_t * optflowflags;
lidarFlags_t * lidarflags;
SensorErrorFlags_t * errorflags;
int flag_count;
}SensorRTFlags_t;
......@@ -63,7 +64,7 @@ void initializeFlags( SensorRTFlags_t * flag_pointer);
void freeFlags(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);
u32 shift(u32 data, int position);
quad/src/quad_app/quad_app.c
+ 5
− 1
View file @ 9e8db90b
......@@ -37,6 +37,8 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
// Xilinx Platform, Loop Timer, Control Algorithm
int init_error = init_structs(&(structs));
int rt_configured_size = 0;
if (init_error != 0) {
return -1;
}
......@@ -66,7 +68,9 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
get_sensors(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
//Send the sensor data in RT (TODO WILL NEED TO BE EDITED FROM CURRENT CONDITION DUE TO TINA)
send_sensor_data(&flags, &(structs.hardware_struct.comm), &(structs.raw_sensor_struct));
//send_sensor_data(&flags, &(structs.hardware_struct.comm), &(structs.raw_sensor_struct));
if (rt_configured_size != 0)
send_RT_data(&(structs.hardware_struct.comm), &(structs.raw_sensor_struct), &flags, rt_configured_size);
// 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));
......
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