Using LiDAR for altitude now.

quad/src/gen_diagram/network.dot

@@ -3,12 +3,12 @@ rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
+"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=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
 "Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
+"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]"]
@@ -28,7 +28,7 @@ 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=29700.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
 "dPsi" -> "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.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030] |<f7> [alpha=0.000]"]
@@ -42,7 +42,7 @@ label="<f0>Y pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
 "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]"]
-"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"]
 "X Setpoint"[shape=record
@@ -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.020]"]
 "Pitch trim add"[shape=record
@@ -106,7 +108,7 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 label="<f0>RC Throttle  |<f1> [Constant=0.000]"]
 "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/quad_app/control_algorithm.c

@@ -61,6 +61,7 @@ int control_algorithm_init(parameter_t * ps)
     ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
     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");
     // 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");
@@ -131,9 +132,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
     // Alt autonomous
     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);
+    graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
     graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
-    graph_set_source(graph, ps->alt_set, CONST_VAL, ps->vrpn_alt, 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);
     // Yaw autonomous
@@ -347,6 +347,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->gyr_y);
     graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->gyr_x);
     graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->gyr_z);
+    graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
 
     // RC values
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -355,8 +356,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[4] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll};
-    graph_compute_nodes(graph, outputs, 4);
+    int outputs[5] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar};
+    graph_compute_nodes(graph, outputs, 5);
 
 	 // 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);

quad/src/quad_app/iic_utils.c

@@ -240,7 +240,8 @@ int iic0_lidarlite_read_distance(lidar_t *lidar) {
 
 	// Read the sensor value
 	status = iic0_lidarlite_read(buf, 0x8f, 2);
-	lidar->distance_cm = buf[0] << 8 | buf[1];
+	float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
+	lidar->distance_m = 0.01 * distance_cm;
 
 	return status;
 }

quad/src/quad_app/iic_utils.h

@@ -119,6 +119,7 @@ int iic0_mpu9150_read_gam(gam_t* gam);
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 #define LIDARLITE_DEVICE_ADDR		0x62
+#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
 
 int iic0_lidarlite_write(u8 register_addr, u8 data);
 int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size);

quad/src/quad_app/sensor.c

@@ -8,11 +8,17 @@
 #include "sensor.h"
 #include "communication.h"
 #include "commands.h"
+#include "type_def.h"
 
 int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 {
-	if(iic0_mpu9150_start() == -1)
+	if (iic0_lidarlite_init()) { // init LiDAR
 		return -1;
+	}
+
+	if(iic0_mpu9150_start() == -1) {
+		return -1;
+	}
 
 	// read sensor board and fill in gryo/accelerometer/magnetometer struct
 	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
@@ -67,6 +73,10 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 
 	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;
+
     return 0;
 }
  

quad/src/quad_app/sensor_processing.c

@@ -117,16 +117,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
 			+ (1. - ALPHA) * accel_roll;
 
-//	static int loop_counter = 0;
-//	loop_counter++;
-//
-//	if(loop_counter == 50)
-//	{
-//		char dMsg[100] = {};
-//		sprintf(dMsg, "Loop time: %.4f\n", LOOP_TIME);
-//		uart0_sendStr(dMsg);
-//		loop_counter = 0;
-//	}
+
+	// Z-axis points upward, so negate distance
+	sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
 	return 0;
 }
 

quad/src/quad_app/type_def.h

@@ -112,7 +112,7 @@ typedef struct {
 }gam_t;
 
 typedef struct {
-	unsigned short distance_cm;
+	float distance_m; // distance in meters
 } lidar_t;
 
 typedef struct PID_t {
@@ -239,6 +239,10 @@ typedef struct raw_sensor {
 	// Structures to hold the current quad position & orientation
 	quadPosition_t currentQuadPosition;
 
+	// Distance from the ground, in meters, that the quadcopter is.
+	// Ideally equals 0 when landed
+	float lidar_distance_m;
+
 } raw_sensor_t;
 
 /**
@@ -266,11 +270,13 @@ typedef struct sensor {
 	float gyr_z;		// gyroscope z data
 	int gyr_z_t;	// time of gyroscope z data
 
-	int ldr_z;		//lidar z data (altitude)
-	int ldr_z_t;	//time of lidar z data
 
+	// Complementary filter outputs
 	float pitch_angle_filtered;
 	float roll_angle_filtered;
+
+	// Z-axis value obtained from LiDAR
+	// Note that this is not distance, as our Z-axis points upwards.
 	float lidar_altitude;
 
 	float phi_dot, theta_dot, psi_dot;
@@ -319,6 +325,7 @@ typedef struct parameter_t {
 	int theta_dot;
 	int phi_dot;
 	int psi_dot;
+	int lidar;
 	// VRPN blocks
 	int vrpn_x;
 	int vrpn_y;