Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: separate origin and home in SITL 

this allows for accurate sensor data when flying a very long distance
from the takeoff location
This commit is contained in:
Andrew Tridgell 2021-07-10 16:10:54 +10:00 committed by Randy Mackay
parent da2b69d521
commit 34eb9f7328
14 changed files with 67 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SITL/SIM_ADSB.cpp
View File

@ -192,7 +192,7 @@ void ADSB::send_report(void)
ADSB_Vehicle &vehicle = vehicles[i];
Location loc = home;
loc.offset_double(vehicle.position.x, vehicle.position.y);
loc.offset(vehicle.position.x, vehicle.position.y);
// re-init when exceeding radius range
if (home.get_distance(loc) > _sitl->adsb_radius_m) {

2
libraries/SITL/SIM_AirSim.cpp
View File

@ -307,7 +307,7 @@ void AirSim::recv_fdm(const sitl_input& input)
location.lng = state.gps.lon * 1.0e7;
location.alt = state.gps.alt * 100.0f;
position = home.get_distance_NED_double(location);
position = origin.get_distance_NED_double(location);
dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw);

48
libraries/SITL/SIM_Aircraft.cpp
View File

@ -74,6 +74,8 @@ Aircraft::Aircraft(const char *frame_str) :
void Aircraft::set_start_location(const Location &start_loc, const float start_yaw)
{
home = start_loc;
origin = home;
position.xy().zero();
home_yaw = start_yaw;
home_is_set = true;
@ -86,6 +88,11 @@ void Aircraft::set_start_location(const Location &start_loc, const float start_y
location = home;
ground_level = home.alt * 0.01f;
#if 0
// useful test for home position being very different from origin
home.offset(-3000*1000, 1800*1000);
#endif
dcm.from_euler(0.0f, 0.0f, radians(home_yaw));
}
@ -131,12 +138,15 @@ bool Aircraft::on_ground() const
*/
void Aircraft::update_position(void)
{
location = home;
location.offset_double(position.x, position.y);
location = origin;
location.offset(position.x, position.y);
location.alt = static_cast<int32_t>(home.alt - position.z * 100.0f);
#if 0
Vector3d pos_home = position;
pos_home.xy() += home.get_distance_NE_double(origin);
// logging of raw sitl data
Vector3f accel_ef = dcm * accel_body;
// @LoggerMessage: SITL
@ -155,8 +165,20 @@ void Aircraft::update_position(void)
AP_HAL::micros64(),
velocity_ef.x, velocity_ef.y, velocity_ef.z,
accel_ef.x, accel_ef.y, accel_ef.z,
position.x, position.y, position.z);
pos_home.x, pos_home.y, pos_home.z);
#endif
uint32_t now = AP_HAL::millis();
if (now - last_one_hz_ms >= 1000) {
// shift origin of position at 1Hz to current location
// this prevents sperical errors building up in the GPS data
last_one_hz_ms = now;
Vector2d diffNE = origin.get_distance_NE_double(location);
position.xy() -= diffNE;
smoothing.position.xy() -= diffNE;
origin.lat = location.lat;
origin.lng = location.lng;
}
}
/*
@ -387,6 +409,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.altitude = smoothing.location.alt * 1.0e-2;
}
if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
if (imu_rotation != last_imu_rotation) {
@ -673,7 +696,7 @@ void Aircraft::update_wind(const struct sitl_input &input)
sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)),
sinf(radians(input.wind.dir_z))) * input.wind.speed;
wind_ef.z += get_local_updraft(position);
wind_ef.z += get_local_updraft(position + home.get_distance_NED_double(origin));
const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
const float iir_coef = 0.98f; // filtering high frequencies from turbulence
@ -781,8 +804,8 @@ void Aircraft::smooth_sensors(void)
smoothing.velocity_ef += accel_e * delta_time;
smoothing.position += (smoothing.velocity_ef * delta_time).todouble();
smoothing.location = home;
smoothing.location.offset_double(smoothing.position.x, smoothing.position.y);
smoothing.location = origin;
smoothing.location.offset(smoothing.position.x, smoothing.position.y);
smoothing.location.alt = static_cast<int32_t>(home.alt - smoothing.position.z * 100.0f);
smoothing.last_update_us = now;
@ -886,7 +909,7 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
}
if (precland && precland->is_enabled()) {
precland->update(get_location(), get_position());
precland->update(get_location(), get_position_relhome());
if (precland->_over_precland_base) {
local_ground_level += precland->_origin_height;
}
@ -1012,3 +1035,14 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel)
sitl->twist.t = 0;
}
}
/*
get position relative to home
*/
Vector3d Aircraft::get_position_relhome() const
{
Vector3d pos = position;
pos.xy() += home.get_distance_NE_double(origin);
return pos;
}

7
libraries/SITL/SIM_Aircraft.h
View File

@ -127,7 +127,8 @@ public:
const Location &get_location() const { return location; }
const Vector3d &get_position() const { return position; }
// get position relative to home
Vector3d get_position_relhome() const;
// distance the rangefinder is perceiving
float rangefinder_range() const;
@ -153,6 +154,9 @@ public:
protected:
SITL *sitl;
// origin of position vector
Location origin;
// home location
Location home;
bool home_is_set;
Location location;
@ -176,6 +180,7 @@ protected:
float battery_current;
float local_ground_level; // ground level at local position
bool lock_step_scheduled;
uint32_t last_one_hz_ms;
// battery model
Battery battery;

9
libraries/SITL/SIM_CRRCSim.cpp
View File

