Adding quad/callbacks.c and related code for decoding groundstation flag_struct update messages

quad/src/commands/commands.c

@@ -76,6 +76,8 @@ command_cb cb_addnode __attribute__((weak, alias("cb_default")));
 command_cb cb_respaddnode __attribute__((weak, alias("cb_default")));
 command_cb cb_overrideoutput __attribute__((weak, alias("cb_default")));
 
+command_cb cb_sendrtdata __attribute__((weak, alias("cb_default")));
+
 
 /*
  * Command structure.
@@ -269,6 +271,15 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		stringType,
 		// Function pointer
 		&cb_overrideoutput
+	},
+	// SENDRTDATA
+	{
+		// Command text
+		"sendrtdata",
+		// Type of the command data
+		stringType,
+		// Function pointer
+		&cb_sendrtdata
 	}
 
 

quad/src/quad_app/callbacks.c

@@ -411,6 +411,36 @@ int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned
 }
 
 int cb_sendrtdata(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
-	
+	//Check to ensure sufficient length for rt_data config packet
+	if (length < 4)
+		return -1;
+	int size = 0;
+    structs->flags.lidarflags.quadHeight = read_bit(configData, 31); size += read_bit(configData, 31);
+	/*BEGIN DOUBLE-TYPE FLAGS. If these are enabled, actual size of data sent is 2*read_bit due to
+	* length of data. This needs to be compensated for in methods where total length of payload packet
+	* needs to be known ahead of time. */
+    structs->flags.optflowflags.x_flow = read_bit(configData, 30); size += read_bit(configData, 30); 
+    structs->flags.optflowflags.y_flow = read_bit(configData, 29); size += read_bit(configData, 29); 
+    structs->flags.optflowflags.x_filter = read_bit(configData, 28); size += read_bit(configData, 28); 
+    structs->flags.optflowflags.y_filter = read_bit(configData, 27); size += read_bit(configData, 27); 
+    structs->flags.optflowflags.x_velocity = read_bit(configData, 26); size += read_bit(configData, 26); 
+    structs->flags.optflowflags.y_velocity = read_bit(configData, 25); size += read_bit(configData, 25);
+	//END DOUBLE TYPE FLAGS. 
+    structs->flags.imuflags.gyro_x = read_bit(configData, 24); size += read_bit(configData, 24); 
+    structs->flags.imuflags.gyro_y = read_bit(configData, 23); size += read_bit(configData, 23); 
+    structs->flags.imuflags.gyro_z = read_bit(configData, 22); size += read_bit(configData, 22); 
+    structs->flags.imuflags.acc_x = read_bit(configData, 21); size += read_bit(configData, 21); 
+    structs->flags.imuflags.acc_y = read_bit(configData, 20); size += read_bit(configData, 20); 
+    structs->flags.imuflags.acc_z = read_bit(configData, 19); size += read_bit(configData, 19); 
+    structs->flags.imuflags.mag_x = read_bit(configData, 18); size += read_bit(configData, 18); 
+    structs->flags.imuflags.mag_y = read_bit(configData, 17); size += read_bit(configData, 17); 
+    structs->flags.imuflags.mag_z = read_bit(configData, 16); size += read_bit(configData, 16); 
+    structs->flags.errorflags.lidar = read_bit(configData, 15); size += read_bit(configData, 15); 
+    structs->flags.errorflags.consec_lidar = read_bit(configData, 14); size += read_bit(configData, 14); 
+    structs->flags.errorflags.optFlow = read_bit(configData, 13); size += read_bit(configData, 13); 
+    structs->flags.errorflags.consec_optFlow = read_bit(configData, 12); size += read_bit(configData, 12); 
+    structs->flags.errorflags.imu = read_bit(configData, 11); size += read_bit(configData, 11); 
+    structs->flags.errorflags.consec_imu = read_bit(configData, 10); size += read_bit(configData, 10);
+    structs->flags.flag_count = size; 
 	return 0;
 }

quad/src/quad_app/debug_rt.c

@@ -5,27 +5,7 @@
 
 #include "debug_rt.h"
 
