Skip to content
Snippets Groups Projects
Commit 709aa55b authored by dawehr's avatar dawehr
Browse files

Adds gyroscope compensation.

parent 447899fa
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/quad_app/control_algorithm.c
+ 0
− 2
View file @ 709aa55b
......@@ -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);
......
quad/src/quad_app/sensor_processing.c
+ 74
− 16
View file @ 709aa55b
......@@ -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
......
quad/src/quad_app/type_def.h
+ 5
− 2
View file @ 709aa55b
......@@ -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;
......
quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
+ 23
− 29
View file @ 709aa55b
......@@ -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;
......
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