require reset() as first call, checks for reset(), update() seq

ppueyor · Aug 1, 2017 · d7cd82b · d7cd82b
1 parent 0397979
commit d7cd82b
Show file tree

Hide file tree

Showing 49 changed files with 845 additions and 651 deletions.
diff --git a/AirLib/include/common/DebugClock.hpp b/AirLib/include/common/DebugClock.hpp
@@ -6,6 +6,7 @@
 
 #include "ClockBase.hpp"
 #include "Common.hpp"
+#include <atomic>
 
 namespace msr { namespace airlib {
 
@@ -52,7 +53,7 @@ class DebugClock : public ClockBase {
 
 
 private:
-    TTimePoint current_;
+    std::atomic<TTimePoint> current_;
     TTimeDelta step_;
 };
 

diff --git a/AirLib/include/common/DelayLine.hpp b/AirLib/include/common/DelayLine.hpp
@@ -22,7 +22,6 @@ class DelayLine : UpdatableObject {
     void initialize(TTimeDelta delay)  //in seconds
     {
         setDelay(delay);
-        DelayLine::reset();
     }
     void setDelay(TTimeDelta delay)
     {
@@ -36,6 +35,8 @@ class DelayLine : UpdatableObject {
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         values_.clear();
         times_.clear();
         last_time_ = 0;
@@ -44,6 +45,8 @@ class DelayLine : UpdatableObject {
 
     virtual void update() override
     {
+        UpdatableObject::update();
+
         if (!times_.empty() && 
             ClockBase::elapsedBetween(clock()->nowNanos(), times_.front()) >= delay_) {
 

diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp
@@ -142,6 +142,9 @@ class EarthUtils {
         //Below 51km: Practical Meteorology by Roland Stull, pg 12
         //Above 51km: http://www.braeunig.us/space/atmmodel.htm
         //Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf
+
+        //TODO: handle -ve altitude better (shouldn't grow indefinitely!)
+
         if (geopot_height <= 11)
             //at alt 0, return sea level pressure
             return  SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f);

diff --git a/AirLib/include/common/FirstOrderFilter.hpp b/AirLib/include/common/FirstOrderFilter.hpp
@@ -39,19 +39,22 @@ class FirstOrderFilter : UpdatableObject {
         timeConstant_ = timeConstant;
         initial_input_ = initial_input;
         initial_output_ = initial_output;
-        FirstOrderFilter::reset();
     }
 
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         last_time_ = clock()->nowNanos();
         input_ = initial_input_;
         output_ = initial_output_;
     }
 
     virtual void update() override
     {
+        UpdatableObject::update();
+
         TTimeDelta dt = clock()->updateSince(last_time_);
 
         //lower the weight for previous value if its been long time

diff --git a/AirLib/include/common/FrequencyLimiter.hpp b/AirLib/include/common/FrequencyLimiter.hpp
@@ -21,13 +21,13 @@ class FrequencyLimiter : UpdatableObject {
     {
         frequency_ = frequency;
         startup_delay_ = startup_delay;
-        reset();
-
     }
 
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         last_time_ = clock()->nowNanos();
         first_time_ = last_time_;
 
@@ -46,6 +46,8 @@ class FrequencyLimiter : UpdatableObject {
 
     virtual void update() override
     {
+        UpdatableObject::update();
+
         elapsed_total_sec_ = clock()->elapsedSince(first_time_);
         elapsed_interval_sec_ = clock()->elapsedSince(last_time_);
         ++update_count_;

diff --git a/AirLib/include/common/GaussianMarkov.hpp b/AirLib/include/common/GaussianMarkov.hpp
@@ -29,13 +29,13 @@ class GaussianMarkov : UpdatableObject {
             initial_output_ = getNextRandom() * sigma_;
         else
             initial_output_ = initial_output;
-
-        GaussianMarkov::reset();
     }
 
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         last_time_ = clock()->nowNanos();
         output_ = initial_output_;
         rand_.reset();
@@ -52,6 +52,8 @@ class GaussianMarkov : UpdatableObject {
             John H Wall, 2007, eq 2.5, pg 13, http://etd.auburn.edu/handle/10415/945
         */
 
+        UpdatableObject::update();
+
         TTimeDelta dt = clock()->updateSince(last_time_);
 
         double alpha = exp(-dt / tau_);

diff --git a/AirLib/include/common/StateReporterWrapper.hpp b/AirLib/include/common/StateReporterWrapper.hpp
@@ -28,7 +28,6 @@ class StateReporterWrapper : public UpdatableObject {
         enabled_ = enabled;
         report_.initialize(float_precision, is_scientific_notation);
         report_freq_.initialize(DefaultReportFreq);
-        StateReporterWrapper::reset();
     }
 
     void clearReport()
@@ -40,6 +39,8 @@ class StateReporterWrapper : public UpdatableObject {
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         last_time_ = clock()->nowNanos();
         clearReport();
         dt_stats_.clear();
@@ -48,6 +49,8 @@ class StateReporterWrapper : public UpdatableObject {
 
     virtual void update() override
     {
+        UpdatableObject::update();
+
         TTimeDelta dt = clock()->updateSince(last_time_);
 
         if (enabled_) {

diff --git a/AirLib/include/common/UpdatableContainer.hpp b/AirLib/include/common/UpdatableContainer.hpp
@@ -33,12 +33,16 @@ class UpdatableContainer : public UpdatableObject {
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
+        UpdatableObject::reset();
+
         for (TUpdatableObjectPtr& member : members_)
             member->reset();
     }
 
     virtual void update() override
     {
+        UpdatableObject::update();
+
         for (TUpdatableObjectPtr& member : members_)
             member->update();
     }

diff --git a/AirLib/include/common/UpdatableObject.hpp b/AirLib/include/common/UpdatableObject.hpp
@@ -12,8 +12,20 @@ namespace msr { namespace airlib {
 
 class UpdatableObject {
 public:
-    virtual void reset() = 0;
-    virtual void update() = 0;
+    virtual void reset()
+    {
+        if (reset_called && !update_called)
+            throw std::runtime_error("Multiple reset() calls detected without call to update()");
+
+        reset_called = true;
+    }
+    virtual void update()
+    {
+        if (!reset_called)
+            throw std::runtime_error("reset() must be called first before update()");
+        update_called = true;
+    }
+
     virtual ~UpdatableObject() = default;
 
     virtual void reportState(StateReporter& reporter)
@@ -31,6 +43,10 @@ class UpdatableObject {
     {
         return ClockFactory::get();
     }
+
+private:
+    bool reset_called = false;
+    bool update_called = false;
 };
 
 }} //namespace

diff --git a/AirLib/include/controllers/ControllerBase.hpp b/AirLib/include/controllers/ControllerBase.hpp
@@ -11,7 +11,7 @@
 namespace msr { namespace airlib {
 
 /*
-    This class defined the interface for vehicle controllers. A typical vehicle controller would consume
+    This class defines the interface for vehicle controllers. A typical vehicle controller would consume
     sensor values as inputs and return control signals for various vertices on physics body as output. The update()
     method should be used to process the sensor inputs from vehicle as needed (the implementor should have vehicle object
     possibly through initialization). The getVertexControlSignal() is called to retrieve the value of control signal to be applied
@@ -20,10 +20,6 @@ namespace msr { namespace airlib {
 */
 class ControllerBase : public UpdatableObject {
 public:
-    //reset any state in the controller
-    virtual void reset() override = 0;
-    virtual void update() override = 0;
-
     //return 0 to 1 (corresponds to zero to full thrust)
     virtual real_T getVertexControlSignal(unsigned int rotor_index) = 0;
     virtual size_t getVertexCount() = 0;

diff --git a/AirLib/include/controllers/MavLinkDroneController.hpp b/AirLib/include/controllers/MavLinkDroneController.hpp
@@ -1297,10 +1297,12 @@ std::string MavLinkDroneController::findPixhawk()
 //*** Start: VehicleControllerBase implementation ***//
 void MavLinkDroneController::reset()
 {
+    DroneControllerBase::reset();
     pimpl_->reset();
 }
 void MavLinkDroneController::update()
 {
+    DroneControllerBase::update();
     pimpl_->update();
 }
 real_T MavLinkDroneController::getVertexControlSignal(unsigned int rotor_index)

diff --git a/AirLib/include/controllers/MotorDirectController.hpp b/AirLib/include/controllers/MotorDirectController.hpp
@@ -25,20 +25,23 @@ class MotorDirectController : public ControllerBase {
     {
         params_ = params;
         motor_control_signals_.resize(params_.rotor_count);
-        MotorDirectController::reset();
     }
 
 
     //*** Start ControllerBase implementation ****//
     virtual void reset() override
     {
+        ControllerBase::reset();
+
         motor_control_signals_.assign(params_.rotor_count, 0);
     }
 
-	virtual void update() override
-	{
-		//nothing to update in direct motor control
-	}
+    virtual void update() override
+    {
+        ControllerBase::update();
+
+        //nothing to update in direct motor control
+    }
 
     virtual real_T getVertexControlSignal(unsigned int rotor_index) override
     {

diff --git a/AirLib/include/controllers/RpyDirectController.hpp b/AirLib/include/controllers/RpyDirectController.hpp
@@ -24,27 +24,30 @@ class RpyDirectController : public ControllerBase {
 
         if (params_.rotor_count != 4)
             throw std::invalid_argument(Utils::stringf("Rotor count of %d is not supported yet", params_.rotor_count));
-        RpyDirectController::reset();
     }
 
     //*** Start ControllerBase implementation ****//
-	virtual void reset() override
-	{
+    virtual void reset() override
+    {
+        ControllerBase::reset();
+
         motor_control_signals_.assign(params_.rotor_count, 0);
-	}
+    }
+
+    virtual void update() override
+    {
+        ControllerBase::update();
 
-	virtual void update() override
-	{
-		real_T throttle_speed = scale(throttle_, 1.0f / params_.throttle_scale);
-		real_T roll_speed = scale(roll_, throttle_speed / params_.roll_scale);
-		real_T pitch_speed = scale(pitch_, throttle_speed / params_.pitch_scale);
-		real_T yaw_speed = scale(yaw_, throttle_speed / params_.yaw_scale);
+        real_T throttle_speed = scale(throttle_, 1.0f / params_.throttle_scale);
+        real_T roll_speed = scale(roll_, throttle_speed / params_.roll_scale);
+        real_T pitch_speed = scale(pitch_, throttle_speed / params_.pitch_scale);
+        real_T yaw_speed = scale(yaw_, throttle_speed / params_.yaw_scale);
 
         motor_control_signals_[0] = Utils::clip(throttle_speed - pitch_speed + roll_speed + yaw_speed, 0.0f, 1.0f);
         motor_control_signals_[1] = Utils::clip(throttle_speed + pitch_speed + roll_speed - yaw_speed, 0.0f, 1.0f);
         motor_control_signals_[2] = Utils::clip(throttle_speed + pitch_speed - roll_speed + yaw_speed, 0.0f, 1.0f);
         motor_control_signals_[3] = Utils::clip(throttle_speed - pitch_speed - roll_speed - yaw_speed, 0.0f, 1.0f);
-	}
+    }
 
     virtual real_T getVertexControlSignal(unsigned int rotor_index) override
     {
@@ -66,11 +69,11 @@ class RpyDirectController : public ControllerBase {
     virtual ~RpyDirectController() = default;
 
 private:
-	//input between -1 to 1
-	real_T scale(real_T input, real_T factor)
-	{
-		return input * factor;
-	}
+    //input between -1 to 1
+    real_T scale(real_T input, real_T factor)
+    {
+        return input * factor;
+    }
 
 private:
     real_T roll_ = 0, pitch_ = 0, yaw_ = 0, throttle_ = 0;

diff --git a/AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp b/AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp
@@ -48,20 +48,26 @@ class RosFlightDroneController : public DroneControllerBase {
     //*** Start: VehicleControllerBase implementation ***//
     virtual void reset() override
     {
+        DroneControllerBase::reset();
+
         board_->system_reset(false);
     }
 
     virtual void update() override
     {
+        DroneControllerBase::update();
+
         board_->notifySensorUpdated(ros_flight::Board::SensorType::Imu);
         firmware_->loop();
     }
 
     virtual void start() override
     {
+        DroneControllerBase::start();
     }
     virtual void stop() override
     {
+        DroneControllerBase::stop();
     }
 
     virtual size_t getVertexCount() override

diff --git a/AirLib/include/controllers/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/controllers/simple_flight/AirSimSimpleFlightBoard.hpp
@@ -90,12 +90,16 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
 
     virtual void reset() override 
     {
+        IBoard::reset();
+
         motor_output_.assign(params_->motor_count, 0);
         input_channels_.assign(params_->rc_channel_count, 0);
     }
 
     virtual void update() override
     {
+        IBoard::update();
+
         //no op for now
     }
 

diff --git a/AirLib/include/controllers/simple_flight/AirSimSimpleFlightCommLink.hpp b/AirLib/include/controllers/simple_flight/AirSimSimpleFlightCommLink.hpp
@@ -25,11 +25,14 @@ class AirSimSimpleFlightCommLink : public simple_flight::ICommLink {
 public: // implement CommLink interface
     virtual void reset()
     {
+        simple_flight::ICommLink::reset();
+
         messages_.clear();
     }
 
     virtual void update()
     {
+        simple_flight::ICommLink::update();
     }
 
     virtual void log(const std::string& message, int32_t log_level)