diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index 732c428313b38ecbe72fccc1426d5af8d84ced30..8df1844b8ba7bb74b552835f8766b46b5e291fb5 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/quad/src/quad_app/debug_rt.c
@@ -12,20 +12,21 @@
/*
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 -> imuflags = calloc(1, sizeof(IMUFlags_t));
flags -> optflowflags = calloc(1, sizeof(OptFlowFlags_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){
free(flags -> imuflags);
free(flags -> optflowflags);
free(flags -> lidarflags);
- free(flags -> errorFlags);
+ free(flags -> errorflags);
free(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)
{
- char * payload = malloc(sizeof(char) * size);
- int currentBytePosition = 0;
+ char * payload = malloc(sizeof(u32) * size);
+ int currentPosition = 0;
/*
Since it's calloced (all zeros), a simple & operation should set this properly
*/
if(flags -> lidarflags -> quadHeight){
- (*payload) &= ((data -> lidar -> distance_m) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> lidar.distance_m) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> lidar.distance_m) / 8;
+ payload[currentPosition] = (u32)(data -> lidar.distance_m);
+ currentPosition++;
}
if(flags -> optflowflags -> x_flow)
{
- (*payload) &= ((data -> optical_flow -> flow_x_rad) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> optflowflags -> y_flow)
{
- (*payload) &= ((data -> optical_flow -> flow_y_rad) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> optflowflags -> x_filter)
{
- (*payload) &= ((data -> optical_flow -> xVelFilt) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> optflowflags -> y_filter)
{
- (*payload) &= ((data -> optical_flow -> yVelFilt) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> optflowflags -> x_velocity)
{
- (*payload) &= ((data -> optical_flow -> xVel) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> optflowflags -> y_velocity)
{
- (*payload) &= ((data -> optical_flow -> yVel) << (currentBytePosition * 8));
- currentBytePosition += 2;
+ //(*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++;
}
if(flags -> imuflags -> gyro_x)
{
- (*payload) &= ((data -> gam -> gyro_xVel_p) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*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++;
}
if(flags -> imuflags -> gyro_y)
{
- (*payload) &= ((data -> gam -> gyro_yVel_q) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*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++;
}
if(flags -> imuflags -> gyro_z)
{
- (*payload) &= ((data -> gam -> gyro_zVel_r) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*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++;
}
if(flags -> imuflags -> acc_x)
{
- (*payload) &= ((data -> gam -> accel_x) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.accel_x) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data->gam.accel_x) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.accel_x);
+ currentPosition++;
}
if(flags -> imuflags -> acc_y)
{
- (*payload) &= ((data -> gam -> accel_y) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.accel_y) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> gam.accel_y) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.accel_y);
+ currentPosition++;
}
if(flags -> imuflags -> acc_z)
{
- (*payload) &= ((data -> gam -> accel_z) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.accel_z) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> gam.accel_z) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.accel_z);
+ currentPosition++;
}
if(flags -> imuflags -> mag_x)
{
- (*payload) &= ((data -> gam -> mag_x) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.mag_x) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> gam.mag_x) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.mag_x);
+ currentPosition++;
}
if(flags -> imuflags -> mag_y)
{
- (*payload) &= ((data -> gam -> mag_y) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.mag_y) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data->gam.mag_y) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.mag_y);
+ currentPosition++;
}
if(flags -> imuflags -> mag_z)
{
- (*payload) &= ((data -> gam -> mag_z) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.mag_z) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> gam.mag_z) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.mag_z);
+ currentPosition++;
}
if(flags -> errorflags -> lidar)
{
- (*payload) &= ((data -> lidar -> error -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> lidar.error.errorCount) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> lidar.error.errorCount) / 8;
+
+ payload[currentPosition] = (u32)(data -> lidar.error.errorCount);
+ currentPosition++;
}
if(flags -> errorflags -> consec_lidar)
{
- (*payload) &= ((data -> lidar -> consErrorCount -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> lidar.error.consErrorCount) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> lidar.error.consErrorCount) / 8;
+
+ payload[currentPosition] = (u32)(data -> lidar.error.consErrorCount);
+ currentPosition++;
}
if(flags -> errorflags -> optFlow)
{
- (*payload) &= ((data -> optical_flow -> error -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*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++;
}
if(flags -> errorflags -> consec_optFlow)
{
- (*payload) &= ((data -> optical_flow -> consErrorCount -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*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++;
}
if(flags -> errorflags -> imu)
{
- (*payload) &= ((data -> gam -> error -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.error.errorCount) << (currentBytePosition * 8));
+ //currentBytePosition += sizeof(data -> gam.error.errorCount) / 8;
+
+ payload[currentPosition] = (u32)(data -> gam.error.errorCount);
+ currentPosition++;
}
if(flags -> errorflags -> consec_imu)
{
- (*payload) &= ((data -> gam -> consErrorCount -> errorCount) << (currentBytePosition * 8));
- currentBytePosition++;
+ //(*payload) |= ((u32)(data -> gam.error.consErrorCount) << (currentBytePosition * 8));
+ //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));
diff --git a/quad/src/quad_app/debug_rt.h b/quad/src/quad_app/debug_rt.h
index 4552e6eda4782013b946f92e44cd62fb627ea491..516f9d0ab084acf25f9fe90dde082a1d4c6defa4 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/quad/src/quad_app/debug_rt.h
@@ -57,7 +57,7 @@ typedef struct SensorRTFlags
}SensorRTFlags_t;
-SensorRTFlags_t * initializeFlags(void);
+void initializeFlags( SensorRTFlags_t * flag_pointer);
//int setDebugLevel(SensorRTFlags_t * flags, float flags);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index c520904cad57e5046365fc597459bb7b476aec25..4fee4d2266fade4c7b71ebc0f46af533a8cd2c48 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -250,8 +250,7 @@ typedef struct raw_sensor {
gam_t gam;
lidar_t lidar;
- px4flow_t optical_flow;
-
+ px4flow_t optical_flow;
// Structures to hold the current quad position & orientation
// This is mostly unused?
quadPosition_t currentQuadPosition;