@ -119,12 +119,9 @@ void CRRCSim::recv_fdm(const struct sitl_input &input)
gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
Location loc1, loc2;
loc2.lat = pkt.latitude * 1.0e7;
loc2.lng = pkt.longitude * 1.0e7;
const Vector2f posdelta = loc1.get_distance_NE(loc2);
position.x = posdelta.x;
position.y = posdelta.y;
origin.lat = pkt.latitude * 1.0e7;
origin.lng = pkt.longitude * 1.0e7;
position.xy().zero();
position.z = -pkt.altitude;
airspeed = pkt.airspeed;

1
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -415,6 +415,7 @@ void FlightAxis::update(const struct sitl_input &input)
position = Vector3d(state.m_aircraftPositionY_MTR,
state.m_aircraftPositionX_MTR,
-state.m_altitudeASL_MTR - home.alt*0.01);
position.xy() += origin.get_distance_NE_double(home);
accel_body = {
float(state.m_accelerationBodyAX_MPS2),

2
libraries/SITL/SIM_Gazebo.cpp
View File

@ -120,7 +120,7 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
position = Vector3d(pkt.position_xyz[0],
pkt.position_xyz[1],
pkt.position_xyz[2]);
position.xy() += origin.get_distance_NE_double(home);
// auto-adjust to simulation frame rate
time_now_us += static_cast<uint64_t>(deltat * 1.0e6);

1
libraries/SITL/SIM_JSON.cpp
View File

@ -284,6 +284,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
gyro = state.imu.gyro;
velocity_ef = state.velocity;
position = state.position;
position.xy() += origin.get_distance_NE_double(home);
// deal with euler or quaternion attitude
if ((received_bitmask & QUAT_ATT) != 0) {

1
libraries/SITL/SIM_Morse.cpp
View File

@ -513,6 +513,7 @@ void Morse::update(const struct sitl_input &input)
-state.velocity.world_linear_velocity[2]);
position = Vector3d(state.gps.x, -state.gps.y, -state.gps.z);
position.xy() += origin.get_distance_NE_double(home);
// Morse IMU accel is NEU, convert to NED
accel_body = Vector3f(state.imu.linear_acceleration[0],

6
libraries/SITL/SIM_SerialProximitySensor.cpp
View File

@ -97,7 +97,7 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
for (int8_t x=-num_post_offset; x<num_post_offset; x++) {
for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
Location post_location = post_origin;
post_location.offset_double(x*10+3, y*10+2);
post_location.offset(x*10+3, y*10+2);
if (postfile != nullptr) {
::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0);
}
@ -111,8 +111,8 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
float dist_cm = (intersection_point_cm-vehicle_pos_cm).length();
if (intersectionsfile != nullptr) {
Location intersection_point = location;
intersection_point.offset_double(intersection_point_cm.x/100.0,
intersection_point_cm.y/100.0);
intersection_point.offset(intersection_point_cm.x/100.0,
intersection_point_cm.y/100.0);
::fprintf(intersectionsfile,
"map icon %f %f barrell\n",
intersection_point.lat*1e-7,

4
libraries/SITL/SIM_Ship.cpp
View File

@ -85,7 +85,7 @@ Vector2f ShipSim::get_ground_speed_adjustment(const Location &loc, float &yaw_ra
return Vector2f(0,0);
}
Location shiploc = home;
shiploc.offset_double(ship.position.x, ship.position.y);
shiploc.offset(ship.position.x, ship.position.y);
if (loc.get_distance(shiploc) > deck_size) {
yaw_rate = 0;
return Vector2f(0,0);
@ -172,7 +172,7 @@ void ShipSim::send_report(void)
send a GLOBAL_POSITION_INT messages
*/
Location loc = home;
loc.offset_double(ship.position.x, ship.position.y);
loc.offset(ship.position.x, ship.position.y);
int32_t alt;
bool have_alt = false;

4
libraries/SITL/SIM_SilentWings.cpp
View File

@ -195,7 +195,7 @@ void SilentWings::process_packet()
curr_location.lng = pkt.position_longitude * 1.0e7;
curr_location.alt = pkt.altitude_msl * 100.0f;
ground_level = curr_location.alt * 0.01f - pkt.altitude_ground;
Vector3f posdelta = home.get_distance_NED(curr_location);
Vector3f posdelta = origin.get_distance_NED(curr_location);
position.x = posdelta.x;
position.y = posdelta.y;
position.z = posdelta.z;
@ -209,6 +209,8 @@ void SilentWings::process_packet()
// reset home location
home.lat = curr_location.lat;
home.lng = curr_location.lng;
origin.lat = home.lat;
origin.lng = home.lng;
// Resetting altitude reference point in flight can throw off a bunch
// of important calculations, so let the home altitude always be 0m MSL
home.alt = 0;

2
libraries/SITL/SIM_Webots.cpp
View File

@ -516,7 +516,7 @@ void Webots::update(const struct sitl_input &input)
-state.velocity.world_linear_velocity[2]);
position = Vector3d(state.gps.x, state.gps.y, -state.gps.z);
position.xy() += origin.get_distance_NE_double(home);
// limit to 16G to match pixhawk1
float a_limit = GRAVITY_MSS*16;

2
libraries/SITL/SIM_XPlane.cpp
View File

@ -303,6 +303,7 @@ bool XPlane::receive_data(void)
}
}
position = pos + position_zero;
position.xy() += origin.get_distance_NE_double(home);
update_position();
time_advance();
@ -318,6 +319,7 @@ bool XPlane::receive_data(void)
home.lat = loc.lat;
home.lng = loc.lng;
home.alt = loc.alt;
origin = home;
position.x = 0;
position.y = 0;
position.z = 0;