AP_Beacon: Improve simulator testing of beacons

libraries/AP_Beacon/AP_Beacon_SITL.cpp

@@ -24,8 +24,21 @@
 extern const AP_HAL::HAL& hal;
 
 #define NUM_BEACONS 4
-#define ORIGIN_DISTANCE_EAST 20
-#define ORIGIN_DISTANCE_NORTH 10
+/*
+ *  Define a rectangular pattern of beacons with the pattern centroid located at the beacon origin as defined by the following params:
+ *
+ * BCN_ALT - Height above the WGS-84 geoid (m)
+ * BCN_LATITUDE - WGS-84 latitude (deg)
+ * BCN_LONGITUDE - WGS-84 longitude (deg)
+ *
+ * The spacing between beacons in the North/South and East/West directions is defined by the following parameters:
+ */
+#define BEACON_SPACING_NORTH 10.0
+#define BEACON_SPACING_EAST 20.0
+
+// The centroid of the pattern can be moved using using the following parameters:
+#define ORIGIN_OFFSET_NORTH 2.5 // shifts beacon pattern centroid North (m)
+#define ORIGIN_OFFSET_EAST 5.0 // shifts beacon pattern centroid East (m)
 
 // constructor
 AP_Beacon_SITL::AP_Beacon_SITL(AP_Beacon &frontend) :
@@ -52,43 +65,51 @@ void AP_Beacon_SITL::update(void)
     uint8_t beacon_id = next_beacon;
     next_beacon = (next_beacon+1) % NUM_BEACONS;
 
-    Location origin;
-    origin.lat = get_beacon_origin_lat() * 1.0e7;
-    origin.lng = get_beacon_origin_lon() * 1.0e7;
-    origin.alt = get_beacon_origin_alt() * 1.0e2;
-    Location beacon_loc = origin;
-    beacon_loc.alt += 120; // this is a hack to replicate the placement of beacons on tripods
+    // truth location of the flight vehicle
+    Location current_loc;
+    current_loc.lat = sitl->state.latitude * 1.0e7;
+    current_loc.lng = sitl->state.longitude * 1.0e7;
+    current_loc.alt = sitl->state.altitude * 1.0e2;
+
+    // where the beacon system origin is located
+    Location beacon_origin;
+    beacon_origin.lat = get_beacon_origin_lat() * 1.0e7;
+    beacon_origin.lng = get_beacon_origin_lon() * 1.0e7;
+    beacon_origin.alt = get_beacon_origin_alt() * 1.0e2;
+
+    // position of each beacon
+    Location beacon_loc = beacon_origin;
     switch (beacon_id) {
     case 0:
-        location_offset(beacon_loc, 0, 0);
+        // NW corner
+        location_offset(beacon_loc, ORIGIN_OFFSET_NORTH + BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST + BEACON_SPACING_EAST/2);
         break;
     case 1:
-        location_offset(beacon_loc, 0, ORIGIN_DISTANCE_EAST);
+        // SE corner
+        location_offset(beacon_loc, ORIGIN_OFFSET_NORTH - BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST + BEACON_SPACING_EAST);
         break;
     case 2:
-        location_offset(beacon_loc, ORIGIN_DISTANCE_NORTH, 0);
+        // SW corner
+        location_offset(beacon_loc, ORIGIN_OFFSET_NORTH - BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST - BEACON_SPACING_EAST/2);
         break;
     case 3:
-        location_offset(beacon_loc, ORIGIN_DISTANCE_NORTH, ORIGIN_DISTANCE_EAST);
+        // NW corner
+        location_offset(beacon_loc, ORIGIN_OFFSET_NORTH + BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST - BEACON_SPACING_EAST/2);
         break;
     }
 
-    Location current_loc;
-    current_loc.lat = sitl->state.latitude * 1.0e7;
-    current_loc.lng = sitl->state.longitude * 1.0e7;
-    current_loc.alt = sitl->state.altitude * 1.0e2;
-
-    Vector2f beac_diff = location_diff(origin, beacon_loc);
-    Vector2f veh_diff = location_diff(origin, current_loc);
+    Vector2f beac_diff = location_diff(beacon_origin, beacon_loc);
+    Vector2f veh_diff = location_diff(beacon_origin, current_loc);
 
-    Vector3f veh_pos3d(veh_diff.x, veh_diff.y, (current_loc.alt - origin.alt)*1.0e-2);
-    Vector3f beac_pos3d(beac_diff.x, beac_diff.y, (origin.alt - beacon_loc.alt)*1.0e-2);
+    Vector3f veh_pos3d(veh_diff.x, veh_diff.y, (current_loc.alt - beacon_origin.alt)*1.0e-2);
+    Vector3f beac_pos3d(beac_diff.x, beac_diff.y, (beacon_origin.alt - beacon_loc.alt)*1.0e-2);
     Vector3f beac_veh_offset = veh_pos3d - beac_pos3d;
 
     set_beacon_position(beacon_id, beac_pos3d);
     set_beacon_distance(beacon_id, beac_veh_offset.length());
     set_vehicle_position(veh_pos3d, 0.5f);
     last_update_ms = now;
+
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_Beacon/sitl/sitl_beacons.param

@@ -1,13 +1,17 @@
-#parameters to allow use of indoor loiter with AP_Beacon library and Pozyx running IndoorLoiter2.ino and plugged into Telem2
-# Beacon origin set to default SITL location (Australia)
-# Beacon type set to SITL
-# EK2_GPS_TYPE to 0 for regular GPS useage
-# GPS_TYPE to 0 to disable GPS
+# Parameters to allow use of indoor loiter with AP_Beacon library and Pozyx running IndoorLoiter2.ino and plugged into Telem2
+#
 # disable GPS arming check
 ARMING_CHECK,-9
-BCN_ALT,584
+# Set beacon origin to be approx 1.2m above ground level
+BCN_ALT,585.3
+# the beacon origin must match the flight simulator origin - set to default SITL location (Australia)
 BCN_LATITUDE,-35.363261
 BCN_LONGITUDE,149.165230
+# tell the beacon library we are using simulated beacons
 BCN_TYPE,10
+# tell the autopilot to ignore the GPS
+GPS_TYPE,0
+# log data before arming to asist with diasnostics
+LOG_DISARM,1
+# Make sure the normal EKF aiding mode is selected (beacon fusion not tested in other modes)
 EK2_GPS_TYPE,0
-GPS_TYPE,0