Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  * AP_IRLock_SITL.cpp
      18             :  *
      19             :  *  Created on: June 09, 2016
      20             :  *      Author: Ian Chen
      21             :  */
      22             : #include "AP_IRLock_config.h"
      23             : 
      24             : #if AP_IRLOCK_SITL_ENABLED
      25             : 
      26             : #include "AP_IRLock_SITL.h"
      27             : #include "AP_AHRS/AP_AHRS.h"
      28             : 
      29           2 : void AP_IRLock_SITL::init(int8_t bus)
      30             : {
      31           2 :     _sitl = AP::sitl();
      32           2 :     _sitl->precland_sim._type.set_and_notify(SITL::SIM_Precland::PreclandType::PRECLAND_TYPE_CONE);
      33           2 : }
      34             : 
      35             : // retrieve latest sensor data - returns true if new data is available
      36       36450 : bool AP_IRLock_SITL::update()
      37             : {
      38             :     // return immediately if not healthy
      39       36450 :     _flags.healthy = _sitl->precland_sim.healthy();
      40       36450 :     if (!_flags.healthy) {
      41       23762 :         return false;
      42             :     }
      43             : 
      44       12688 :     if (_sitl->precland_sim.last_update_ms() != _last_timestamp) {
      45        3172 :         const Vector3d position = _sitl->precland_sim.get_target_position();
      46        3172 :         const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble();
      47        3172 :         const Vector3d real_position =  body_to_ned.mul_transpose(-position);
      48        3172 :         _last_timestamp = _sitl->precland_sim.last_update_ms();
      49        3172 :         _last_update_ms = _last_timestamp;
      50        3172 :         _target_info.timestamp = _last_timestamp;
      51        3172 :         _target_info.pos_x = real_position.y;
      52        3172 :         _target_info.pos_y = -real_position.x;
      53        3172 :         _target_info.pos_z = real_position.z;
      54        3172 :         return true;
      55             :     }
      56        9516 :     return false;
      57             : }
      58             : 
      59             : #endif  // AP_IRLOCK_SITL_ENABLED