AP_Beacon: Improve simulator testing of beacons
This commit is contained in:
priseborough
Randy Mackay
parent
1225e18898
commit
4482743af5
@ -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
|
||||
switch (beacon_id) {
|
||||
case 0:
|
||||
location_offset(beacon_loc, 0, 0);
|
||||
break;
|
||||
case 1:
|
||||
location_offset(beacon_loc, 0, ORIGIN_DISTANCE_EAST);
|
||||
break;
|
||||
case 2:
|
||||
location_offset(beacon_loc, ORIGIN_DISTANCE_NORTH, 0);
|
||||
break;
|
||||
case 3:
|
||||
location_offset(beacon_loc, ORIGIN_DISTANCE_NORTH, ORIGIN_DISTANCE_EAST);
|
||||
break;
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
Vector2f beac_diff = location_diff(origin, beacon_loc);
|
||||
Vector2f veh_diff = location_diff(origin, current_loc);
|
||||
// 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;
|
||||
|
||||
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);
|
||||
// position of each beacon
|
||||
Location beacon_loc = beacon_origin;
|
||||
switch (beacon_id) {
|
||||
case 0:
|
||||
// NW corner
|
||||
location_offset(beacon_loc, ORIGIN_OFFSET_NORTH + BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST + BEACON_SPACING_EAST/2);
|
||||
break;
|
||||
case 1:
|
||||
// SE corner
|
||||
location_offset(beacon_loc, ORIGIN_OFFSET_NORTH - BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST + BEACON_SPACING_EAST);
|
||||
break;
|
||||
case 2:
|
||||
// SW corner
|
||||
location_offset(beacon_loc, ORIGIN_OFFSET_NORTH - BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST - BEACON_SPACING_EAST/2);
|
||||
break;
|
||||
case 3:
|
||||
// NW corner
|
||||
location_offset(beacon_loc, ORIGIN_OFFSET_NORTH + BEACON_SPACING_NORTH/2, ORIGIN_OFFSET_EAST - BEACON_SPACING_EAST/2);
|
||||
break;
|
||||
}
|
||||
|
||||
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 - 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
|
||||
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user