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             :   Base class for serially-attached simulated positioning beacons
      17             : */
      18             : 
      19             : #include "SIM_SerialBeacon.h"
      20             : 
      21             : #if AP_SIM_NOOPLOOP_ENABLED
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : #include <SITL/SITL.h>
      25             : 
      26             : using namespace SITL;
      27             : 
      28      137493 : void SerialBeacon::update()
      29             : {
      30      137493 :     if (!init_sitl_pointer()) {
      31      136921 :         return;
      32             :     }
      33             : 
      34             :     // send data out at the beacon's reading rate:
      35      137493 :     const uint32_t now_ms = AP_HAL::millis();
      36      137493 :     if (now_ms - last_sent_ms < reading_interval_ms()) {
      37      136921 :         return;
      38             :     }
      39         572 :     last_sent_ms = now_ms;
      40             : 
      41             :     // the beacon system origin is the same as the vehicle/EKF origin:
      42         572 :     const Location &origin = _sitl->state.home;
      43         572 :     if (!origin.initialised()) {
      44           0 :         return;
      45             :     }
      46             : 
      47             :     // truth location of the simulated vehicle:
      48             :     const Location veh_loc {
      49         572 :         int32_t(_sitl->state.latitude * 1.0e7),
      50         572 :         int32_t(_sitl->state.longitude * 1.0e7),
      51         572 :         int32_t(_sitl->state.altitude * 1.0e2),
      52             :         Location::AltFrame::ABSOLUTE
      53         572 :     };
      54             : 
      55             :     // vehicle position in NED metres relative to the beacon origin:
      56         572 :     const Vector3f pos_ned = origin.get_distance_NED(veh_loc);
      57             : 
      58         572 :     send_data(pos_ned);
      59             : }
      60             : 
      61             : #endif  // AP_SIM_NOOPLOOP_ENABLED