Skip to content
Snippets Groups Projects
Commit 0f309787 authored by dawehr's avatar dawehr
Browse files

Added #defines for using optical flow vs. VRPN and using integrated gyro for... 

Added #defines for using optical flow vs. VRPN and using integrated gyro for yaw. Updated Gyro Z bias. Changed some sensor processing regarding optical flow.
parent 7b9a0ff4
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/gen_diagram/network.dot
+ 23
− 32
View file @ 0f309787
......@@ -3,18 +3,18 @@ rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
"RC Roll" -> "Roll PID":f2 [label="Constant"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"Yaw" -> "Yaw PID":f1 [label="Constant"]
"Integrated gyro z" -> "Yaw PID":f1 [label="Integrated"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
"Ts_VRPN" -> "Yaw PID":f3 [label="Constant"]
"Ts_IMU" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record
label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.030] |<f5> [Ki=0.000] |<f6> [Kd=0.005] |<f7> [alpha=0.880]"]
"Gyro X" -> "Roll Rate PID":f1 [label="Constant"]
......@@ -28,23 +28,23 @@ label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
"Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN X" -> "X pos PID":f1 [label="Constant"]
"OF X Trim Add" -> "X pos PID":f1 [label="Sum"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
"Ts_IMU" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
"OF Y Trim Add" -> "Y pos PID":f1 [label="Sum"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
"Ts_IMU" -> "Y pos PID":f3 [label="Constant"]
"Altitude PID"[shape=record
label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.098] |<f5> [Ki=-0.008] |<f6> [Kd=-0.074] |<f7> [alpha=0.880]"]
"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Lidar" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
"Ts_IMU" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
label="<f0>X Setpoint |<f1> [Constant=0.000]"]
"Y Setpoint"[shape=record
......@@ -116,35 +116,23 @@ label="<f0>RC Yaw |<f1> [Constant=0.000]"]
label="<f0>RC Throttle |<f1> [Constant=0.000]"]
"X Vel PID"[shape=record
label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.000]"]
"X Vel" -> "X Vel PID":f1 [label="Correction"]
"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
"Y Vel PID"[shape=record
label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.000]"]
"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
"X Vel"[shape=record
label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
"VRPN X" -> "X Vel":f1 [label="Constant"]
"zero" -> "X Vel":f2 [label="Constant"]
"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.880]"]
"Y Vel"[shape=record
label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
"VRPN Y" -> "Y Vel":f1 [label="Constant"]
"zero" -> "Y Vel":f2 [label="Constant"]
"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.880]"]
"X Vel Clamp"[shape=record
label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
"X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
"Y vel Clamp"[shape=record
label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
"Yaw Correction"[shape=record
label="<f0>Yaw Correction |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
"Yaw" -> "Yaw Correction":f1 [label="Constant"]
"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
"Integrated gyro z" -> "Yaw Correction":f1 [label="Integrated"]
"X Vel Clamp" -> "Yaw Correction":f2 [label="Bounded"]
"Y vel Clamp" -> "Yaw Correction":f3 [label="Bounded"]
"OF Offset Angle"[shape=record
label="<f0>OF Offset Angle |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
"OF Offset Add" -> "OF Offset Angle":f1 [label="Sum"]
......@@ -155,7 +143,7 @@ label="<f0>OF Offset Rot |<f1> [Constant=0.622]"]
"OF Offset Add"[shape=record
label="<f0>OF Offset Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"OF Offset Rot" -> "OF Offset Add":f1 [label="Constant"]
"Yaw" -> "OF Offset Add":f2 [label="Constant"]
"Integrated gyro z" -> "OF Offset Add":f2 [label="Integrated"]
"OF Integrate X"[shape=record
label="<f0>OF Integrate X |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
"OF Offset Angle" -> "OF Integrate X":f1 [label="Rotated X"]
......@@ -176,9 +164,12 @@ label="<f0>OF X Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
label="<f0>OF Y Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"OF Integrate Y" -> "OF Y Trim Add":f1 [label="Integrated"]
"OF Trim Y" -> "OF Y Trim Add":f2 [label="Constant"]
"Integrated gyro z"[shape=record
label="<f0>Integrated gyro z |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
"Gyro Z" -> "Integrated gyro z":f1 [label="Constant"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
"T trim add" -> "Signal Mixer":f1 [label="Sum"]
"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
......
quad/src/gen_diagram/network.png
+ 0
− 0
View replaced file @ 7b9a0ff4 View file @ 0f309787
quad/src/gen_diagram/network.png

607 KiB | W: | H:

quad/src/gen_diagram/network.png

582 KiB | W: | H:

quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
  • 2-up
  • Swipe
  • Onion skin
quad/src/quad_app/control_algorithm.c
+ 78
− 5
View file @ 0f309787
......@@ -13,9 +13,14 @@
#include "util.h"
#include "timer.h"
//#define USE_LIDAR
#define USE_LIDAR
#define USE_OF
#define USE_FAKE_YAW
#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
//10 degrees
#define ANGLE_CLAMP (0.1745)
#define PX4FLOW_QUAL_MIN (100)
#define OF_OFFSET_ANGLE (0.62204) // 35.64 degrees
......@@ -29,8 +34,13 @@ void connect_autonomous(parameter_t* ps) {
//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
#ifdef USE_OF
//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, ADD_SUM);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
#else
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
#endif
}
void connect_manual(parameter_t* ps) {
......@@ -118,6 +128,9 @@ int control_algorithm_init(parameter_t * ps)
ps->of_trimmed_x = graph_add_defined_block(graph, BLOCK_ADD, "OF X Trim Add");
ps->of_trimmed_y = graph_add_defined_block(graph, BLOCK_ADD, "OF Y Trim Add");
// gyroscope z integrator
ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
......@@ -152,6 +165,9 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->gyro_z, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->gyro_yaw, INTEGRATE_IN, ps->gyro_z, CONST_VAL);
#ifndef USE_OF
// X velocity PID
// Use a PID block to compute the derivative
graph_set_param_val(graph, ps->x_vel, PID_KD, -1);
......@@ -166,7 +182,11 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
// X/Y global to local conversion
#ifdef USE_FAKE_YAW
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
......@@ -194,6 +214,33 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
//graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
#endif
#ifdef USE_OF
// X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, ADD_SUM);
//graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
// Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, ADD_SUM);
//graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
// X/Y global to local conversion
#ifdef USE_FAKE_YAW
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_clamp, PID_CORRECTION);
graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_clamp, PID_CORRECTION);
#endif
// Alt autonomous
#ifdef USE_LIDAR
......@@ -207,9 +254,14 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
// Yaw angle
#ifdef USE_FAKE_YAW
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->gyro_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
#endif
graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
// Connect PWM clamping blocks
graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
......@@ -224,7 +276,11 @@ int control_algorithm_init(parameter_t * ps)
// Connect optical flow
graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND1, ps->of_angle_offset, CONST_VAL);
#ifdef USE_FAKE_YAW
graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->gyro_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->cur_yaw, CONST_VAL);
#endif
graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->of_angle_add, ADD_SUM);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_X, ps->flow_vel_x, CONST_VAL);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_Y, ps->flow_vel_y, CONST_VAL);
......@@ -299,11 +355,19 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
#ifdef USE_OF
//Set angle clamping limits
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
#else
// Set velocity clamping limits
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
#endif
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
......@@ -354,10 +418,17 @@ int control_algorithm_init(parameter_t * ps)
// also reset the previous error and accumulated error from the position PIDs
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
{
#ifdef USE_OF
graph_set_param_val(graph, ps->x_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_x, ADD_SUM));
graph_set_param_val(graph, ps->y_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_y, ADD_SUM));
//graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
#else
graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
#endif
// reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0;
......@@ -407,10 +478,12 @@ int control_algorithm_init(parameter_t * ps)
//}
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) {
//NOTE: As per the Wehrneyton(R) Method(TM), we will be exponentially decaying the
//optical flow velocities when the quality is below the threshold
//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);
}
//}
graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);
// RC values
......
quad/src/quad_app/log_data.c
+ 1
− 6
View file @ 0f309787
......@@ -118,11 +118,10 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->of_angle_corr, ROT_OUT_Y, m_s);
addOutputToLog(log_struct, ps->of_integ_x, INTEGRATED, m);
addOutputToLog(log_struct, ps->of_integ_y, INTEGRATED, m);
addOutputToLog(log_struct, ps->gyro_yaw, INTEGRATED, rad);
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);
......@@ -134,10 +133,6 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
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;
......
quad/src/quad_app/sensor_processing.c
+ 13
− 4
View file @ 0f309787
......@@ -14,6 +14,8 @@
#include <math.h>
#define ALPHA 0.99
#define PX4FLOW_QUAL_MIN (100)
#define PX4FLOW_VEL_DECAY (0.99)
int sensor_processing_init(sensor_t* sensor_struct) {
float a0 = 0.0200833310260;
......@@ -43,8 +45,14 @@ static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal
void flow_to_vel(px4flow_t* flow_data, double distance) {
double loop_time = get_last_loop_time();
if (loop_time != 0) {
flow_data->xVel = distance * flow_data->flowX / focal_length_px / get_last_loop_time();
flow_data->yVel = distance * flow_data->flowY / focal_length_px / get_last_loop_time();
if(flow_data->quality > PX4FLOW_QUAL_MIN) {
flow_data->xVel = distance * flow_data->flowX / focal_length_px / loop_time;
flow_data->yVel = distance * flow_data->flowY / focal_length_px / loop_time;
}
else {
flow_data->xVel *= PX4FLOW_VEL_DECAY;
flow_data->yVel *= PX4FLOW_VEL_DECAY;
}
}
}
......@@ -124,8 +132,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
// sensor_struct->optical_flow.xVel = biquad_execute(&sensor_struct->flow_x_filt, -sensor_struct->optical_flow.xVel);
// sensor_struct->optical_flow.yVel = biquad_execute(&sensor_struct->flow_y_filt, -sensor_struct->optical_flow.yVel);
sensor_struct->optical_flow.xVel *= -1;
sensor_struct->optical_flow.yVel *= -1;
//Note: This was wrong and dumb
//sensor_struct->optical_flow.xVel *= -1;
//sensor_struct->optical_flow.yVel *= -1;
/*
* Altitude double complementary filter
......
quad/src/quad_app/type_def.h
+ 1
− 0
View file @ 0f309787
......@@ -412,6 +412,7 @@ typedef struct parameter_t {
int of_trim_y;
int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
int of_trimmed_y;
int gyro_yaw; // Integrates the gyro z value
} parameter_t;
/**
......
quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+ 1
− 1
View file @ 0f309787
......@@ -44,7 +44,7 @@
#define GYRO_X_BIAS 0.005f
#define GYRO_Y_BIAS -0.014f
#define GYRO_Z_BIAS 0.045f
#define GYRO_Z_BIAS 0.0541f
#define ACCEL_X_BIAS 0.023f
#define ACCEL_Y_BIAS 0.009f
......
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