diff --git a/MicroCART b/MicroCART
deleted file mode 160000
index d01645c0571a335594396942e6ffa1b53bc710f0..0000000000000000000000000000000000000000
--- a/MicroCART
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit d01645c0571a335594396942e6ffa1b53bc710f0
diff --git a/quad/src/quad_app/debug_rt.c b/quad/src/quad_app/debug_rt.c
index 8571562272fc81d1e150b17aaf35e95daba0c002..732c428313b38ecbe72fccc1426d5af8d84ced30 100644
--- a/quad/src/quad_app/debug_rt.c
+++ b/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;
diff --git a/quad/src/quad_app/debug_rt.h b/quad/src/quad_app/debug_rt.h
index 8c685ac24cbb651200d231267f3cd67062b8098c..4552e6eda4782013b946f92e44cd62fb627ea491 100644
--- a/quad/src/quad_app/debug_rt.h
+++ b/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);
+
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index d4374ddfa7e9b7634bcc99e78171a41a73d341aa..fee17bff79e88df26cf1def41cc701468f82a2bf 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/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));