Skip to content
Snippets Groups Projects
Commit 7715194d authored by bertucci's avatar bertucci
Browse files

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

parent d1300181
No related branches found
No related tags found
1 merge request!43Resolve "Transition current hardware platform to Vivado"
Hide whitespace changes
Inline Side-by-side
MicroCART @ d01645c0
Subproject commit d01645c0571a335594396942e6ffa1b53bc710f0
quad/src/quad_app/debug_rt.c
+ 53
− 82
View file @ 7715194d
......@@ -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
+ 2
− 0
View file @ 7715194d
......@@ -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
+ 3
− 4
View file @ 7715194d
......@@ -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));
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment