[ARP] Fixed Follower missing mutex for set gain

- Fix: For the set gain function missing mutex lock.
  Also moved yaw and pitch angle computation to mutex-protected
  area