SITL: convert to double precision for positions

libraries/SITL/SIM_AirSim.cpp

@@ -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);
 

libraries/SITL/SIM_Aircraft.cpp

@@ -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]));
     }

libraries/SITL/SIM_Aircraft.h

@@ -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;

libraries/SITL/SIM_BalanceBot.cpp

@@ -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);

libraries/SITL/SIM_FlightAxis.cpp

@@ -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);
 

libraries/SITL/SIM_FlightAxis.h

@@ -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";

libraries/SITL/SIM_Gazebo.cpp

@@ -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]),
+    position = Vector3d(pkt.position_xyz[0],
-                        static_cast<float>(pkt.position_xyz[1]),
+                        pkt.position_xyz[1],
-                        static_cast<float>(pkt.position_xyz[2]));
+                        pkt.position_xyz[2]);
 
 
     // auto-adjust to simulation frame rate

libraries/SITL/SIM_JSON.cpp

@@ -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) {

libraries/SITL/SIM_JSON.h

@@ -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 },

libraries/SITL/SIM_Morse.cpp

@@ -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],

libraries/SITL/SIM_Precland.cpp

@@ -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;
 }
 

libraries/SITL/SIM_Precland.h

@@ -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;
 };
 
 }

libraries/SITL/SIM_Rover.cpp

@@ -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);
 

libraries/SITL/SIM_Sailboat.cpp

@@ -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();

libraries/SITL/SIM_Submarine.cpp

@@ -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;
 }

libraries/SITL/SIM_Submarine.h

@@ -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

libraries/SITL/SIM_Vicon.cpp

@@ -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;

libraries/SITL/SIM_Vicon.h

@@ -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
 };
 
 }

libraries/SITL/SIM_Webots.cpp

@@ -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

libraries/SITL/SIM_XPlane.cpp

@@ -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();
 

libraries/SITL/SIM_XPlane.h

@@ -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;