Line data    Source code

       1             : #include "AP_DAL_Beacon.h"
       2             : 
       3             : #if AP_BEACON_ENABLED
       4             : 
       5             : #include <AP_Logger/AP_Logger.h>
       6             : #include "AP_DAL.h"
       7             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       8             : 
       9          25 : AP_DAL_Beacon::AP_DAL_Beacon()
      10             : {
      11             : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
      12           5 :     const auto *bcon = AP::beacon();
      13           5 :     _RBCH.count = bcon->count();
      14          25 :     for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
      15          20 :         _RBCI[i].instance = i;
      16             :     }
      17             : #endif
      18           5 : }
      19             : 
      20      153757 : void AP_DAL_Beacon::start_frame()
      21             : {
      22      153757 :     const auto *bcon = AP::beacon();
      23             : 
      24      153757 :     const log_RBCH old = _RBCH;
      25      153757 :     if (bcon != nullptr) {
      26      153757 :         _RBCH.get_vehicle_position_ned_returncode = bcon->get_vehicle_position_ned(_RBCH.vehicle_position_ned, _RBCH.accuracy_estimate);
      27      153757 :         Location loc;
      28      153757 :         _RBCH.get_origin_returncode = bcon->get_origin(loc);
      29      153757 :         _RBCH.enabled = bcon->enabled();
      30      153757 :         _RBCH.origin_lat = loc.lat;
      31      153757 :         _RBCH.origin_lng = loc.lng;
      32      153757 :         _RBCH.origin_alt = loc.alt;
      33             :     }
      34      153757 :     WRITE_REPLAY_BLOCK_IFCHANGED(RBCH, _RBCH, old);
      35      153757 :     if (bcon == nullptr) {
      36           0 :         return;
      37             :     }
      38             : 
      39      768785 :     for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
      40      615028 :         log_RBCI &RBCI = _RBCI[i];
      41      615028 :         const log_RBCI old_RBCI = RBCI;
      42      615028 :         RBCI.last_update_ms = bcon->beacon_last_update_ms(i);
      43      615028 :         RBCI.position = bcon->beacon_position(i);
      44      615028 :         RBCI.distance = bcon->beacon_distance(i);
      45      615028 :         RBCI.healthy = bcon->beacon_healthy(i);
      46             : 
      47      615028 :         WRITE_REPLAY_BLOCK_IFCHANGED(RBCI, RBCI, old_RBCI);
      48             :     }
      49             : }
      50             : 
      51             : #endif