aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: convert to double precision for positions

Browse Source
This commit is contained in:
Andrew Tridgell 2021-06-21 12:59:02 +10:00
parent 70f874e288
commit fb275c9874
21 changed files with 55 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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(location);
position = home.get_distance_NED_double(location);
dcm.from_euler(state.pose.roll, state.pose.pitch, state.pose.yaw);

18
libraries/SITL/SIM_Aircraft.cpp
View File

@ -132,7 +132,7 @@ bool Aircraft::on_ground() const
void Aircraft::update_position(void)
{
location = home;
location.offset(position.x, position.y);
location.offset_double(position.x, position.y);
location.alt = static_cast<int32_t>(home.alt - position.z * 100.0f);
@ -552,7 +552,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
const bool was_on_ground = on_ground();
// new position vector
position += velocity_ef * delta_time;
position += (velocity_ef * delta_time).todouble();
// velocity relative to air mass, in earth frame
velocity_air_ef = velocity_ef + wind_ef;
@ -700,7 +700,7 @@ void Aircraft::update_wind(const struct sitl_input &input)
void Aircraft::smooth_sensors(void)
{
uint64_t now = time_now_us;
Vector3f delta_pos = position - smoothing.position;
Vector3d delta_pos = position - smoothing.position;
if (smoothing.last_update_us == 0 || delta_pos.length() > 10) {
smoothing.position = position;
smoothing.rotation_b2e = dcm;
@ -719,7 +719,7 @@ void Aircraft::smooth_sensors(void)
// calculate required accel to get us to desired position and velocity in the time_constant
const float time_constant = 0.1f;
Vector3f dvel = (velocity_ef - smoothing.velocity_ef) + (delta_pos / time_constant);
Vector3f dvel = (velocity_ef - smoothing.velocity_ef) + (delta_pos / time_constant).tofloat();
Vector3f accel_e = dvel / time_constant + (dcm * accel_body + Vector3f(0.0f, 0.0f, GRAVITY_MSS));
const float accel_limit = 14 * GRAVITY_MSS;
accel_e.x = constrain_float(accel_e.x, -accel_limit, accel_limit);
@ -779,7 +779,7 @@ void Aircraft::smooth_sensors(void)
// integrate to get new position
smoothing.velocity_ef += accel_e * delta_time;
smoothing.position += smoothing.velocity_ef * delta_time;
smoothing.position += (smoothing.velocity_ef * delta_time).todouble();
smoothing.location = home;
smoothing.location.offset(smoothing.position.x, smoothing.position.y);
@ -843,7 +843,7 @@ void Aircraft::extrapolate_sensors(float delta_time)
// new velocity and position vectors
velocity_ef += accel_earth * delta_time;
position += velocity_ef * delta_time;
position += (velocity_ef * delta_time).todouble();
velocity_air_ef = velocity_ef + wind_ef;
velocity_air_bf = dcm.transposed() * velocity_air_ef;
}
@ -928,7 +928,7 @@ void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
}
}
float Aircraft::get_local_updraft(Vector3f currentPos)
float Aircraft::get_local_updraft(const Vector3d &currentPos)
{
int scenario = sitl->thermal_scenario;
@ -973,10 +973,10 @@ float Aircraft::get_local_updraft(Vector3f currentPos)
float w = 0.0f;
float r2;
for (iThermal=0;iThermal<n_thermals;iThermal++) {
Vector3f thermalPos(thermals_x[iThermal] + driftX/thermals_w[iThermal],
Vector3d thermalPos(thermals_x[iThermal] + driftX/thermals_w[iThermal],
thermals_y[iThermal] + driftY/thermals_w[iThermal],
0);
Vector3f relVec = currentPos - thermalPos;
Vector3d relVec = currentPos - thermalPos;
r2 = relVec.x*relVec.x + relVec.y*relVec.y;
w += thermals_w[iThermal]*exp(-r2/(thermals_r[iThermal]*thermals_r[iThermal]));
}

8
libraries/SITL/SIM_Aircraft.h
View File

@ -127,7 +127,7 @@ public:
const Location &get_location() const { return location; }
const Vector3f &get_position() const { return position; }
const Vector3d &get_position() const { return position; }
// distance the rangefinder is perceiving
float rangefinder_range() const;
@ -166,7 +166,7 @@ protected:
Vector3f wind_ef; // m/s, earth frame
Vector3f velocity_air_ef; // velocity relative to airmass, earth frame
Vector3f velocity_air_bf; // velocity relative to airmass, body frame
Vector3f position; // meters, NED from origin
Vector3d position; // meters, NED from origin
float mass; // kg
float external_payload_mass; // kg
Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame
@ -297,7 +297,7 @@ protected:
void add_twist_forces(Vector3f &rot_accel);
// get local thermal updraft
float get_local_updraft(Vector3f currentPos);
float get_local_updraft(const Vector3d &currentPos);
private:
uint64_t last_time_us;
@ -313,7 +313,7 @@ private:
Vector3f accel_body;
Vector3f gyro;
Matrix3f rotation_b2e;
Vector3f position;
Vector3d position;
Vector3f velocity_ef;
uint64_t last_update_us;
Location location;

2
libraries/SITL/SIM_BalanceBot.cpp
View File

@ -149,7 +149,7 @@ void BalanceBot::update(const struct sitl_input &input)
velocity_ef += accel_earth * delta_time;
// new position vector
position += (velocity_ef * delta_time);
position += (velocity_ef * delta_time).todouble();
// neglect roll
dcm.to_euler(&r, &p, &y);

2
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -412,7 +412,7 @@ void FlightAxis::update(const struct sitl_input &input)
velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
state.m_velocityWorldV_MPS,
state.m_velocityWorldW_MPS);
position = Vector3f(state.m_aircraftPositionY_MTR,
position = Vector3d(state.m_aircraftPositionY_MTR,
state.m_aircraftPositionX_MTR,
-state.m_altitudeASL_MTR - home.alt*0.01);

2
libraries/SITL/SIM_FlightAxis.h
View File

@ -178,7 +178,7 @@ private:
uint64_t socket_frame_counter;
uint64_t last_socket_frame_counter;
double last_frame_count_s;
Vector3f position_offset;
Vector3d position_offset;
Vector3f last_velocity_ef;
const char *controller_ip = "127.0.0.1";

6
libraries/SITL/SIM_Gazebo.cpp
View File

@ -117,9 +117,9 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
static_cast<float>(pkt.velocity_xyz[1]),
static_cast<float>(pkt.velocity_xyz[2]));
position = Vector3f(static_cast<float>(pkt.position_xyz[0]),
static_cast<float>(pkt.position_xyz[1]),
static_cast<float>(pkt.position_xyz[2]));
position = Vector3d(pkt.position_xyz[0],
pkt.position_xyz[1],
pkt.position_xyz[2]);
// auto-adjust to simulation frame rate

10
libraries/SITL/SIM_JSON.cpp
View File

@ -184,6 +184,16 @@ uint16_t JSON::parse_sensors(const char *json)
break;
}
case DATA_VECTOR3D: {
Vector3d *v = (Vector3d *)key.ptr;
if (sscanf(p, "[%lf, %lf, %lf]", &v->x, &v->y, &v->z) != 3) {
printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
return received_bitmask;
}
//printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z);
break;
}
case QUATERNION: {
Quaternion *v = static_cast<Quaternion*>(key.ptr);
if (sscanf(p, "[%f, %f, %f, %f]", &(v->q1), &(v->q2), &(v->q3), &(v->q4)) != 4) {

5
libraries/SITL/SIM_JSON.h
View File

@ -68,6 +68,7 @@ private:
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR3F,
DATA_VECTOR3D,
QUATERNION,
};
@ -77,7 +78,7 @@ private:
Vector3f gyro;
Vector3f accel_body;
} imu;
Vector3f position;
Vector3d position;
Vector3f attitude;
Quaternion quaternion;
Vector3f velocity;
@ -100,7 +101,7 @@ private:
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F, true },
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
{ "", "position", &state.position, DATA_VECTOR3F, true },
{ "", "position", &state.position, DATA_VECTOR3D, true },
{ "", "attitude", &state.attitude, DATA_VECTOR3F, false },
{ "", "quaternion", &state.quaternion, QUATERNION, false },
{ "", "velocity", &state.velocity, DATA_VECTOR3F, true },

