From 40d438eeaff61c487b960d6e40ed372caed6cda6 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Tue, 25 Apr 2017 20:45:15 -0500
Subject: [PATCH] Adds gyroscope compensation.
---
quad/src/quad_app/control_algorithm.c | 2 -
quad/src/quad_app/sensor_processing.c | 90 +++++++++++++++----
quad/src/quad_app/type_def.h | 7 +-
.../real_quad/src/hw_impl_zybo_optical_flow.c | 52 +++++------
4 files changed, 102 insertions(+), 49 deletions(-)
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 05e4b4b74..7654903b8 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -80,7 +80,6 @@ int control_algorithm_init(parameter_t * ps)
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_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");
@@ -478,7 +477,6 @@ 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);
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 59511fa9d..1c6afabe1 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -36,26 +36,79 @@ int sensor_processing_init(sensor_t* sensor_struct) {
return 0;
}
+
+ // Focal length in mm = 16
+// static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/*
- * Populates the xVel and yVel fields of flow_data,
- * using the flowX and flowY, and the given distance
+ * Modifies the pixel flow values to account for rotations
+ * Theta = pitch, phi = roll
*/
-// Focal length in mm = 16
-static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
-void flow_to_vel(px4flow_t* flow_data, double distance) {
- double loop_time = get_last_loop_time();
- if (loop_time != 0) {
- 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;
- }
+ void flow_gyro_compensation(px4flow_t* flow_data, double distance, double phi, double theta, double psi_d) {
+ static double last_phi = 0;
+ static double last_theta = 0;
+
+ // Calculate difference in angles
+ double phi_d = phi - last_phi;
+ double theta_d = theta - last_theta;
+
+ // Convert angles to body rotations (gyroscope equivalents)
+ ///////////////////------- Inverse of AEB matrix -------//////////////////
+ // | p | | 1 0 -sin(Phi) | | Phi_d |
+ // | q | = | 0 cos(Phi) cos(Theta)*sin(Phi) | | theta_d |
+ // | r | | 0 -sin(Phi) cos(Phi)*cos(Theta) | | Psi_d |
+
+ double sin_phi = sin(phi);
+ double cos_theta = cos(theta);
+ double cos_phi = cos(phi);
+
+ double p = phi_d - sin_phi*psi_d;
+ double q = cos_phi*theta_d + cos_theta*sin_phi*psi_d;
+ double r = -sin_phi*theta_d + cos_phi*cos_theta*psi_d;
+
+ // TODO: Clamp correction to maximum search radius
+ // TODO: Verify p vs q here
+ // Compensates for angle rotation using the gyroscope
+ double flow_x_rad_corr = flow_data->flow_x_rad - p;
+ double flow_y_rad_corr = flow_data->flow_y_rad - q;
+
+ // Swap x and y to switch from rotation around an axis to movement along an axis
+ double x_dist = -flow_y_rad_corr * distance;
+ double y_dist = flow_x_rad_corr * distance;
+
+ if (flow_data->quality > PX4FLOW_QUAL_MIN) {
+ flow_data->xVel = x_dist / flow_data->dt;
+ flow_data->yVel = y_dist / flow_data->dt;
+ }
+ // Gradually decay towards 0 if quality is bad
+ else {
+ flow_data->xVel *= PX4FLOW_VEL_DECAY;
+ flow_data->yVel *= PX4FLOW_VEL_DECAY;
}
+
+ // Store angles for next call
+ last_phi = phi;
+ last_theta = theta;
}
+
+/*
+ * Populates the xVel and yVel fields of flow_data,
+ * using the flowX and flowY, and the given distance
+ */
+// void flow_to_vel(px4flow_t* flow_data, double distance) {
+// double loop_time = get_last_loop_time();
+// if (loop_time != 0) {
+// 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;
+// }
+// }
+// }
+
int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
{
// Filter accelerometer values
@@ -129,7 +182,12 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// Simply copy optical flow data
sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
- flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
+ flow_gyro_compensation(&sensor_struct->optical_flow,
+ raw_sensor_struct->lidar_distance_m,
+ sensor_struct->roll_angle_filtered,
+ sensor_struct->pitch_angle_filtered,
+ sensor_struct->currentQuadPosition.yaw);
+ // 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);
//Note: This was wrong and dumb
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index c424869c4..a16efb123 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -122,9 +122,12 @@ typedef struct lidar {
typedef struct px4flow {
double xVel, yVel;
- double distance;
- double flowX, flowY;
+ // Flow around the x and y axes in radians
+ double flow_x_rad, flow_y_rad;
+
+ // Time since last readout in seconds
+ double dt;
int16_t quality;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
index 8a7e17d3a..d9a947a4a 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
@@ -17,40 +17,34 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
struct I2CDriver *i2c = self->i2c;
int error = 0;
- struct {
- uint16_t frameCount;
-
- int16_t pixelFlowXSum;
- int16_t pixelFlowYSum;
- int16_t flowCompX;
- int16_t flowCompY;
- int16_t qual;
-
- int16_t gyroXRate;
- int16_t gyroYRate;
- int16_t gyroZRate;
-
- uint8_t gyroRange;
- uint8_t sonarTimestamp;
- int16_t groundDistance;
- } i2cFrame;
-
- u8 buf[sizeof(i2cFrame)];
+ typedef struct i2c_integral_frame
+ {
+ uint16_t frame_count_since_last_readout;//number of flow measurements since last I2C readout [#frames]
+ int16_t pixel_flow_x_integral;//accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000]
+ int16_t pixel_flow_y_integral;//accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000]
+ int16_t gyro_x_rate_integral;//accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000]
+ int16_t gyro_y_rate_integral;//accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000]
+ int16_t gyro_z_rate_integral;//accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000]
+ uint32_t integration_timespan;//accumulation timespan in microseconds since last I2C readout [microseconds]
+ uint32_t sonar_timestamp;// time since last sonar update [microseconds]
+ int16_t ground_distance;// Ground distance in meters*1000 [meters*1000]
+ int16_t gyro_temperature;// Temperature * 100 in centi-degrees Celsius [degcelsius*100]
+ uint8_t quality;// averaged quality of accumulated flow values [0:bad quality;255: max quality]
+ } __attribute__((packed)) i2c_integral_frame;
+
+ u8 buf[sizeof(i2c_integral_frame)];
// Read the sensor value
- error = px4flow_read(i2c, buf, 0x00, sizeof(i2cFrame));
+ error = px4flow_read(i2c, buf, 0x16, sizeof(i2c_integral_frame));
if(error == 0) {
//Copy into struct
- memcpy(&i2cFrame, buf, sizeof(i2cFrame));
-
- of->xVel = i2cFrame.flowCompX / 1000.;
- of->yVel = i2cFrame.flowCompY / 1000.;
- // They call it "sum", but it's just the latest pixel flow * 10
- of->flowX = (double)i2cFrame.pixelFlowXSum / 10.;
- of->flowY = (double)i2cFrame.pixelFlowYSum / 10.;
- of->quality = i2cFrame.qual;
- of->distance = i2cFrame.groundDistance / 1000.;
+ memcpy(&i2c_integral_frame, buf, sizeof(i2c_integral_frame));
+
+ of->flow_x_rad = 0.0001 * i2c_integral_frame.pixel_flow_x_integral;
+ of->flow_y_rad = 0.0001 * i2c_integral_frame.pixel_flow_y_integral;
+ of->quality = i2c_integral_frame.quality;
+ of->dt = (double)i2c_integral_frame.integration_timespan / 1000000;
}
return error;
--
GitLab