The work continues. Fixing issues with RT data on quad side

MicroCART @ d01645c0

-Subproject commit d01645c0571a335594396942e6ffa1b53bc710f0

quad/src/quad_app/debug_rt.c

@@ -9,37 +9,32 @@
 #include "computation_graph.h"
 #include "timer.h"
 
-
-
-
-#define ReadBit(data, position) ( (data>>y) & 1)
-#define SetBit(data,position) data |= (1 << y)
-#define ClearBit(data,y) data &= ~(1 << y)
-
-
-
 /*
     Initially sets it so that all debug values are zero
 */
-SensorRTFlags_t * initializeFlags(void){
-
-    SensorRTFlags_t * flags = malloc(sizeof(SensorRTFlags_t));
-#if 0
-    flags -> imuflags = calloc(sizeof(IMUFlags_t));
-    flags -> optflowflags = calloc(sizeof(OptFlowFlags_t));
-    flags -> lidarflags = calloc(sizeof(lidarFlags_t));
-    flags -> errorFlags = calloc(sizeof(SensorErrorFlags_t));
-#endif
+SensorRTFlags_t * initializeFlags(sensorRTF_flags_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));
 }
 
 void freeFlags(SensorRTFlags_t * flags){
-#if 0
     free(flags -> imuflags);
     free(flags -> optflowflags); 
     free(flags -> lidarflags);
     free(flags -> errorFlags);
     free(flags);
-#endif
+}
+
+/*
+    Helper to return single bit of u32 data. This returns the "position"'th bit of the given u32,
+    assuming it is Zero indexed.
+*/
+u32 read_bit(u32 data, int position){
+    return (data >> position) & 1;
 }
 
 /*
@@ -51,60 +46,38 @@ See RTSensorPacketDocumentation.md for which flags each bit of the config data c
 
 It returns the size of the packet that will be sent, so the payload is correctly determined
 */
-
-int process_configuration_packet(float configData, SensorRTFlags_t * flags)
+int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
 {
-
     int size = 0;
-#if 0
-    flags -> lidarflags -> quadHeight = ReadBit(31); size += ReadBit(31); 
-    flags -> optflowflags ->  x_flow = ReadBit(30); size += 2 * ReadBit(30); 
-    flags -> optflowflags ->  y_flow = ReadBit(29); size += 2 * ReadBit(29); 
-    flags -> optflowflags ->  x_filter = ReadBit(28); size += 2 * ReadBit(28); 
-    flags -> optflowflags ->  y_filter = ReadBit(27); size += 2 * ReadBit(27); 
-    flags -> optflowflags ->  x_velocity = ReadBit(26); size += 2 * ReadBit(26); 
-    flags -> optflowflags ->  y_velocity = ReadBit(25); size += 2 * ReadBit(25); 
-    flags -> imuflags -> gyro_x = ReadBit(24); size += ReadBit(24); 
-    flags -> imuflags -> gyro_y = ReadBit(23); size += ReadBit(23); 
-    flags -> imuflags -> gyro_z = ReadBit(22); size += ReadBit(22); 
-    flags -> imuflags -> acc_x = ReadBit(21); size += ReadBit(21); 
-    flags -> imuflags -> acc_y = ReadBit(20); size += ReadBit(20); 
-    flags -> imuflags -> acc_z = ReadBit(19); size += ReadBit(19); 
-    flags -> imuflags -> mag_x = ReadBit(18); size += ReadBit(18); 
-    flags -> imuflags -> mag_y = ReadBit(17); size += ReadBit(17); 
-    flags -> imuflags -> mag_z = ReadBit(16); size += ReadBit(16); 
-    flags -> errorflags -> lidar = ReadBit(15); size += ReadBit(15); 
-    flags -> errorflags -> consec_lidar = ReadBit(14); size += ReadBit(14); 
-    flags -> errorflags -> optFlow = ReadBit(13); size += ReadBit(13); 
-    flags -> errorflags -> consec_optFlow = ReadBit(12); size += ReadBit(12); 
-    flags -> errorflags -> imu = ReadBit(11); size += ReadBit(11); 
-    flags -> errorflags -> consec_imu = ReadBit(10); size += ReadBit(10); 
-#endif
+    flags -> lidarflags -> quadHeight = read_bit(configData, 31); size += read_bit(configData, 31); 
+    flags -> optflowflags ->  x_flow = read_bit(configData, 30); size += 2 * read_bit(configData, 30); 
+    flags -> optflowflags ->  y_flow = read_bit(configData, 29); size += 2 * read_bit(configData, 29); 
+    flags -> optflowflags ->  x_filter = read_bit(configData, 28); size += 2 * read_bit(configData, 28); 
+    flags -> optflowflags ->  y_filter = read_bit(configData, 27); size += 2 * read_bit(configData, 27); 
+    flags -> optflowflags ->  x_velocity = read_bit(configData, 26); size += 2 * read_bit(configData, 26); 
+    flags -> optflowflags ->  y_velocity = read_bit(configData, 25); size += 2 * read_bit(configData, 25); 
+    flags -> imuflags -> gyro_x = read_bit(configData, 24); size += read_bit(configData, 24); 
+    flags -> imuflags -> gyro_y = read_bit(configData, 23); size += read_bit(configData, 23); 
+    flags -> imuflags -> gyro_z = read_bit(configData, 22); size += read_bit(configData, 22); 
+    flags -> imuflags -> acc_x = read_bit(configData, 21); size += read_bit(configData, 21); 
+    flags -> imuflags -> acc_y = read_bit(configData, 20); size += read_bit(configData, 20); 
+    flags -> imuflags -> acc_z = read_bit(configData, 19); size += read_bit(configData, 19); 
+    flags -> imuflags -> mag_x = read_bit(configData, 18); size += read_bit(configData, 18); 
+    flags -> imuflags -> mag_y = read_bit(configData, 17); size += read_bit(configData, 17); 
+    flags -> imuflags -> mag_z = read_bit(configData, 16); size += read_bit(configData, 16); 
+    flags -> errorflags -> lidar = read_bit(configData, 15); size += read_bit(configData, 15); 
+    flags -> errorflags -> consec_lidar = read_bit(configData, 14); size += read_bit(configData, 14); 
+    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); 
     return size;
 }
 