2
libraries/SITL/SIM_Morse.cpp
View File

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

4
libraries/SITL/SIM_Precland.cpp
View File

@ -101,7 +101,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
AP_GROUPEND
};
void SIM_Precland::update(const Location &loc, const Vector3f &position)
void SIM_Precland::update(const Location &loc, const Vector3d &position)
{
if (!_enable) {
_healthy = false;
@ -162,7 +162,7 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position)
break;
}
}
_target_pos = position - Vector3f(center.x, center.y, origin_height_m);
_target_pos = position - Vector3d(center.x, center.y, origin_height_m);
_healthy = true;
}

6
libraries/SITL/SIM_Precland.h
View File

@ -28,7 +28,7 @@ public:
};
// update precland state
void update(const Location &loc, const Vector3f &position);
void update(const Location &loc, const Vector3d &position);
// true if precland sensor is online and healthy
bool healthy() const { return _healthy; }
@ -36,7 +36,7 @@ public:
// timestamp of most recent data read from the sensor
uint32_t last_update_ms() const { return _last_update_ms; }
const Vector3f &get_target_position() const { return _target_pos; }
const Vector3d &get_target_position() const { return _target_pos; }
bool is_enabled() const {return static_cast<bool>(_enable);}
void set_default_location(float lat, float lon, int16_t yaw);
static const struct AP_Param::GroupInfo var_info[];
@ -60,7 +60,7 @@ public:
private:
uint32_t _last_update_ms;
bool _healthy;
Vector3f _target_pos;
Vector3d _target_pos;
};
}

2
libraries/SITL/SIM_Rover.cpp
View File

@ -158,7 +158,7 @@ void SimRover::update(const struct sitl_input &input)
velocity_ef += accel_earth * delta_time;
// new position vector
position += velocity_ef * delta_time;
position += (velocity_ef * delta_time).todouble();
update_external_payload(input);

2
libraries/SITL/SIM_Sailboat.cpp
View File

@ -294,7 +294,7 @@ void Sailboat::update(const struct sitl_input &input)
velocity_ef = velocity_ef_water + tide_velocity_ef;
// new position vector
position += velocity_ef * delta_time;
position += (velocity_ef * delta_time).todouble();
// update lat/lon/altitude
update_position();

2
libraries/SITL/SIM_Submarine.cpp
View File

@ -138,7 +138,7 @@ void Submarine::calculate_buoyancy_torque(Vector3f &torque)
* @param position
* @return float
*/
float Submarine::calculate_sea_floor_depth(const Vector3f &/*position*/)
float Submarine::calculate_sea_floor_depth(const Vector3d &/*position*/)
{
return 50;
}

2
libraries/SITL/SIM_Submarine.h
View File

@ -90,7 +90,7 @@ protected:
bool on_ground() const override;
// calculate sea floor depth based for terrain follow
float calculate_sea_floor_depth(const Vector3f &/*position*/);
float calculate_sea_floor_depth(const Vector3d &/*position*/);
// calculate rotational and linear accelerations
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
// calculate buoyancy

12
libraries/SITL/SIM_Vicon.cpp
View File

