You have already implemented a velocity controller in the horizontal plane (x and y) and a position controller for altitude. In this section of the project you will use the position estimation you just implemented in order to give position control to your drone. Feel free to use any type of controller that you would like to. For example, you can use a PID controller, a constant controller, or any other type of controller that you know of.
When your position controller is enabled, it will accept commands from the JavaScript interface on the topic /pidrone/command/javascript and treat them
as target coordinates in the real world. For example, when given the inputs $(1, 1.5)$
your drone should fly $1$ meter forward and $1.5$ meters sideways from its
starting point. When your position controller is not enabled, it should pass the
commands that it receives to the underlying velocity controller. Upon being
given the same command with position control disabled, your drone should fly forward at $1$ m/s and sideways
at $1.5$ m/s.
You will be implementing your position controller from scratch. If you choose to
create any additional files, for example, a file to contain a specialized PID
class, make sure that you document the name of the files in your PDF handin.
Make sure to initialize the values of your controller properly and call the step
function (or equivalent) whenever you estimate a new position in
picam_pos_class.py.
/pidrone/command/javascript - Your controller should listen to this topic in order
to receive position or velocity commands. Arm and disarm commands should be
directly passed to the velocity controller. Flight commands (Mode 5) should be
interpreted as position setpoints if your position controller is running and
velocity setpoints otherwise. Velocity setpoint commands can be directly passed
to the velocity controller. You can initialize this subscriber in your setup
function.
/pidrone/command/velocity - Your controller should publish messages here in
order to send them to the velocity controller. This includes arm and disarm
messages, velocity messages when your position controller is not running, and
velocity messages that your position controller publishes in order to attain its
target position.
As mentioned above, you may choose any type of controller to control your position. We have successfully controlled our position with a PID controller and a constant controller. The constant controller we used published a set velocity if the drone was too far to one side and the negative if it was too far to the other side.
Depending on your controller, you may actually have to write two controllers: one that acts when the drone can see the initial frame and one when the drone cannot.
In addition to the x and y axes, your position controller will also have to publish yaw commands. If your drone turns more than $15^{\circ}$ from its starting yaw, it will no longer be able to find the saved first frame. Make sure that your controller will rotate your drone to its original yaw. We have had success in achieving this with P or PI controllers.
Show a staff member that you can complete the following tasks in order to be checked off: