AP_DAL: added methods to support EKF sources selection

libraries/AP_DAL/AP_DAL.cpp

@@ -4,6 +4,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
 
 #if APM_BUILD_TYPE(APM_BUILD_Replay)
 #include <AP_NavEKF2/AP_NavEKF2.h>
@@ -61,6 +62,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
     _RFRN.ahrs_airspeed_sensor_enabled = AP::ahrs().airspeed_sensor_enabled();
     _RFRN.available_memory = hal.util->available_memory();
     _RFRN.ahrs_trim = ahrs.get_trim();
+    _RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled();
     WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old);
 
     // update body conversion

libraries/AP_DAL/AP_DAL.h

@@ -192,6 +192,10 @@ public:
         return _RFRH.time_flying_ms;
     }
 
+    bool opticalflow_enabled(void) const {
+        return _RFRN.opticalflow_enabled;
+    }
+
     // log optical flow data
     void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
 

libraries/AP_DAL/AP_DAL_Beacon.cpp

@@ -24,6 +24,7 @@ void AP_DAL_Beacon::start_frame()
 
         Location loc;
         _RBCH.get_origin_returncode = bcon->get_origin(loc);
+        _RBCH.enabled = bcon->enabled();
         _RBCH.origin_lat = loc.lat;
         _RBCH.origin_lng = loc.lng;
         _RBCH.origin_alt = loc.alt;

libraries/AP_DAL/AP_DAL_Beacon.h

@@ -22,6 +22,11 @@ public:
         return _RBCH.get_origin_returncode;
     }
 
+    // return beacon enabled
+    bool enabled(void) const {
+        return _RBCH.enabled;
+    }
+
     // return beacon health
     bool beacon_healthy(uint8_t i) const {
         return _RBCI[i].healthy;

libraries/AP_DAL/AP_DAL_RangeFinder.cpp

@@ -89,6 +89,17 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
     WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
 }
 
+// return true if we have a range finder with the specified orientation
+bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const
+{
+    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+        if (_RRNI[i].orientation == orientation) {
+            return true;
+        }
+    }
+    return false;
+}
+
 
 AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const
 {

libraries/AP_DAL/AP_DAL_RangeFinder.h

@@ -23,6 +23,9 @@ public:
     int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
     int16_t max_distance_cm_orient(enum Rotation orientation) const;
 
+    // return true if we have a range finder with the specified orientation
+    bool has_orientation(enum Rotation orientation) const;
+
     // DAL methods:
     AP_DAL_RangeFinder();
 

libraries/AP_DAL/AP_DAL_VisualOdom.cpp

@@ -7,6 +7,19 @@
 #include <AP_Logger/AP_Logger.h>
 #include "AP_DAL.h"
 
+/*
+   update position offsets to align to AHRS position
+   should only be called when this library is not being used as the position source
+   This function does not change EKF state, so does not need to be logged
+*/
+void AP_DAL_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z)
+{
+#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
+    auto *vo = AP::visualodom();
+    vo->align_position_to_ahrs(align_xy, align_z);
+#endif
+}
+
 void AP_DAL_VisualOdom::start_frame()
 {
     const auto *vo = AP::visualodom();

libraries/AP_DAL/AP_DAL_VisualOdom.h

@@ -29,6 +29,10 @@ public:
         return RVOH.pos_offset;
     }
 
+    // update position offsets to align to AHRS position
+    // should only be called when this library is not being used as the position source
+    void align_position_to_ahrs(bool align_xy, bool align_z);
+
     AP_DAL_VisualOdom *visualodom() {
         if (RVOH.ptr_is_nullptr) {
             return nullptr;
@@ -40,7 +44,7 @@ public:
 
     void handle_message(const log_RVOH &msg) {
         RVOH = msg;
-   }
+    }
 
 private:
 

libraries/AP_DAL/LogStructure.h

@@ -67,6 +67,7 @@ struct log_RFRN {
     uint8_t airspeed_ptr_is_null:1;
     uint8_t fly_forward:1;
     uint8_t ahrs_airspeed_sensor_enabled:1;
+    uint8_t opticalflow_enabled:1;
     uint8_t _end;
 };
 
@@ -265,10 +266,11 @@ struct log_RBCH {
     int32_t origin_lat;
     int32_t origin_lng;
     int32_t origin_alt;
-    bool get_vehicle_position_ned_returncode;
+    uint8_t get_vehicle_position_ned_returncode:1;
+    uint8_t get_origin_returncode:1;
+    uint8_t enabled:1;
+    uint8_t ptr_is_nullptr:1;
     uint8_t count;
-    bool get_origin_returncode;
-    uint8_t ptr_is_nullptr;
     uint8_t _end;
 };
 
@@ -402,7 +404,7 @@ struct log_RBOH {
     { LOG_RMGI_MSG, RLOG_SIZE(RMGI),                                   \
       "RMGI", "IffffffBBBB", "LU,OX,OY,OZ,FX,FY,FZ,UFY,H,HSF,I", "----------#", "-----------" },                                        \
     { LOG_RBCH_MSG, RLOG_SIZE(RBCH),                                   \
-      "RBCH", "ffffiiiBBBB", "PX,PY,PZ,AE,OLat,OLng,OAlt,GVPR,NumInst,ORet,NPtr", "-----------", "-----------" },  \
+      "RBCH", "ffffiiiBB", "PX,PY,PZ,AE,OLat,OLng,OAlt,Flags,NumInst", "---------", "---------" },  \
     { LOG_RBCI_MSG, RLOG_SIZE(RBCI),                                   \
       "RBCI", "IffffBB", "LU,PX,PY,PZ,Dist,H,I", "smmmm-#", "?0000--" }, \
     { LOG_RVOH_MSG, RLOG_SIZE(RVOH),                                   \