diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index 8df1844b8ba7bb74b552835f8766b46b5e291fb5..f4bd83f24ebe1bbd10c7b0b39501e8bbf888bb4f 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/quad/src/quad_app/debug_rt.c
@@ -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));
diff --git a/quad/src/quad_app/debug_rt.h b/quad/src/quad_app/debug_rt.h
index 516f9d0ab084acf25f9fe90dde082a1d4c6defa4..2393c9f50760d9057eea728a94da78dcd1d1babc 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/quad/src/quad_app/debug_rt.h
@@ -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);
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index fee17bff79e88df26cf1def41cc701468f82a2bf..b30736f3a4eb233a849a766de00c0b7e2967c695 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -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));