@ -68,7 +68,7 @@ bool Vicon::get_free_msg_buf_index(uint8_t &index)
}
void Vicon::update_vicon_position_estimate(const Location &loc,
const Vector3f &position,
const Vector3d &position,
const Vector3f &velocity,
const Quaternion &attitude)
{
@ -122,7 +122,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
Vector3f pos_offset_ef = rot * pos_offset;
// add earth frame sensor offset and glitch to position
Vector3f pos_corrected = position + pos_offset_ef + _sitl->vicon_glitch.get();
Vector3d pos_corrected = position + (pos_offset_ef + _sitl->vicon_glitch.get()).todouble();
// calculate a velocity offset due to the antenna position offset and body rotation rate
// note: % operator is overloaded for cross product
@ -140,10 +140,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
if (vicon_yaw_deg != 0) {
const float vicon_yaw_rad = radians(vicon_yaw_deg);
yaw = wrap_PI(yaw - vicon_yaw_rad);
Matrix3f vicon_yaw_rot;
Matrix3d vicon_yaw_rot;
vicon_yaw_rot.from_euler(0, 0, -vicon_yaw_rad);
pos_corrected = vicon_yaw_rot * pos_corrected;
vel_corrected = vicon_yaw_rot * vel_corrected;
vel_corrected = vicon_yaw_rot.tofloat() * vel_corrected;
}
// add yaw error reported to vehicle
@ -220,7 +220,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
Matrix3f body_ned_m;
attitude_curr.rotation_matrix(body_ned_m);
Vector3f pos_delta = body_ned_m * (pos_corrected - _position_prev);
Vector3f pos_delta = body_ned_m * (pos_corrected - _position_prev).tofloat();
float postion_delta[3] = {pos_delta.x, pos_delta.y, pos_delta.z};
// send vision position delta
@ -252,7 +252,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
/*
update vicon sensor state
*/
void Vicon::update(const Location &loc, const Vector3f &position, const Vector3f &velocity, const Quaternion &attitude)
void Vicon::update(const Location &loc, const Vector3d &position, const Vector3f &velocity, const Quaternion &attitude)
{
if (!init_sitl_pointer()) {
return;

6
libraries/SITL/SIM_Vicon.h
View File

@ -34,7 +34,7 @@ public:
Vicon();
// update state
void update(const Location &loc, const Vector3f &position, const Vector3f &velocity, const Quaternion &attitude);
void update(const Location &loc, const Vector3d &position, const Vector3f &velocity, const Quaternion &attitude);
private:
@ -70,7 +70,7 @@ private:
bool get_free_msg_buf_index(uint8_t &index);
void update_vicon_position_estimate(const Location &loc,
const Vector3f &position,
const Vector3d &position,
const Vector3f &velocity,
const Quaternion &attitude);
@ -79,7 +79,7 @@ private:
// position delta message
Quaternion _attitude_prev; // Rotation to previous MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED
Vector3f _position_prev; // previous position from origin (m) MAV_FRAME_LOCAL_NED
Vector3d _position_prev; // previous position from origin (m) MAV_FRAME_LOCAL_NED
};
}

2
libraries/SITL/SIM_Webots.cpp
View File

@ -515,7 +515,7 @@ void Webots::update(const struct sitl_input &input)
+state.velocity.world_linear_velocity[1],
-state.velocity.world_linear_velocity[2]);
position = Vector3f(state.gps.x, state.gps.y, -state.gps.z);
position = Vector3d(state.gps.x, state.gps.y, -state.gps.z);
// limit to 16G to match pixhawk1

2
libraries/SITL/SIM_XPlane.cpp
View File

@ -115,7 +115,7 @@ bool XPlane::receive_data(void)
one << PropPitch | one << EngineRPM | one << PropRPM | one << Generator |
one << Mixture);
Location loc {};
Vector3f pos;
Vector3d pos;
uint32_t wait_time_ms = 1;
uint32_t now = AP_HAL::millis();

2
libraries/SITL/SIM_XPlane.h
View File

@ -54,7 +54,7 @@ private:
uint64_t time_base_us;
uint32_t last_data_time_ms;
Vector3f position_zero;
Vector3d position_zero;
Vector3f accel_earth;
float throttle_sent = -1;
bool connected = false;