Code for automatic anti-rotation stabilization done. (Heli puts some force against helicopter rotation, by deflecting blades.)
Next up will be force-trim code (as in real helicopter). Turns out it's actually easy to do. The principle is that when you need to keep your stick forward, you press the button and it relieves the forces on the stick, so that that position acts as new center (new 0-0 position) for it. That is, with forced feedbak (controls in helicopter). Since we mostly use spring-loaded joysticks, it will be done so that you press the button, center the stick - but it will still remember how much you are pushing. So any new deflection of the stick will be done from that remembered position to the front.
(I hope someone can do a clearer explanation.)
The actual trim will be used for autopilot. The autopilot is trying to level the helicopter, where the desired attitude can be changed. Which means that if you let go of controls with autopilot on, it will level the helicopter. Good for hovering.