Top mounted indoor PX4Flow

1

Hello,

I have been working with the px4flow sensor to allow a quadcopter to hover in place. My vision-based application, however, requires the optical flow sensor to collect features from the ceiling (room is ~8 ft high) rather than the floor due to dynamic imagery on the ground. I’ve gone through the px4flow source code and made, what I believe, to be appropriate modifications, but after test flights (with PX4Flow mounted on top) it seems that the optical flow doesn’t stabilize the quadcopter in the XY plane (it stabilizes well when facing the ground). So, I’m hoping that someone more familiar with the system could potentially review my changes? There aren’t too many changes so I’m hoping it won’t take someone familiar with the system very long.

Repo: GitHub - priseborough/px4flow
Branch: klt_flow

All changes are made within main.c:

// Original gyroscope coordinate transformation (PX4Flow on Bottom)
float x_rate =   y_rate_sensor;
float y_rate = - x_rate_sensor;
float z_rate =   z_rate_sensor;

To me the above code makes sense if you look at the picture under the ‘orientation’ section of the following link, which has the y-direction of the px4flow pointing in the x-direction of the autopilot system: https://pixhawk.org/modules/px4flow

// New gyroscope coordinate transformation (PX4Flow on Top). x-rate does
// not change from original, y-rate is in the opposite direction, and z-rate is 
//also in the opposite direction
float x_rate =   y_rate_sensor;
float y_rate =   x_rate_sensor;
float z_rate = - z_rate_sensor;

Now that the gyroscope is rotated, I change the flow rate direction in the y_direction after the KLT optical flow has been computed (x_direction does not change will a roll of 180 degrees):

//original:
qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);

//new:
qual = compute_klt(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
pixel_flow_y = - pixel_flow_y;

Lastly, since I use an offboard sonar (one separate from the one that comes on the PX4Flow board) which is always pointing down, I use the height of the room in meters (RM_HGT_M), the distance between the optical flow sonar sensor and the offboard sonar sensor (SENS_SENS_M), and the sonar reading which gives the distance to the floor to get the distance to the ceiling:

//Original
if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
{
   sonar_distance_filtered = 0.0f;
   distance_valid = false;
}
else
{
   distance_valid = true;
}

//New:
if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
{
   sonar_distance_filtered = 0.0f;
   distance_valid = false;
}
else
{
   distance_valid = true;
}
sonar_distance_filtered = (RM_HGT_M - SENS_SENS_M) - sonar_distance_filtered;
sonar_distance_raw = (RM_HGT_M - SENS_SENS_M) - sonar_distance_raw;

Does anyone more familiar with the system have any thoughts? Is there another section that I need to modify that I missed?

I’m currently looking into a way to print out debug statements within the code for further inspection as my only tests currently are to fly the quadcopter and see if it stabilizes (it currently drifts, similar to flying manual mode).

Thanks in advance for any comments/advice (and for actually reading this entire post).

1 Like