-/*
-
-typedef struct raw_sensor {
-
-    gam_t gam;
-    lidar_t lidar;
-    px4flow_t optical_flow;
-
-    // Structures to hold the current quad position & orientation
-    // This is mostly unused?
-    quadPosition_t currentQuadPosition;
-
-} raw_sensor_t;
-
-
-*/
-
 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;
-#if 0
     /*
     Since it's calloced (all zeros), a simple & operation should set this properly
     */
@@ -116,90 +89,90 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
 
     if(flags -> optflowflags ->  x_flow) 
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> flow_x_rad) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
 
     if(flags -> optflowflags ->  y_flow)
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> flow_y_rad) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
 
     if(flags -> optflowflags ->  x_filter)
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> xVelFilt) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
 
     if(flags -> optflowflags ->  y_filter)
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> yVelFilt) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
     if(flags -> optflowflags ->  x_velocity)
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> xVel) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
 
     if(flags -> optflowflags ->  y_velocity)
     {
-        (*payload) &= ((data -> optical_flow -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> optical_flow -> yVel) << (currentBytePosition * 8));
         currentBytePosition += 2;
     }
     
     if(flags -> imuflags -> gyro_x)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> gyro_xVel_p) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> gyro_y)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> gyro_yVel_q) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> gyro_z)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> gyro_zVel_r) << (currentBytePosition * 8));
         currentBytePosition++; 
     } 
 
     if(flags -> imuflags -> acc_x)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> accel_x) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> acc_y)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> accel_y) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> acc_z)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> accel_z) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> mag_x)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> mag_x) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> mag_y)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> mag_y) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
 
     if(flags -> imuflags -> mag_z)
     {
-        (*payload) &= ((data -> gam -> ) << (currentBytePosition * 8));
+        (*payload) &= ((data -> gam -> mag_z) << (currentBytePosition * 8));
         currentBytePosition++; 
     }
     
@@ -239,8 +212,6 @@ int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t *
         currentBytePosition++; 
     }
 
-
-#endif
     int bytes_sent = send_data(comm->uart, SEND_RT_ID, 0, (u8*)payload, strlen(payload));
     free(payload);
     return bytes_sent; 

quad/src/quad_app/debug_rt.h

@@ -65,3 +65,5 @@ void freeFlags(SensorRTFlags_t * flags);
 
 int send_RT_data(struct CommDriver *comm, raw_sensor_t * data, SensorRTFlags_t * flags, int size);
 
+u32 shift(u32 data, int position);
+

quad/src/quad_app/quad_app.c

@@ -26,9 +26,8 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
     //flags that indicate which rt data to send
     SensorRTFlags_t flags;
     
-    // TEST : only send accelerometer data for now
-    memset(&flags, 0, sizeof(flags));
-    flags.imuflags->acc_x = 1;
+    // initialize internal flag data;
+	initializeFlags(&flags);
     
 	// Wire up hardware
 	setup_hardware(&structs.hardware_struct);
@@ -67,7 +66,7 @@ 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));
 
 		// 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));