Line data    Source code

       1             : /*
       2             :  * AP_IRLock.cpp
       3             :  *
       4             :  *  Created on: Nov 12, 2014
       5             :  *      Author: MLandes
       6             :  */
       7             : 
       8             : #include "AP_IRLock_config.h"
       9             : 
      10             : #if AP_IRLOCK_ENABLED
      11             : 
      12             : #include "AP_IRLock.h"
      13             : 
      14             : // retrieve body frame unit vector in direction of target
      15             : // returns true if data is available
      16        3172 : bool AP_IRLock::get_unit_vector_body(Vector3f& ret) const
      17             : {
      18             :     // return false if we have no target
      19        3172 :     if (!_flags.healthy) {
      20           0 :         return false;
      21             :     }
      22             : 
      23             :     // use data from first (largest) object
      24        3172 :     ret.x = -_target_info.pos_y;
      25        3172 :     ret.y = _target_info.pos_x;
      26        3172 :     ret.z = _target_info.pos_z;
      27        3172 :     ret /= ret.length();
      28        3172 :     return true;
      29             : }
      30             : 
      31             : #endif  // AP_IRLOCK_ENABLED