diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 845c584334bdd8b83083ed1215398fb2ca7e0237..dd9425b65eb10112924773042dc7088df79f3369 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -64,6 +64,9 @@ int control_algorithm_init(parameter_t * ps)
ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
ps->lidar = graph_add_defined_block(graph, BLOCK_CONSTANT, "Lidar");
+ ps->flow_vel_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X");
+ ps->flow_vel_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel y");
+ ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
// Sensor trims
ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index 035085dfe81ad94f37648888e26bbe329c97d372..02d21cc83c4421c99da73301500811002c1f7020 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -212,7 +212,10 @@ int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
int status = 0;
status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
- status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+ // Only read if write succeeded
+ if (status == 0) {
+ status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+ }
return status;
}
@@ -263,29 +266,24 @@ int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size) {
int status = 0;
status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
- status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+ if (status == 0) {
+ status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+ }
return status;
}
int iic0_px4flow_init(px4flow_t *of) {
//Clear struct
- of->xPos = 0;
- of->yPos = 0;
of->xVel = 0;
of->yVel = 0;
//Perform initial update
- return iic0_px4flow_update(of, 0.);
+ // TODO: Re-enable this initial read once virtual mock-up is made
+ //return iic0_px4flow_update(of);
+ return 0;
}
-int iic0_px4flow_update(px4flow_t *of, double dt) {
- static double time = 0.;
-
- time += dt;
- if(time >= 10.) {
- time = 0.;
- }
-
+int iic0_px4flow_update(px4flow_t *of) {
int status = 0;
struct {
@@ -316,13 +314,9 @@ int iic0_px4flow_update(px4flow_t *of, double dt) {
memcpy(&i2cFrame, buf, sizeof(i2cFrame));
//As per documentation, disregard frames with low quality, as they contain unreliable data
- if(i2cFrame.qual >= PX4FLOW_QUAL_MIN) {
- of->xVel = i2cFrame.flowCompX / 1000.;
- of->yVel = i2cFrame.flowCompY / 1000.;
-
- of->xPos += of->xVel * dt;
- of->yPos += of->yVel * dt;
- }
+ of->xVel = i2cFrame.flowCompX / 1000.;
+ of->yVel = i2cFrame.flowCompY / 1000.;
+ of->quality = i2cFrame.qual;
}
return status;
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
index 637c9d7ab3b564be33b6ffa2922e3ba1e4ddaadd..d0af4394d11a6c3ed7f211ea773612b8ad95e8fe 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -140,7 +140,7 @@ int iic0_lidarlite_sleep();
int iic0_px4flow_write(u8 register_addr, u8 data);
int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size);
-int iic0_px4flow_update(px4flow_t *of, double dt);
+int iic0_px4flow_update(px4flow_t *of);
int iic0_px4flow_init(px4flow_t *of);
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index b2f7592c88f667ca9ed9f897ea65dd4b22293ecc..c74e4a681d919472a8e72840027c6c9d74935cf7 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -16,7 +16,11 @@ int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
// return -1;
// }
- if(iic0_mpu9150_start() == -1) {
+ if(iic0_mpu9150_start()) {
+ return -1;
+ }
+
+ if (iic0_px4flow_init(&raw_sensor_struct->optical_flow)) {
return -1;
}
@@ -70,14 +74,18 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
int status = 0;
// the the sensor board and fill in the readings into the GAM struct
- iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
-
+ status |= iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
log_struct->gam = raw_sensor_struct->gam;
-// static lidar_t lidar_val;
-// iic0_lidarlite_read_distance(&lidar_val);
-// raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+ static lidar_t lidar_val;
+ int lidar_status = iic0_lidarlite_read_distance(&lidar_val);
+ if (lidar_status == 0) {
+ raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+ }
+ status |= lidar_status;
+
+ status |= iic0_px4flow_update(&raw_sensor_struct->optical_flow);
- return 0;
+ return status;
}
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 5e3ca9f2ff4484625da6df72009e77ce44ade924..668afa23fc6d16d4056d6b118ca5674c8a62f78e 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -98,6 +98,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// Z-axis points upward, so negate distance
sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
+
+ // Simply copy optical flow data
+ sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
return 0;
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index e2e40c1aac046af72fe499ef9b1f7f583bf27be5..df47c6d94b22932950876a28305be692f2ca0d8e 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -117,7 +117,7 @@ typedef struct {
typedef struct {
double xVel, yVel;
- double xPos, yPos;
+ int16_t quality;
} px4flow_t;
typedef struct PID_t {
@@ -248,6 +248,9 @@ typedef struct raw_sensor {
// Ideally equals 0 when landed
float lidar_distance_m;
+ // Information obtained from optical flow sensor
+ px4flow_t optical_flow;
+
} raw_sensor_t;
/**
@@ -294,6 +297,8 @@ typedef struct sensor {
struct biquadState accel_y_filt;
struct biquadState accel_z_filt;
+ // Information obtained from optical flow sensor
+ px4flow_t optical_flow;
} sensor_t;
/**
@@ -331,6 +336,9 @@ typedef struct parameter_t {
int gyro_x;
int gyro_z;
int lidar;
+ int flow_vel_x; // optical flow
+ int flow_vel_y;
+ int flow_quality; // Quality value returned by optical flow sensor
// VRPN blocks
int vrpn_x;
int vrpn_y;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index 5971d023b9f4026040217b8368e6ee5d7967ce4a..cf248f6af93600632918105cf6dc897b2d0362e1 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -99,7 +99,7 @@ int test_zybo_i2c_px4flow() {
sys.sleep(&sys, delay * 1000);
- iic0_px4flow_update(&of, delay / 1000.);
+ iic0_px4flow_update(&of);
}
return 0;
}
@@ -129,7 +129,7 @@ int test_zybo_i2c_all() {
sys.sleep(&sys, delay * 1000);
- if (iic0_px4flow_update(&of, delay / 1000.)) {
+ if (iic0_px4flow_update(&of)) {
of_errors += 1;
}