Added define for USE_LIDAR. Lidar is still polled and the node is updated, but...

Added define for USE_LIDAR. Lidar is still polled and the node is updated, but it is not used for altitude control by default.

quad/src/gen_diagram/network.dot

@@ -31,7 +31,7 @@ label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
 "Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
 "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]"]
+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"]
 "X Setpoint" -> "X pos PID":f2 [label="Constant"]
 "Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
@@ -41,7 +41,7 @@ label="<f0>Y pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
 "Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
 "Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
 "Altitude PID"[shape=record
-label="<f0>Altitude PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.000]"]
+label="<f0>Altitude PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.880]"]
 "VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
 "Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
 "Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
@@ -65,6 +65,8 @@ label="<f0>Pitch  |<f1> [Constant=0.000]"]
 label="<f0>Roll  |<f1> [Constant=0.000]"]
 "Yaw"[shape=record
 label="<f0>Yaw  |<f1> [Constant=0.000]"]
+"Lidar"[shape=record
+label="<f0>Lidar  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -105,22 +107,22 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 "RC Throttle"[shape=record
 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.880]"]
+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.880]"]
+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.000]"]
+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"]
 "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.000]"]
+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"]

quad/src/quad_app/control_algorithm.c

@@ -13,6 +13,8 @@
 #include "util.h"
 #include "timer.h"
 
+//#define USE_LIDAR
+
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
 #define VRPN_REFRESH_TIME 0.01f // 10ms
@@ -163,8 +165,13 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
 
     // Alt autonomous
+#ifdef USE_LIDAR
     graph_set_source(graph, ps->alt_pid, PID_DT, ps->angle_time, CONST_VAL);
     graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
+#else
+    graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
+#endif
     graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
     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);