diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 22b822f80919ab18c96297fe83b71ec137eefe48..87c1b3106e8708b4b929c87d4bb07d44805e3912 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -67,8 +67,9 @@ int control_algorithm_init(parameter_t * ps)
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_vel_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y");
ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
+ ps->flow_distance = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Distance");
// 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");
@@ -372,6 +373,7 @@ int control_algorithm_init(parameter_t * ps)
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);
}
+ graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);
// RC values
graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -380,9 +382,8 @@ 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[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);
+ int outputs[1] = {ps->mixer};
+ graph_compute_nodes(graph, outputs, 1);
// 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 f6b09b5b13ce93ebce27cca67e9300c593c4ae4b..dffe2f2313b956a413eb86da40e6839fe5c30eb6 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -316,6 +316,7 @@ int iic0_px4flow_update(px4flow_t *of) {
of->xVel = i2cFrame.flowCompX / 1000.;
of->yVel = i2cFrame.flowCompY / 1000.;
of->quality = i2cFrame.qual;
+ of->distance = i2cFrame.groundDistance / 1000.;
}
return status;
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 92019f3efd502857fea0c20080000ed620743766..338e63bc6a6884a727ac7376d72ef279d41df15a 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -106,32 +106,34 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->pitch_trim_add, CONST_VAL, rad);
- addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
- addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
- addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
- addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
- addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
- 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);
addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR0, pwm_val);
addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR1, pwm_val);
addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR2, pwm_val);
addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR3, pwm_val);
- addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
- addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
- addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);
+ addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
+ addParamToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
+ addParamToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
+ addParamToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
+ addParamToLog(log_struct, ps->x_set, CONST_VAL, m);
+ addParamToLog(log_struct, ps->y_set, CONST_VAL, m);
+ addParamToLog(log_struct, ps->alt_set, CONST_VAL, m);
+ addParamToLog(log_struct, ps->lidar, CONST_VAL, m);
+ addParamToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
+ addParamToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
+ addParamToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
+ addParamToLog(log_struct, ps->flow_vel_x, CONST_VAL, m_s);
+ addParamToLog(log_struct, ps->flow_vel_y, CONST_VAL, m_s);
+ addParamToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
+ addParamToLog(log_struct, ps->flow_distance, CONST_VAL, m);
+ addParamToLog(log_struct, ps->rc_throttle, CONST_VAL, pwm_val);
+ addParamToLog(log_struct, ps->rc_pitch, CONST_VAL, pwm_val);
+ addParamToLog(log_struct, ps->rc_roll, CONST_VAL, pwm_val);
+
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
row_size = n_outputs + n_params + 6 + 1;
size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index df47c6d94b22932950876a28305be692f2ca0d8e..02dde9e0b0314d582366a1abf984d9ea2652ac16 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -117,6 +117,11 @@ typedef struct {
typedef struct {
double xVel, yVel;
+ double distance;
+
+ double flowX, flowY;
+ double gyroX, gyroY, gyroZ;
+
int16_t quality;
} px4flow_t;
@@ -339,6 +344,7 @@ typedef struct parameter_t {
int flow_vel_x; // optical flow
int flow_vel_y;
int flow_quality; // Quality value returned by optical flow sensor
+ int flow_distance;
// VRPN blocks
int vrpn_x;
int vrpn_y;