Skip to content
Snippets Groups Projects
Commit 55cd2a74 authored by bertucci's avatar bertucci
Browse files

The work continues

parent 46c36b45
No related branches found
No related tags found
1 merge request!43Resolve "Transition current hardware platform to Vivado"
Hide whitespace changes
Inline Side-by-side
quad/src/quad_app/debug_rt.c
+ 62
− 60
View file @ 55cd2a74
......@@ -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 @ 55cd2a74
......@@ -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 @ 55cd2a74
......@@ -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