I'm adding simple object tracking to the Arducopter code. I have come up with a system running on a BeagleBoard that has a downward pointing camera that captures an image, finds the object in the image, calculates it's centroid and sends that data as a MAVLink message to the ArduPilot Mega. This all works fine.
I then take that centroid information and use an algorithm similar to the existing optical flow one to find the actual offset in centimeters in the x and y directions. I then calculate the distance and bearing to the object. I then use functions very similar to those used for waypoint navigation:
- calc_XY_velocity() I've copied and modified to calculate the relative velocity to the object (this is the first place I'm not sure about)
- calc_location_error() is replaced by the object x and y distances, which give me values just like long_error and lat_error in centimeters
- calc_desired_speed() is calculated the same way
- calc_nav_rate() I've copied and modified to use the relative speed to the object. It uses the same x/y_rate_error, pi_loiter_lon/lat, pid_nav_lon/lat, and nav_lon/lat calculations as calc_nav_rate(). The PID values are the same as for waypoint (same objects are used).
- calc_loiter_pitch_roll() is unmodified
- I have extended the ALT_HOLD mode with object tracking (if an object is detected) and use these parameters:
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
So roll/pitch get updated as with waypoint navigation.
I have been testing it but I get strange results. When the copter detects an object (verified through logs), I see that it flies away from it. For example, if the object is in front of the copter, the copter pitches left and flies away. Other times, it pitches forward and keeps going fast, sometimes out of control.
I've tripled checked my code and I don't think I've swapped an x/y or lat/lon and I believe all the units are correct. The implementation is similar to what is there already. I've logged values extensively, analyzed them, and see that indeed large values are being produced, but I'm not sure why yet. I am kind of stuck and wanted to know your thoughts on what to try.
1) What are the units of nav_lon/nav_lat? I have tried to figure it out, but it is not clear.
2) Should I use the GPS speed instead of the relative speed? I would like to not have to depend on a GPS fix for object tracking...
3) Does my approach seem sound?
My log is attached with the column headings inserted at the top. The scenario is described in the log. Object tracking starts at row 812/817.
Thank you in advance!