diff --git a/quad/src/commands/commands.c b/quad/src/commands/commands.c
index 9942b62c9a1ac5dc783f5731bde8ec670546f733..6f99dd9f6a229dc42dbcbd058209017f2caec5ed 100644
--- a/quad/src/commands/commands.c
+++ b/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
}
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 13d3a72e10f2310943cd86ec222378f5feb7205c..005485bdb300fc47e43440d2b96fd18cdc4b32fc 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/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;
}
diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index 4ccee0f6de67e1ccf794fe404963cf467a4d4f28..147a65e9999c4a26e01a927f32ead6701cba7c6c 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/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)
{
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index edccbbbf88d06f4d48bc7bd24053eeb8d8dd43dc..232125ac34aaaac5497059ee9b5004f755254dd6 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/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;
}
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index e7f29061cf97cf8fc5f2138dbed3ff542194f364..c4cfd903ec9a87571d2bde6483deafb6c17c2ded 100644
--- a/quad/src/quad_app/log_data.h
+++ b/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.
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 636c683a4a4a2f68f4a7b6c03cc86e7f66696b4b..d9174003ae74df44b06525062481ae997ffad406 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/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;
}
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index d8ad249f27145c710e25398f4f428e751fdca753..8eb2565d3cba3410b7e2eebadd41f0be89d59765 100644
--- a/quad/src/quad_app/type_def.h
+++ b/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 */
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index d675428293bc3108673bf953f78e089f8877caea..f3e87401478dac45cef99f6373e24a00dddbc544 100644
--- a/quad/src/quad_app/util.h
+++ b/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