-/*
-    Initially sets it so that all debug values are zero
-*/
-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 -> flag_count = 0;
-}
-
-void freeFlags(SensorRTFlags_t * flags){
-    free(flags -> imuflags);
-    free(flags -> optflowflags); 
-    free(flags -> lidarflags);
-    free(flags -> errorflags);
-    free(flags -> flag_count);
-    free(flags);
-}
 
 /*
     Helper to return single bit of u32 data. This returns the "position"'th bit of the given u32,
@@ -45,7 +25,7 @@ u32 read_bit(u32 data, int position){
 					 will be sent back during each control loop. Due to the limited bandwidth capacity of the UART, it is undesirable to have a large amount of these flags enabled 
 					 when sending data.
 	Returns:
-		int size:		 Total size of payload assuming all data enabled by flags are transmitted.
+		int size:		 Total number of data types desired to be displayed via flight data tool.
 */
 int process_configuration_packet(u32 configData, SensorRTFlags_t * flags)
 {

quad/src/quad_app/initialize_components.c

@@ -105,6 +105,7 @@ int init_structs(modular_structs_t *structs) {
   }
 
   sensor_processing_init(&structs->sensor_struct);
+  initializeFlags(&structs->flags);
 
   return 0;
 }

quad/src/quad_app/log_data.h

@@ -45,9 +45,19 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
  void updateLog(log_t log_struct);
 
 /**
- * Prints the latest log entry
+ * Prints the latest log entry. Used for Real-Time Data Transfer
  */
 void RTprintLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags);
+/**
+ *  Prints the beginning header for a flight data file. Used for Real-Time Data Transfer
+ * */
+void RTprintheader(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors, SensorRTFlags_t * flags)
+
+/**
+ * Sends the required ending message to close a log file following the end of a flight. Used for 
+ * Real-Time Data Transfer
+ * */
+void RTprintend()
 
  /**
   * Prints all the log information.

quad/src/quad_app/quad_app.c

@@ -24,12 +24,6 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 	// Structures to be used throughout
 	modular_structs_t structs = { };
     
-    //pointer to flags struct that indicate which rt data to send
-    SensorRTFlags_t * flags;
-    
-    // initialize internal flag data for RT data transfer
-	initializeFlags(&flags);
-    
 	// Wire up hardware
 	setup_hardware(&structs.hardware_struct);
 
@@ -105,11 +99,11 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 			if (init_cond == 0x0)
 			{
 				init_cond = 0x1;
-				initialize_logging(&(structs.log_struct), &(structs.parameter_struct), &flags);
-				RTprintheader(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct, &flags);
+				initialize_logging(&(structs.log_struct), &(structs.parameter_struct), &(structs.flags_struct));
+				RTprintheader(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &(structs.raw_sensor_struct), &(structs.flags_struct));
 			}
-			log_data(&(structs.log_struct), &(structs.parameter_struct));
-			RTprintLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct, &flags);
+			log_data(&(structs.log_struct), &(structs.parameter_struct), &(structs.flags_struct));
+			RTprintLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct, &(structs.flags_struct));
 			if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
 			{
 				static int loop_counter = 0;
@@ -148,7 +142,10 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 			kill_motors(&(structs.hardware_struct.motors));
 			char err_msg[] = "More than 10 IMU errors";
 			send_data(structs.hardware_struct.comm.uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
-			printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+			//Send end of log message. Uncomment printLogging below for post-flight log.
+			RTprintend();
+		    resetLogging();
+			//printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 			break;
 		}
 	}

quad/src/quad_app/type_def.h

@@ -492,6 +492,7 @@ typedef struct modular_structs {
 	raw_actuator_t raw_actuator_struct;
 	actuator_command_t actuator_command_struct;
 	hardware_t hardware_struct;
+	SensorRTFlags_t flags_struct;
 } modular_structs_t;
 
 /* BEGIN FLAG STRUCTS CONTAINING ENABLED/DISABLED RT DATA TRANSFERS */

quad/src/quad_app/util.h

@@ -24,5 +24,6 @@ int16_t build_short(u8* buff);
 
 void pack_short(int16_t val, u8* buff);
 void pack_float(float val, u8* buff);
+u32 read_bit(u32 data, int position);
 
 #endif //_UTIL_H