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             : #include "AP_Beacon_Backend.h"
      17             : 
      18             : #if AP_BEACON_ENABLED
      19             : 
      20             : // debug
      21             : #include <stdio.h>
      22             : #include <AP_SerialManager/AP_SerialManager.h>
      23             : 
      24             : /*
      25             :   base class constructor. 
      26             :   This incorporates initialisation as well.
      27             : */
      28           5 : AP_Beacon_Backend::AP_Beacon_Backend(AP_Beacon &frontend) :
      29           5 :     _frontend(frontend)
      30             : {
      31           5 :     const AP_SerialManager &serialmanager = AP::serialmanager();
      32           5 :     uart = serialmanager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
      33           5 :     if (uart == nullptr) {
      34           4 :         return;
      35             :     }
      36             : 
      37           1 :     uart->begin(serialmanager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
      38             : }
      39             : 
      40             : // set vehicle position
      41             : // pos should be in meters in NED frame from the beacon's local origin
      42             : // accuracy_estimate is also in meters
      43       31164 : void AP_Beacon_Backend::set_vehicle_position(const Vector3f& pos, float accuracy_estimate)
      44             : {
      45       31164 :     _frontend.veh_pos_update_ms = AP_HAL::millis();
      46       31164 :     _frontend.veh_pos_accuracy = accuracy_estimate;
      47       31164 :     _frontend.veh_pos_ned = correct_for_orient_yaw(pos);
      48       31164 : }
      49             : 
      50             : // set individual beacon distance from vehicle in meters in NED frame
      51       32865 : void AP_Beacon_Backend::set_beacon_distance(uint8_t beacon_instance, float distance)
      52             : {
      53             :     // sanity check instance
      54       32865 :     if (beacon_instance >= AP_BEACON_MAX_BEACONS) {
      55           0 :         return;
      56             :     }
      57             : 
      58             :     // setup new beacon
      59       32865 :     if (beacon_instance >= _frontend.num_beacons) {
      60           0 :         _frontend.num_beacons = beacon_instance+1;
      61             :     }
      62             : 
      63       32865 :     _frontend.beacon_state[beacon_instance].distance_update_ms = AP_HAL::millis();
      64       32865 :     _frontend.beacon_state[beacon_instance].distance = distance;
      65       32865 :     _frontend.beacon_state[beacon_instance].healthy = true;
      66             : }
      67             : 
      68             : // set beacon's position
      69             : // pos should be in meters in NED from the beacon's local origin
      70       32865 : void AP_Beacon_Backend::set_beacon_position(uint8_t beacon_instance, const Vector3f& pos)
      71             : {
      72             :     // sanity check instance
      73       32865 :     if (beacon_instance >= AP_BEACON_MAX_BEACONS) {
      74           0 :         return;
      75             :     }
      76             : 
      77             :     // setup new beacon
      78       32865 :     if (beacon_instance >= _frontend.num_beacons) {
      79          20 :         _frontend.num_beacons = beacon_instance+1;
      80             :     }
      81             : 
      82             :     // set position after correcting yaw
      83       32865 :     _frontend.beacon_state[beacon_instance].position = correct_for_orient_yaw(pos);
      84             : }
      85             : 
      86             : // rotate vector (meters) to correct for beacon system yaw orientation
      87       64029 : Vector3f AP_Beacon_Backend::correct_for_orient_yaw(const Vector3f &vector)
      88             : {
      89             :     // exit immediately if no correction
      90       64029 :     if (_frontend.orient_yaw == 0) {
      91       33425 :         return vector;
      92             :     }
      93             : 
      94             :     // check for change in parameter value and update constants
      95       30604 :     if (orient_yaw_deg != _frontend.orient_yaw) {
      96           1 :         _frontend.orient_yaw.set(wrap_180(_frontend.orient_yaw.get()));
      97             : 
      98             :         // calculate rotation constants
      99           1 :         orient_yaw_deg = _frontend.orient_yaw;
     100           1 :         orient_cos_yaw = cosf(radians(orient_yaw_deg));
     101           1 :         orient_sin_yaw = sinf(radians(orient_yaw_deg));
     102             :     }
     103             : 
     104             :     // rotate x,y by -orient_yaw
     105       30604 :     Vector3f vec_rotated;
     106       30604 :     vec_rotated.x = vector.x*orient_cos_yaw - vector.y*orient_sin_yaw;
     107       30604 :     vec_rotated.y = vector.x*orient_sin_yaw + vector.y*orient_cos_yaw;
     108       30604 :     vec_rotated.z = vector.z;
     109       30604 :     return vec_rotated;
     110             : }
     111             : 
     112             : #endif  // AP_BEACON_ENABLED