From c4ccd4ccd76c0a8c5a0f6b228afd3cdb005cf240 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Wed, 12 Apr 2017 19:00:24 -0500
Subject: [PATCH] Optical flow data is being polled, part of the graph, and
being logged now. It is not used for flight.
---
quad/src/quad_app/control_algorithm.c | 14 ++++++++++++--
quad/src/quad_app/iic_utils.c | 1 -
quad/src/quad_app/iic_utils.h | 1 -
quad/src/quad_app/log_data.c | 3 +++
4 files changed, 15 insertions(+), 4 deletions(-)
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index dd9425b65..22b822f80 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -15,6 +15,8 @@
//#define USE_LIDAR
+#define PX4FLOW_QUAL_MIN (100)
+
#define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
#define PWM_DIFF_BOUNDS 20000
#define VRPN_REFRESH_TIME 0.01f // 10ms
@@ -352,6 +354,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->pos_time, CONST_SET, VRPN_REFRESH_TIME * (current_vrpn_id - last_vrpn_id));
last_vrpn_id = current_vrpn_id;
}
+
// Loop time
graph_set_param_val(graph, ps->angle_time, CONST_SET, get_last_loop_time());
@@ -363,6 +366,12 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
+ graph_set_param_val(graph, ps->flow_quality, CONST_SET, sensor_struct->optical_flow.quality);
+ //As per documentation, disregard frames with low quality, as they contain unreliable data
+ if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {
+ graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
+ graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
+ }
// RC values
graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -371,8 +380,9 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
// Compute VRPN blocks so they can be logged
- int outputs[5] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar};
- graph_compute_nodes(graph, outputs, 5);
+ int outputs[8] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar,
+ ps->flow_quality, ps->flow_vel_x, ps->flow_vel_y};
+ graph_compute_nodes(graph, outputs, 8);
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index 02d21cc83..f6b09b5b1 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -313,7 +313,6 @@ int iic0_px4flow_update(px4flow_t *of) {
//Copy into struct
memcpy(&i2cFrame, buf, sizeof(i2cFrame));
- //As per documentation, disregard frames with low quality, as they contain unreliable data
of->xVel = i2cFrame.flowCompX / 1000.;
of->yVel = i2cFrame.flowCompY / 1000.;
of->quality = i2cFrame.qual;
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
index d0af4394d..59ad8ed7c 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -134,7 +134,6 @@ int iic0_lidarlite_sleep();
////////////////////////////////////////////////////////////////////////////////////////////
#define PX4FLOW_DEVICE_ADDR 0x42
-#define PX4FLOW_QUAL_MIN (100)
int iic0_px4flow_write(u8 register_addr, u8 data);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index bdc38f0ee..92019f3ef 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -114,6 +114,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
addOutputToLog(log_struct, ps->lidar, CONST_VAL, m);
+ addOutputToLog(log_struct, ps->flow_vel_x, CONST_VAL, m_s);
+ addOutputToLog(log_struct, ps->flow_vel_y, CONST_VAL, m_s);
+ addOutputToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
--
GitLab