New thrust pid controller work
Modify plotwidget, show only graph
need to improve roll and pitch for small error