set Home point

AirLib/include/api/RpcLibClient.hpp

@@ -64,7 +64,7 @@ class RpcLibClient {
     void confirmConnection();
     DroneControllerBase::LandedState getLandedState();
     TTimePoint timestampNow();
-    GeoPoint getHomePoint();
+    GeoPoint getHomeGeoPoint();
 
     bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
         float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);

AirLib/include/controllers/DroneControllerBase.hpp

@@ -92,7 +92,7 @@ class DroneControllerBase : public VehicleControllerBase {
     /// Set arm to false to disarm the drone.  This will disable the motors, so don't do that
     /// unless the drone is on the ground!  Arming the drone also sets the "home position"
     /// This home position is local position x=0,y=0,z=0.  You can also query what GPS location
-    /// that is via getHomePoint.  
+    /// that is via getHomeGeoPoint.  
     virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) = 0;
 
     /// When armed you can tell the drone to takeoff.  This will fly to a preset altitude (like 2.5 meters)
@@ -222,7 +222,7 @@ class DroneControllerBase : public VehicleControllerBase {
 
     /// Get the home point (where drone was armed before takeoff).  This is the location the drone 
     /// will return to if you call goHome().
-    virtual GeoPoint getHomePoint() = 0;
+    virtual GeoPoint getHomeGeoPoint() = 0;
 
     /// Get the current GPS location of the drone.
     virtual GeoPoint getGpsLocation() = 0;

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -221,9 +221,9 @@ class DroneControllerCancelable  {
     {
         return controller_->clock()->nowNanos();
     }
-    GeoPoint getHomePoint()
+    GeoPoint getHomeGeoPoint()
     {
-        return controller_->getHomePoint();
+        return controller_->getHomeGeoPoint();
     }
 
     //TODO: add GPS health, accuracy in API

AirLib/include/controllers/MavLinkDroneController.hpp

@@ -132,7 +132,7 @@ class MavLinkDroneController : public DroneControllerBase
     bool land(float max_wait_seconds, CancelableBase& cancelable_action) override;
     bool goHome(CancelableBase& cancelable_action) override; 
     bool hover(CancelableBase& cancelable_action) override;
-    GeoPoint getHomePoint() override;
+    GeoPoint getHomeGeoPoint() override;
     GeoPoint getGpsLocation() override;
     virtual void reportTelemetry(float renderTime) override;
 
@@ -927,7 +927,7 @@ struct MavLinkDroneController::impl {
         return Vector3r(current_state.local_est.vel.vx, current_state.local_est.vel.vy, current_state.local_est.vel.vz);
     }
 
-    GeoPoint getHomePoint()
+    GeoPoint getHomeGeoPoint()
     {
         updateState();
         if (current_state.home.is_set)
@@ -1340,9 +1340,9 @@ Vector3r MavLinkDroneController::getVelocity()
     return pimpl_->getVelocity();
 }
 
-GeoPoint MavLinkDroneController::getHomePoint()
+GeoPoint MavLinkDroneController::getHomeGeoPoint()
 {
-    return pimpl_->getHomePoint();
+    return pimpl_->getHomeGeoPoint();
 }
 
 GeoPoint MavLinkDroneController::getGpsLocation()

AirLib/include/firmwares/ros_flight/RosFlightDroneController.hpp

@@ -211,7 +211,7 @@ class RosFlightDroneController : public DroneControllerBase {
         return true;
     }
 
-    GeoPoint getHomePoint() override
+    GeoPoint getHomeGeoPoint() override
     {
         return environment_->getInitialState().geo_point;
     }

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp

@@ -12,22 +12,22 @@ namespace msr { namespace airlib {
 
 class AirSimSimpleFlightCommon {
 public:
-    static simple_flight::Axis3r vector3rToAxis3r(const Vector3r& vec)
+    static simple_flight::Axis3r toAxis3r(const Vector3r& vec)
     {
         simple_flight::Axis3r conv;
         conv.x() = vec.x(); conv.y() = vec.y(); conv.z() = vec.z();
 
         return conv;
     }
 
-    static Vector3r axis3rToVector3r(const simple_flight::Axis3r& vec)
+    static Vector3r toVector3r(const simple_flight::Axis3r& vec)
     {
         Vector3r conv;
         conv.x() = vec.x(); conv.y() = vec.y(); conv.z() = vec.z();
         return conv;
     }
 
-    static simple_flight::Axis4r quaternion3rToAxis4r(const Quaternionr& q)
+    static simple_flight::Axis4r toAxis4r(const Quaternionr& q)
     {
         simple_flight::Axis4r conv;
         conv.axis3.x() = q.x(); conv.axis3.y() = q.y(); conv.axis3.z() = q.z();
@@ -36,13 +36,33 @@ class AirSimSimpleFlightCommon {
         return conv;
     }
 
-    static Quaternionr axis4rToQuaternionr(const simple_flight::Axis4r& q)
+    static Quaternionr toQuaternion(const simple_flight::Axis4r& q)
     {
         Quaternionr conv;
         conv.x() = q.axis3.x(); conv.y() = q.axis3.y(); conv.z() = q.axis3.z();
         conv.w() = q.val4;
         return conv;
     }
+
+    static simple_flight::GeoPoint toSimpleFlightGeoPoint(const GeoPoint& geo_point)
+    {
+        simple_flight::GeoPoint conv;
+        conv.latitude = geo_point.latitude;
+        conv.longitude = geo_point.longitude;
+        conv.altiude = geo_point.altitude;
+
+        return conv;
+    }
+
+    static GeoPoint toGeoPoint(const simple_flight::GeoPoint& geo_point)
+    {
+        GeoPoint conv;
+        conv.latitude = geo_point.latitude;
+        conv.longitude = geo_point.longitude;
+        conv.altitude = geo_point.altiude;
+
+        return conv;
+    }
 };
 
 

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp

@@ -16,7 +16,7 @@ namespace msr { namespace airlib {
 class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator {
 public:
     //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors)
-    void setKinematics(const Kinematics::State* kinematics, const Environment* environment)
+    void setKinematics(const Kinematics::State* kinematics, Environment* environment)
     {
         kinematics_ = kinematics;
         environment_ = environment;
@@ -45,40 +45,44 @@ class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator {
 
     virtual simple_flight::Axis3r getPosition() const override
     {
-        return AirSimSimpleFlightCommon::vector3rToAxis3r(kinematics_->pose.position);
+        return AirSimSimpleFlightCommon::toAxis3r(kinematics_->pose.position);
     }
 
     virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const override
     {
-        const Vector3r& vec = AirSimSimpleFlightCommon::axis3rToVector3r(world_frame_val);
+        const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val);
         const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation);
-        return AirSimSimpleFlightCommon::vector3rToAxis3r(trans);
+        return AirSimSimpleFlightCommon::toAxis3r(trans);
     }
 
     virtual simple_flight::Axis3r getLinearVelocity() const override
     {
-        return AirSimSimpleFlightCommon::vector3rToAxis3r(kinematics_->twist.linear);
+        return AirSimSimpleFlightCommon::toAxis3r(kinematics_->twist.linear);
     }
 
     virtual simple_flight::Axis4r getOrientation() const override
     {
-        return AirSimSimpleFlightCommon::quaternion3rToAxis4r(kinematics_->pose.orientation);
+        return AirSimSimpleFlightCommon::toAxis4r(kinematics_->pose.orientation);
     }
 
     virtual simple_flight::GeoPoint getGeoPoint() const override
     {
-        const auto& geo_point = environment_->getState().geo_point;
-        simple_flight::GeoPoint conv;
-        conv.latitude = geo_point.latitude;
-        conv.longitude = geo_point.longitude;
-        conv.altiude = geo_point.altitude;
+        return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point);
+    }
 
-        return conv;
+    virtual void setHomeGeoPoint(const simple_flight::GeoPoint& geo_point) override
+    {
+        environment_->setHomeGeoPoint(AirSimSimpleFlightCommon::toGeoPoint(geo_point));
+    }
+
+    virtual simple_flight::GeoPoint getHomeGeoPoint() const override
+    {
+        return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint());
     }
 
 private:
     const Kinematics::State* kinematics_;
-    const Environment* environment_;
+    Environment* environment_;
 };
 
 

AirLib/include/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -124,19 +124,19 @@ class SimpleFlightDroneController : public DroneControllerBase {
     Vector3r getPosition() override
     {
         const auto& val = firmware_->offboardApi().getStateEstimator().getPosition();
-        return AirSimSimpleFlightCommon::axis3rToVector3r(val);
+        return AirSimSimpleFlightCommon::toVector3r(val);
     }
 
     Vector3r getVelocity() override
     {
         const auto& val = firmware_->offboardApi().getStateEstimator().getLinearVelocity();
-        return AirSimSimpleFlightCommon::axis3rToVector3r(val);
+        return AirSimSimpleFlightCommon::toVector3r(val);
     }
 
     Quaternionr getOrientation() override
     {
         const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation();
-        return AirSimSimpleFlightCommon::axis4rToQuaternionr(val);    
+        return AirSimSimpleFlightCommon::toQuaternion(val);    
     }
 
     LandedState getLandedState() override
@@ -211,14 +211,14 @@ class SimpleFlightDroneController : public DroneControllerBase {
         return true;
     }
 
-    GeoPoint getHomePoint() override
+    GeoPoint getHomeGeoPoint() override
     {
-        return physics_body_->getEnvironment().getInitialState().geo_point;
+        return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint());
     }
 
     GeoPoint getGpsLocation() override
     {
-        return physics_body_->getEnvironment().getState().geo_point;
+        return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint());
     }
 
     virtual void reportTelemetry(float renderTime) override

AirLib/include/firmwares/simple_flight/firmware/OffboardApi.hpp

@@ -16,7 +16,7 @@ class OffboardApi :
     public IOffboardApi {
 public:
     OffboardApi(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs, 
-        const IStateEstimator* state_estimator, ICommLink* comm_link)
+        IStateEstimator* state_estimator, ICommLink* comm_link)
         : params_(params), rc_(params, clock, board_inputs, &vehicle_state_, state_estimator, comm_link), 
           state_estimator_(state_estimator), comm_link_(comm_link)
     {
@@ -109,7 +109,8 @@ class OffboardApi :
             || vehicle_state_.getState() == VehicleStateType::Disarmed
             || vehicle_state_.getState() == VehicleStateType::BeingDisarmed)) {
 
-            vehicle_state_.setState(VehicleStateType::Armed, state_estimator_->getGeoPoint());
+            state_estimator_->setHomeGeoPoint(state_estimator_->getGeoPoint());
+            vehicle_state_.setState(VehicleStateType::Armed, state_estimator_->getHomeGeoPoint());
             goal_ = goal;
             goal_mode_ = goal_mode;
 
@@ -155,11 +156,17 @@ class OffboardApi :
         return *state_estimator_;
     }
 
-    virtual GeoPoint getHomePoint() const override
+    virtual GeoPoint getHomeGeoPoint() const override
     {
-        return vehicle_state_.getHomePoint();
+        return state_estimator_->getHomeGeoPoint();
     }
 
+    virtual GeoPoint getGeoPoint() const override
+    {
+        return state_estimator_->getGeoPoint();
+    }
+
+
 private:
     void updateGoalFromRc()
     {
@@ -170,7 +177,7 @@ class OffboardApi :
 private:
     const Params* params_;
     RemoteControl rc_;
-    const IStateEstimator* state_estimator_;
+    IStateEstimator* state_estimator_;
     ICommLink* comm_link_;
 
     VehicleState vehicle_state_;

AirLib/include/firmwares/simple_flight/firmware/RemoteControl.hpp

@@ -14,7 +14,7 @@ class RemoteControl :
     public IUpdatable {
 public:
     RemoteControl(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs,
-        VehicleState* vehicle_state, const IStateEstimator* state_estimator, ICommLink* comm_link)
+        VehicleState* vehicle_state, IStateEstimator* state_estimator, ICommLink* comm_link)
         : params_(params), clock_(clock), board_inputs_(board_inputs), 
         vehicle_state_(vehicle_state), state_estimator_(state_estimator), comm_link_(comm_link)
     {
@@ -85,7 +85,8 @@ class RemoteControl :
                 request_duration_ += dt;
 
                 if (request_duration_ > params_->rc.neutral_duration) {
-                    vehicle_state_->setState(VehicleStateType::Armed, state_estimator_->getGeoPoint());
+                    state_estimator_->setHomeGeoPoint(state_estimator_->getGeoPoint());
+                    vehicle_state_->setState(VehicleStateType::Armed, state_estimator_->getHomeGeoPoint());
                     comm_link_->log(std::string("State:\t ").append("Armed"));
                     request_duration_ = 0;
                 }
@@ -229,7 +230,7 @@ class RemoteControl :
     const IBoardClock* clock_;
     const IBoardInputPins* board_inputs_;
     VehicleState* vehicle_state_;
-    const IStateEstimator* state_estimator_;
+    IStateEstimator* state_estimator_;
     ICommLink* comm_link_;
 
     Axis4r goal_;

AirLib/include/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp

@@ -125,7 +125,7 @@ class VehicleState {
         state_ = state;
     }
 
-    const GeoPoint& getHomePoint() const
+    const GeoPoint& getHomeGeoPoint() const
     {
         return home_point_;
     }

AirLib/include/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp

@@ -21,7 +21,8 @@ class IOffboardApi : public IGoal {
     virtual VehicleStateType getVehicleState() const = 0;
 
     virtual const IStateEstimator& getStateEstimator() = 0;
-    virtual GeoPoint getHomePoint() const = 0;
+    virtual GeoPoint getHomeGeoPoint() const = 0;
+    virtual GeoPoint getGeoPoint() const = 0;
 };
 
 } //namespace

AirLib/include/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp

@@ -13,6 +13,9 @@ class IStateEstimator {
     virtual Axis4r getOrientation() const = 0;
     virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0;
     virtual GeoPoint getGeoPoint() const = 0;
+
+    virtual void setHomeGeoPoint(const GeoPoint& geo_point) = 0;
+    virtual GeoPoint getHomeGeoPoint() const = 0;
 };
 
 }

AirLib/include/physics/Environment.hpp

@@ -45,11 +45,21 @@ class Environment : public UpdatableObject {
     {
         initial_ = initial;
 
-        home_geo_point_ = EarthUtils::HomeGeoPoint(initial_.geo_point);
+        setHomeGeoPoint(initial_.geo_point);
 
         updateState(initial_, home_geo_point_);
     }
 
+    void setHomeGeoPoint(const GeoPoint& home_point)
+    {
+        home_geo_point_ = EarthUtils::HomeGeoPoint(home_point);
+    }
+
+    GeoPoint getHomeGeoPoint() const
+    {
+        return home_geo_point_.home_point;
+    }
+
     //in local NED coordinates
     void setPosition(const Vector3r& position)
     {

AirLib/include/physics/PhysicsBody.hpp

@@ -190,6 +190,10 @@ class PhysicsBody : public UpdatableObject {
     {
         return *environment_;
     }
+    Environment& getEnvironment()
+    {
+        return *environment_;
+    }
     bool hasEnvironment() const
     {
         return environment_ != nullptr;

AirLib/src/api/RpcLibClient.cpp

@@ -213,9 +213,9 @@ TTimePoint RpcLibClient::timestampNow()
     return pimpl_->client.call("timestampNow").as<TTimePoint>();
 }
 
-GeoPoint RpcLibClient::getHomePoint()
+GeoPoint RpcLibClient::getHomeGeoPoint()
 {
-    return pimpl_->client.call("getHomePoint").as<RpcLibAdapators::GeoPoint>().to();
+    return pimpl_->client.call("getHomeGeoPoint").as<RpcLibAdapators::GeoPoint>().to();
 }
 
 GeoPoint RpcLibClient::getGpsLocation()