From d7cd82bd74d2547d1b470235164f20ca992cdbe8 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 1 Aug 2017 02:55:16 -0700 Subject: [PATCH] require reset() as first call, checks for reset(), update() seq --- AirLib/include/common/DebugClock.hpp | 3 +- AirLib/include/common/DelayLine.hpp | 5 +- AirLib/include/common/EarthUtils.hpp | 3 + AirLib/include/common/FirstOrderFilter.hpp | 5 +- AirLib/include/common/FrequencyLimiter.hpp | 6 +- AirLib/include/common/GaussianMarkov.hpp | 6 +- .../include/common/StateReporterWrapper.hpp | 5 +- AirLib/include/common/UpdatableContainer.hpp | 4 + AirLib/include/common/UpdatableObject.hpp | 20 +- AirLib/include/controllers/ControllerBase.hpp | 6 +- .../controllers/MavLinkDroneController.hpp | 2 + .../controllers/MotorDirectController.hpp | 13 +- .../controllers/RpyDirectController.hpp | 37 +- .../ros_flight/RosFlightDroneController.hpp | 6 + .../simple_flight/AirSimSimpleFlightBoard.hpp | 4 + .../AirSimSimpleFlightCommLink.hpp | 3 + .../SimpleFlightDroneController.hpp | 7 +- .../firmware/AngleLevelController.hpp | 6 + .../firmware/AngleRateController.hpp | 4 + .../firmware/CascadeController.hpp | 5 + .../simple_flight/firmware/PidController.hpp | 4 + .../simple_flight/firmware/RemoteControl.hpp | 4 + .../firmware/interfaces/IAxisController.hpp | 2 - .../firmware/interfaces/IController.hpp | 2 - .../firmware/interfaces/IUpdatable.hpp | 22 +- AirLib/include/physics/Environment.hpp | 2 - AirLib/include/physics/FastPhysicsEngine.hpp | 8 +- AirLib/include/physics/Kinematics.hpp | 5 +- AirLib/include/physics/PhysicsBody.hpp | 20 +- AirLib/include/physics/PhysicsBodyVertex.hpp | 6 +- AirLib/include/physics/PhysicsEngineBase.hpp | 38 +- AirLib/include/physics/World.hpp | 14 +- AirLib/include/sensors/SensorBase.hpp | 2 - AirLib/include/sensors/SensorCollection.hpp | 4 + .../sensors/barometer/BarometerSimple.hpp | 4 + AirLib/include/sensors/gps/GpsSimple.hpp | 4 + AirLib/include/sensors/imu/ImuSimple.hpp | 4 + .../magnetometer/MagnetometerSimple.hpp | 4 + AirLib/include/vehicles/MultiRotor.hpp | 14 +- AirLib/include/vehicles/Rotor.hpp | 6 +- AirLibUnitTests/QuaternionTest.hpp | 2 +- DroneServer/main.cpp | 1 + Examples/StandAlonePhysics.hpp | 3 +- Examples/StandAloneSensors.hpp | 15 + MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp | 1108 ++++++++--------- .../AirSim/Source/MultiRotorConnector.cpp | 7 + .../Source/SimMode/SimModeWorldBase.cpp | 8 +- .../Plugins/AirSim/Source/VehiclePawnBase.cpp | 2 +- clean.cmd | 3 + 49 files changed, 831 insertions(+), 637 deletions(-) diff --git a/AirLib/include/common/DebugClock.hpp b/AirLib/include/common/DebugClock.hpp index 163542e0..2e462ce0 100644 --- a/AirLib/include/common/DebugClock.hpp +++ b/AirLib/include/common/DebugClock.hpp @@ -6,6 +6,7 @@ #include "ClockBase.hpp" #include "Common.hpp" +#include namespace msr { namespace airlib { @@ -52,7 +53,7 @@ public: private: - TTimePoint current_; + std::atomic current_; TTimeDelta step_; }; diff --git a/AirLib/include/common/DelayLine.hpp b/AirLib/include/common/DelayLine.hpp index ee4b84bb..1c7022f6 100644 --- a/AirLib/include/common/DelayLine.hpp +++ b/AirLib/include/common/DelayLine.hpp @@ -22,7 +22,6 @@ public: void initialize(TTimeDelta delay) //in seconds { setDelay(delay); - DelayLine::reset(); } void setDelay(TTimeDelta delay) { @@ -36,6 +35,8 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + values_.clear(); times_.clear(); last_time_ = 0; @@ -44,6 +45,8 @@ public: 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 index e4123fec..b7d866d6 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -142,6 +142,9 @@ public: //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 index ae826b87..45bc8854 100644 --- a/AirLib/include/common/FirstOrderFilter.hpp +++ b/AirLib/include/common/FirstOrderFilter.hpp @@ -39,12 +39,13 @@ public: 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_; @@ -52,6 +53,8 @@ public: 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 index 84c4b001..2719ee79 100644 --- a/AirLib/include/common/FrequencyLimiter.hpp +++ b/AirLib/include/common/FrequencyLimiter.hpp @@ -21,13 +21,13 @@ public: { 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 @@ public: 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 index 5645ee4b..e6d44dc2 100644 --- a/AirLib/include/common/GaussianMarkov.hpp +++ b/AirLib/include/common/GaussianMarkov.hpp @@ -29,13 +29,13 @@ public: 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 @@ public: 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 index 35b8d192..13244e73 100644 --- a/AirLib/include/common/StateReporterWrapper.hpp +++ b/AirLib/include/common/StateReporterWrapper.hpp @@ -28,7 +28,6 @@ public: enabled_ = enabled; report_.initialize(float_precision, is_scientific_notation); report_freq_.initialize(DefaultReportFreq); - StateReporterWrapper::reset(); } void clearReport() @@ -40,6 +39,8 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + last_time_ = clock()->nowNanos(); clearReport(); dt_stats_.clear(); @@ -48,6 +49,8 @@ public: 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 index 13b0a4c9..22cfb1f8 100644 --- a/AirLib/include/common/UpdatableContainer.hpp +++ b/AirLib/include/common/UpdatableContainer.hpp @@ -33,12 +33,16 @@ public: //*** 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 index a05b1ad8..cdb86cda 100644 --- 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 @@ public: { 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 index cf89db11..af93fd56 100644 --- 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 index f8e4251e..a064e0a8 100644 --- 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 index 1bb508d6..10114c47 100644 --- a/AirLib/include/controllers/MotorDirectController.hpp +++ b/AirLib/include/controllers/MotorDirectController.hpp @@ -25,20 +25,23 @@ public: { 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 index 6d8a9aba..ef9a7b99 100644 --- a/AirLib/include/controllers/RpyDirectController.hpp +++ b/AirLib/include/controllers/RpyDirectController.hpp @@ -24,27 +24,30 @@ public: 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 - { - motor_control_signals_.assign(params_.rotor_count, 0); - } + virtual void reset() override + { + ControllerBase::reset(); - 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); + motor_control_signals_.assign(params_.rotor_count, 0); + } + + virtual void update() override + { + ControllerBase::update(); + + 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 @@ public: 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 index 34140d88..fb923e2f 100644 --- a/AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp +++ b/AirLib/include/controllers/ros_flight/RosFlightDroneController.hpp @@ -48,20 +48,26 @@ public: //*** 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 index 62d28a56..fdab3771 100644 --- a/AirLib/include/controllers/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/controllers/simple_flight/AirSimSimpleFlightBoard.hpp @@ -90,12 +90,16 @@ public: 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 index 5ed21cc2..940f4ebf 100644 --- a/AirLib/include/controllers/simple_flight/AirSimSimpleFlightCommLink.hpp +++ b/AirLib/include/controllers/simple_flight/AirSimSimpleFlightCommLink.hpp @@ -25,11 +25,14 @@ public: // derived class specific methods 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) diff --git a/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp index e694cc13..a8f7d122 100644 --- a/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp @@ -32,7 +32,6 @@ public: //create firmware firmware_.reset(new simple_flight::Firmware(board_.get(), comm_link_.get(), estimator_.get(), ¶ms_)); - firmware_->reset(); //find out which RC we should use Settings child; @@ -52,20 +51,26 @@ public: //*** Start: VehicleControllerBase implementation ***// virtual void reset() override { + DroneControllerBase::reset(); + firmware_->reset(); } virtual void update() override { + DroneControllerBase::update(); + firmware_->update(); } 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/firmware/AngleLevelController.hpp b/AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp index cfc48406..e1f68657 100644 --- a/AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp @@ -44,6 +44,9 @@ public: virtual void reset() override { + IAxisController::reset(); + IGoalInput::reset(); + pid_->reset(); rate_controller_->reset(); rate_goal_ = Axis4r(); @@ -52,6 +55,9 @@ public: virtual void update() override { + IAxisController::update(); + IGoalInput::update(); + //get response of level PID const auto& level_goal = goal_input_->getGoal(); pid_->setGoal(level_goal.axis3[axis_]); diff --git a/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp b/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp index fc0ad5b3..185f0b0b 100644 --- a/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp @@ -30,12 +30,16 @@ public: virtual void reset() override { + IAxisController::reset(); + pid_->reset(); output_ = TReal(); } virtual void update() override { + IAxisController::update(); + pid_->setGoal(goal_input_->getGoal().axis3[axis_]); pid_->setMeasured(state_estimator_->getAngulerVelocity()[axis_]); pid_->update(); diff --git a/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp b/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp index 9dd807f9..a9fb7445 100644 --- a/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp @@ -27,6 +27,8 @@ public: virtual void reset() override { + IController::reset(); + output_ = Axis4r(); for (unsigned int axis = 0; axis < 3; ++axis) { @@ -38,6 +40,8 @@ public: virtual void update() override { + IController::update(); + const auto& goal_mode = goal_input_->getGoalMode(); //for now we set throttle to same as goal @@ -60,6 +64,7 @@ public: //initialize axis controller axis_controllers_[axis]->initialize(axis, goal_input_, state_estimator_); + axis_controllers_[axis]->reset(); } //update axis controller diff --git a/AirLib/include/controllers/simple_flight/firmware/PidController.hpp b/AirLib/include/controllers/simple_flight/firmware/PidController.hpp index 88640703..51ea2859 100644 --- a/AirLib/include/controllers/simple_flight/firmware/PidController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/PidController.hpp @@ -80,6 +80,8 @@ public: virtual void reset() override { + IUpdatable::reset(); + goal_ = T(); measured_ = T(); last_time_ = clock_ == nullptr ? 0 : clock_->millis(); @@ -90,6 +92,8 @@ public: virtual void update() override { + IUpdatable::update(); + if (!config_.enabled) return; diff --git a/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp b/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp index f5b1130b..43bb32e5 100644 --- a/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/RemoteControl.hpp @@ -18,6 +18,8 @@ public: virtual void reset() override { + IGoalInput::reset(); + rc_channels_.assign(params_->rc_channel_count, 0); last_rec_read_ = 0; @@ -27,6 +29,8 @@ public: virtual void update() override { + IGoalInput::update(); + uint64_t time = clock_->millis(); //don't keep reading if not updated diff --git a/AirLib/include/controllers/simple_flight/firmware/interfaces/IAxisController.hpp b/AirLib/include/controllers/simple_flight/firmware/interfaces/IAxisController.hpp index 46396d1a..433ca84d 100644 --- a/AirLib/include/controllers/simple_flight/firmware/interfaces/IAxisController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/interfaces/IAxisController.hpp @@ -11,8 +11,6 @@ namespace simple_flight { class IAxisController : public IUpdatable { public: virtual void initialize(unsigned int axis, const IGoalInput* goal_input, const IStateEstimator* state_estimator) = 0; - virtual void reset() override = 0; - virtual void update() override = 0; virtual TReal getOutput() = 0; }; diff --git a/AirLib/include/controllers/simple_flight/firmware/interfaces/IController.hpp b/AirLib/include/controllers/simple_flight/firmware/interfaces/IController.hpp index 48570d32..1a73da1d 100644 --- a/AirLib/include/controllers/simple_flight/firmware/interfaces/IController.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/interfaces/IController.hpp @@ -11,8 +11,6 @@ namespace simple_flight { class IController : public IUpdatable { public: virtual void initialize(const IGoalInput* goal_input, const IStateEstimator* state_estimator) = 0; - virtual void reset() override = 0; - virtual void update() override = 0; virtual const Axis4r& getOutput() = 0; }; diff --git a/AirLib/include/controllers/simple_flight/firmware/interfaces/IUpdatable.hpp b/AirLib/include/controllers/simple_flight/firmware/interfaces/IUpdatable.hpp index d1203928..8d7f3875 100644 --- a/AirLib/include/controllers/simple_flight/firmware/interfaces/IUpdatable.hpp +++ b/AirLib/include/controllers/simple_flight/firmware/interfaces/IUpdatable.hpp @@ -4,8 +4,26 @@ namespace simple_flight { class IUpdatable { 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 ~IUpdatable() = default; + + +private: + bool reset_called = false; + bool update_called = false; }; } \ No newline at end of file diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp index e78f2364..d0261cd9 100644 --- a/AirLib/include/physics/Environment.hpp +++ b/AirLib/include/physics/Environment.hpp @@ -48,8 +48,6 @@ public: home_geo_point_ = EarthUtils::HomeGeoPoint(initial_.geo_point); updateState(initial_, home_geo_point_); - - Environment::reset(); } //in local NED coordinates diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 0aab8220..a378e618 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -18,13 +18,14 @@ public: FastPhysicsEngine(bool enable_ground_lock = true) : enable_ground_lock_(enable_ground_lock) { - FastPhysicsEngine::reset(); } //*** Start: UpdatableState implementation ***// virtual void reset() override { + PhysicsEngineBase::reset(); + for (PhysicsBody* body_ptr : *this) { initPhysicsBody(body_ptr); } @@ -39,6 +40,8 @@ public: virtual void update() override { + PhysicsEngineBase::update(); + for (PhysicsBody* body_ptr : *this) { updatePhysics(*body_ptr); } @@ -74,7 +77,7 @@ private: //first compute the response as if there was no collision //this is necessory to take in to account forces and torques generated by body getNextKinematicsNoCollison(dt, body, current, next, next_wrench); - + //if there is collison, see if we need collison response const CollisionInfo collison_info = body.getCollisionInfo(); CollisonResponseInfo& collison_response_info = body.getCollisionResponseInfo(); @@ -193,7 +196,6 @@ private: //else keep the orientation next.pose.position = collison_info.position + (collison_info.normal * collison_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles); - next_wrench = Wrench::zero(); return true; diff --git a/AirLib/include/physics/Kinematics.hpp b/AirLib/include/physics/Kinematics.hpp index 2f96426c..6016d4cc 100644 --- a/AirLib/include/physics/Kinematics.hpp +++ b/AirLib/include/physics/Kinematics.hpp @@ -36,17 +36,20 @@ public: void initialize(const State& initial) { initial_ = initial; - Kinematics::reset(); } //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + current_ = initial_; } virtual void update() override { + UpdatableObject::update(); + //nothing to do because next state should be updated //by physics engine. The reason is that final state //needs to take in to account state of other objects as well, diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index b78fe1cf..12e13b17 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/AirLib/include/physics/PhysicsBody.hpp @@ -74,8 +74,6 @@ public: //methods inertia_ = inertia; inertia_inv_ = inertia_.inverse(); environment_ = environment; - - PhysicsBody::reset(); } //enable physics body detection @@ -88,6 +86,8 @@ public: //methods //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + kinematics_.reset(); if (environment_) @@ -95,10 +95,20 @@ public: //methods wrench_ = Wrench::zero(); collison_info_ = CollisionInfo(); collison_response_info_ = CollisonResponseInfo(); + + //update individual vertices + for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) { + getWrenchVertex(vertex_index).reset(); + } + for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) { + getDragVertex(vertex_index).reset(); + } } virtual void update() override { + UpdatableObject::update(); + //update position from kinematics so we have latest position after physics update environment_->setPosition(getKinematics().pose.position); environment_->update(); @@ -108,6 +118,9 @@ public: //methods //update individual vertices for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) { getWrenchVertex(vertex_index).update(); + + //TODO: should we enable update on drag vertices? + //getDragVertex(vertex_index).update(); } } virtual void reportState(StateReporter& reporter) override @@ -163,6 +176,9 @@ public: //methods } void setKinematics(const Kinematics::State& state) { + if (VectorMath::hasNan(state.twist.linear)) + Utils::DebugBreak(); + kinematics_.setState(state); } const Kinematics::State& getInitialKinematics() const diff --git a/AirLib/include/physics/PhysicsBodyVertex.hpp b/AirLib/include/physics/PhysicsBodyVertex.hpp index ea671170..f5a61d8a 100644 --- a/AirLib/include/physics/PhysicsBodyVertex.hpp +++ b/AirLib/include/physics/PhysicsBodyVertex.hpp @@ -41,14 +41,14 @@ public: initial_position_ = position; initial_normal_ = normal; drag_factor_ = drag_factor; - - PhysicsBodyVertex::reset(); } //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + position_ = initial_position_; normal_ = initial_normal_; @@ -57,6 +57,8 @@ public: virtual void update() override { + UpdatableObject::update(); + setWrench(current_wrench_); } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp index 5b06c27f..5984d8d7 100644 --- a/AirLib/include/physics/PhysicsEngineBase.hpp +++ b/AirLib/include/physics/PhysicsEngineBase.hpp @@ -10,16 +10,46 @@ namespace msr { namespace airlib { -class PhysicsEngineBase : public UpdatableContainer { +class PhysicsEngineBase : public UpdatableObject { public: - //force derived classes to define this - virtual void reset() override = 0; - virtual void update() override = 0; + virtual void reset() override + { + UpdatableObject::reset(); + } + + virtual void update() override + { + UpdatableObject::update(); + } virtual void reportState(StateReporter& reporter) override { //default nothing to report for physics engine } + + //TODO: reduce copy-past from UpdatableContainer which has same code + /********************** Container interface **********************/ + typedef PhysicsBody* TUpdatableObjectPtr; + typedef vector MembersContainer; + typedef typename MembersContainer::iterator iterator; + typedef typename MembersContainer::const_iterator const_iterator; + typedef typename MembersContainer::value_type value_type; + + iterator begin() { return members_.begin(); } + iterator end() { return members_.end(); } + const_iterator begin() const { return members_.begin(); } + const_iterator end() const { return members_.end(); } + uint size() const { return static_cast(members_.size()); } + const TUpdatableObjectPtr &at(uint index) const { members_.at(index); } + TUpdatableObjectPtr &at(uint index) { return members_.at(index); } + //allow to override membership modifications + virtual void clear() { members_.clear(); } + virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member); } + virtual void erase_remove(TUpdatableObjectPtr obj) { + members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); } + +private: + MembersContainer members_; }; diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index 6770e104..07752d7c 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -10,6 +10,7 @@ #include "PhysicsEngineBase.hpp" #include "PhysicsBody.hpp" #include "common/common_utils/ScheduledExecutor.hpp" +//#include "common/DebugClock.hpp" namespace msr { namespace airlib { @@ -27,11 +28,12 @@ public: { World::clear(); + //clock_ = std::make_shared(3E-3f); + //msr::airlib::ClockFactory::get(clock_); + physics_engine_ = physics_engine; if (physics_engine) physics_engine_->clear(); - - World::reset(); } //override updatable interface so we can synchronize physics engine @@ -46,6 +48,9 @@ public: virtual void update() override { + //if (clock_) + // clock_->step(); + //first update our objects UpdatableContainer::update(); @@ -115,17 +120,20 @@ private: update(); } catch(const std::exception& ex) { + Utils::DebugBreak(); Utils::log(Utils::stringf("Exception occurred while updating world: %s", ex.what()), Utils::kLogLevelError); } catch(...) { + Utils::DebugBreak(); Utils::log("Exception occurred while updating world", Utils::kLogLevelError); } + return true; } private: PhysicsEngineBase* physics_engine_ = nullptr; - + //std::shared_ptr clock_; common_utils::ScheduledExecutor executor_; }; diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 390c2a04..1839b9c1 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -29,8 +29,6 @@ public: { ground_truth_.kinematics = kinematics; ground_truth_.environment = environment; - - reset(); } const GroundTruth& getGroundTruth() const diff --git a/AirLib/include/sensors/SensorCollection.hpp b/AirLib/include/sensors/SensorCollection.hpp index 9d3d257d..fb72fb53 100644 --- a/AirLib/include/sensors/SensorCollection.hpp +++ b/AirLib/include/sensors/SensorCollection.hpp @@ -72,6 +72,8 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { + UpdatableObject::reset(); + for (auto& pair : sensors_) { pair.second->reset(); } @@ -79,6 +81,8 @@ public: virtual void update() override { + UpdatableObject::update(); + for (auto& pair : sensors_) { pair.second->update(); } diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index efe21369..f0a8e299 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -35,6 +35,8 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { + BarometerBase::reset(); + pressure_factor_.reset(); correlated_noise_.reset(); uncorrelated_noise_.reset(); @@ -47,6 +49,8 @@ public: virtual void update() override { + BarometerBase::update(); + freq_limiter_.update(); if (freq_limiter_.isWaitComplete()) { diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 4d97458d..d2df628a 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -32,6 +32,8 @@ public: //methods //*** Start: UpdatableState implementation ***// virtual void reset() override { + GpsBase::reset(); + freq_limiter_.reset(); delay_line_.reset(); @@ -43,6 +45,8 @@ public: //methods virtual void update() override { + GpsBase::update(); + freq_limiter_.update(); eph_filter.update(); epv_filter.update(); diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 331e5475..1aab8cb5 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -24,6 +24,8 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { + ImuBase::reset(); + last_time_ = clock()->nowNanos(); state_.gyroscope_bias = params_.gyro.turn_on_bias; @@ -34,6 +36,8 @@ public: virtual void update() override { + ImuBase::update(); + updateOutput(); } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 91d270e0..6fb86ffb 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -30,6 +30,8 @@ public: //*** Start: UpdatableObject implementation ***// virtual void reset() override { + MagnetometerBase::reset(); + //Ground truth is reset before sensors are reset updateReference(getGroundTruth()); noise_vec_.reset(); @@ -42,6 +44,8 @@ public: virtual void update() override { + MagnetometerBase::update(); + freq_limiter_.update(); if (freq_limiter_.isWaitComplete()) { diff --git a/AirLib/include/vehicles/MultiRotor.hpp b/AirLib/include/vehicles/MultiRotor.hpp index 8384f9e0..aac82cc8 100644 --- a/AirLib/include/vehicles/MultiRotor.hpp +++ b/AirLib/include/vehicles/MultiRotor.hpp @@ -35,8 +35,6 @@ public: createDragVertices(); initSensors(*params_, getKinematics(), getEnvironment()); - - MultiRotor::reset(); } DroneControllerBase* getController() @@ -47,16 +45,12 @@ public: //*** Start: UpdatableState implementation ***// virtual void reset() override { - //reset inputs - if (getController()) - getController()->reset(); - //reset rotors, kinematics and environment PhysicsBody::reset(); - //reset rotors after environment is reset - for (Rotor& rotor : rotors_) - rotor.reset(); + //reset inputs + if (getController()) + getController()->reset(); //reset sensors last after their ground truth has been reset resetSensors(); @@ -66,6 +60,8 @@ public: { //update forces and environment as a result of last dt PhysicsBody::update(); + + //Note that controller gets updated after kinematics gets updated in kinematicsUpdated } virtual void reportState(StateReporter& reporter) override { diff --git a/AirLib/include/vehicles/Rotor.hpp b/AirLib/include/vehicles/Rotor.hpp index 072ff5c7..46c17837 100644 --- a/AirLib/include/vehicles/Rotor.hpp +++ b/AirLib/include/vehicles/Rotor.hpp @@ -48,8 +48,6 @@ public: //methods control_signal_filter_.initialize(params_.control_signal_filter_tc, 0, 0); PhysicsBodyVertex::initialize(position, normal); //call base initializer - - Rotor::reset(); } //0 to 1 - will be scalled to 0 to max_speed @@ -67,11 +65,11 @@ public: //methods //*** Start: UpdatableState implementation ***// virtual void reset() override { + PhysicsBodyVertex::reset(); + //update environmental factors before we call base updateEnvironmentalFactors(); - PhysicsBodyVertex::reset(); - control_signal_filter_.reset(); setOutput(output_, params_, control_signal_filter_, turning_direction_); diff --git a/AirLibUnitTests/QuaternionTest.hpp b/AirLibUnitTests/QuaternionTest.hpp index 45aef5af..d481fc1f 100644 --- a/AirLibUnitTests/QuaternionTest.hpp +++ b/AirLibUnitTests/QuaternionTest.hpp @@ -21,7 +21,7 @@ public: VectorMath::toEulerianAngle(q, pitch, roll, yaw); Quaternionr qd = VectorMath::toQuaternion(pitch, roll, yaw); - if (qd.w() < 0) { + if (std::signbit(qd.w()) != std::signbit(q.w())) { qd.coeffs() = - qd.coeffs(); } diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index e6736c8d..2a76856a 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -74,6 +74,7 @@ int main(int argc, const char* argv[]) MavLinkDroneController mav_drone; mav_drone.initialize(connection_info, nullptr, is_simulation); + mav_drone.reset(); mav_drone.start(); DroneControllerCancelable server_wrapper(&mav_drone); diff --git a/Examples/StandAlonePhysics.hpp b/Examples/StandAlonePhysics.hpp index 9566bc71..9926c6b0 100644 --- a/Examples/StandAlonePhysics.hpp +++ b/Examples/StandAlonePhysics.hpp @@ -25,11 +25,12 @@ public: DebugPhysicsBody body; body.initialize(initial_kinematics, &environment); + body.reset(); //create physics engine FastPhysicsEngine physics; physics.insert(&body); - + physics.reset(); //run unsigned int i = 0; diff --git a/Examples/StandAloneSensors.hpp b/Examples/StandAloneSensors.hpp index bc6ce03e..1eee8e03 100644 --- a/Examples/StandAloneSensors.hpp +++ b/Examples/StandAloneSensors.hpp @@ -16,9 +16,11 @@ namespace msr { auto kinematics = Kinematics::State::zero(); msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); ImuSimple imu; imu.initialize(&kinematics, &environment); + imu.reset(); float interations = total_duration / period; @@ -45,9 +47,11 @@ namespace msr { auto kinematics = Kinematics::State::zero(); msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); BarometerSimple baro; baro.initialize(&kinematics, &environment); + baro.reset(); float interations = total_duration / period; @@ -76,9 +80,11 @@ namespace msr { auto kinematics = Kinematics::State::zero(); msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); BarometerSimple baro; baro.initialize(&kinematics, &environment); + baro.reset(); float interations_20s = 20.0f / period; @@ -132,8 +138,11 @@ namespace msr { kinematics.pose.orientation = VectorMath::toQuaternion(0, 0, yaw); msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); + MagnetometerSimple mag; mag.initialize(&kinematics, &environment); + mag.reset(); for (auto i = 0; i < interations; ++i) { const auto& output = mag.getOutput(); @@ -178,8 +187,11 @@ namespace msr { kinematics.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw); msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); + MagnetometerSimple mag; mag.initialize(&kinematics, &environment); + mag.reset(); for (auto i = 0; i < interations; ++i) { const auto& output = mag.getOutput(); @@ -211,8 +223,11 @@ namespace msr { kinematics.pose.orientation = VectorMath::toQuaternion(0, 0, 0); msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0); msr::airlib::Environment environment(initial_environment); + environment.reset(); + MagnetometerSimple mag; mag.initialize(&kinematics, &environment); + mag.reset(); for (float lat = -90; lat < 90; lat++) { diff --git a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp b/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp index 9ff29006..78065788 100644 --- a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp @@ -20,63 +20,63 @@ using milliseconds = std::chrono::milliseconds; /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to /// 32 bit alignment to avoid usage of any pack pragmas. struct FtpPayload { - uint16_t seq_number; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstreadFile - 1: set of burst packets complete, 0: More burst packets coming. - uint8_t padding; ///< 32 bit aligment padding - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data; ///< command data, varies by Opcode + uint16_t seq_number; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstreadFile - 1: set of burst packets complete, 0: More burst packets coming. + uint8_t padding; ///< 32 bit aligment padding + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data; ///< command data, varies by Opcode }; void setPayloadFilename(FtpPayload* payload, const char* filename) { - const size_t maxFileName = 251 - sizeof(FtpPayload); - size_t len = strlen(filename); - if (len > maxFileName) { - len = maxFileName; - } - strncpy(reinterpret_cast(&payload->data), filename, len); - payload->size = static_cast(len); + const size_t maxFileName = 251 - sizeof(FtpPayload); + size_t len = strlen(filename); + if (len > maxFileName) { + len = maxFileName; + } + strncpy(reinterpret_cast(&payload->data), filename, len); + payload->size = static_cast(len); } /// @brief Command opcodes enum Opcode : uint8_t { - kCmdNone, ///< ignored, always acked - kCmdTerminateSession, ///< Terminates open Read session - kCmdResetSessions, ///< Terminates all open Read sessions - kCmdListDirectory, ///< List files in from - kCmdOpenFileRO, ///< Opens file at for reading, returns - kCmdReadFile, ///< Reads bytes from in - kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Writes bytes to in - kCmdRemoveFile, ///< Remove file at - kCmdCreateDirectory, ///< Creates directory at - kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kCmdOpenFileWO, ///< Opens file at for writing, returns - kCmdTruncateFile, ///< Truncate file at to length - kCmdRename, ///< Rename to - kCmdCalcFileCRC32, ///< Calculate CRC32 for file at - kCmdBurstreadFile, ///< Burst download session file + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFileRO, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Writes bytes to in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + kCmdBurstreadFile, ///< Burst download session file - kRspAck = 128, ///< Ack response - kRspNak ///< Nak response + kRspAck = 128, ///< Ack response + kRspNak ///< Nak response }; /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t { - kErrNone, - kErrFail, ///< Unknown failure - kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] - kErrInvalidDataSize, ///< PayloadHeader.size is invalid - kErrInvalidSession, ///< Session is not currently open - kErrNoSessionsAvailable, ///< All available Sessions in use - kErrEOF, ///< Offset past end of file for List and Read commands - kErrRetriesExhausted, - kErrUnknownCommand ///< Unknown command opcode + kErrNone, + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrRetriesExhausted, + kErrUnknownCommand ///< Unknown command opcode }; static const char kDirentFile = 'F'; ///< Identifies File returned from List command @@ -84,7 +84,7 @@ static const char kDirentDir = 'D'; ///< Identifies Directory returned from List static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command MavLinkFtpClientImpl::MavLinkFtpClientImpl(int localSystemId, int localComponentId) - : MavLinkNodeImpl(localSystemId, localComponentId) + : MavLinkNodeImpl(localSystemId, localComponentId) { } @@ -95,98 +95,98 @@ MavLinkFtpClientImpl::~MavLinkFtpClientImpl() bool MavLinkFtpClientImpl::isSupported() { - // request capabilities, it will respond with AUTOPILOT_VERSION. - MavLinkAutopilotVersion ver; - if (!getCapabilities().wait(5000, &ver)) { - throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); - } - return (ver.capabilities & static_cast(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_FTP)) != 0; + // request capabilities, it will respond with AUTOPILOT_VERSION. + MavLinkAutopilotVersion ver; + if (!getCapabilities().wait(5000, &ver)) { + throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); + } + return (ver.capabilities & static_cast(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_FTP)) != 0; } void MavLinkFtpClientImpl::subscribe() { - if (subscription_ == 0) { - subscription_ = getConnection()->subscribe([=](std::shared_ptr connection, const MavLinkMessage& msg) { + if (subscription_ == 0) { + subscription_ = getConnection()->subscribe([=](std::shared_ptr connection, const MavLinkMessage& msg) { unused(connection); - handleResponse(msg); - }); - } + handleResponse(msg); + }); + } } void MavLinkFtpClientImpl::list(MavLinkFtpProgress& progress, const std::string& remotePath, std::vector& files) { - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - files_ = &files; - command_ = FtpCommandList; - remote_file_ = remotePath; + if (waiting_) { + cancel(); + } + ensureConnection(); + progress_ = &progress; + files_ = &files; + command_ = FtpCommandList; + remote_file_ = remotePath; size_t len = remote_file_.size(); if (len > 1 && remote_file_[len - 1] == '/') { // must trim trailing slashes so PX4 doesn't hang! remote_file_ = remote_file_.substr(0, len - 1); } - file_index_ = 0; - runStateMachine(); - progress.complete = true; - files_ = nullptr; - progress_ = nullptr; + file_index_ = 0; + runStateMachine(); + progress.complete = true; + files_ = nullptr; + progress_ = nullptr; } void MavLinkFtpClientImpl::get(MavLinkFtpProgress& progress, const std::string& remotePath, const std::string& localPath) { - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandGet; - local_file_ = localPath; - remote_file_ = remotePath; - bytes_read_ = 0; - file_size_ = 0; - remote_file_open_ = false; + if (waiting_) { + cancel(); + } + ensureConnection(); + progress_ = &progress; + command_ = FtpCommandGet; + local_file_ = localPath; + remote_file_ = remotePath; + bytes_read_ = 0; + file_size_ = 0; + remote_file_open_ = false; - runStateMachine(); - progress_ = nullptr; - progress.complete = true; + runStateMachine(); + progress_ = nullptr; + progress.complete = true; } void MavLinkFtpClientImpl::put(MavLinkFtpProgress& progress, const std::string& remotePath, const std::string& localPath) { - local_file_ = localPath; - remote_file_ = remotePath; + local_file_ = localPath; + remote_file_ = remotePath; - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandPut; + if (waiting_) { + cancel(); + } + ensureConnection(); + progress_ = &progress; + command_ = FtpCommandPut; - if (openSourceFile()) { - remote_file_open_ = false; - runStateMachine(); - } - progress_ = nullptr; - progress.complete = true; + if (openSourceFile()) { + remote_file_open_ = false; + runStateMachine(); + } + progress_ = nullptr; + progress.complete = true; } void MavLinkFtpClientImpl::remove(MavLinkFtpProgress& progress, const std::string& remotePath) { - remote_file_ = remotePath; + remote_file_ = remotePath; - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandRemove; - runStateMachine(); - progress_ = nullptr; - progress.complete = true; + if (waiting_) { + cancel(); + } + ensureConnection(); + progress_ = &progress; + command_ = FtpCommandRemove; + runStateMachine(); + progress_ = nullptr; + progress.complete = true; } @@ -222,100 +222,100 @@ void MavLinkFtpClientImpl::rmdir(MavLinkFtpProgress& progress, const std::string void MavLinkFtpClientImpl::runStateMachine() { - waiting_ = true; - retries_ = 0; - total_time_ = milliseconds::zero(); - messages_ = 0; + waiting_ = true; + retries_ = 0; + total_time_ = milliseconds::zero(); + messages_ = 0; - progress_->cancel = false; - progress_->error = 0; - progress_->current = 0; - progress_->goal = 0; - progress_->complete = false; - progress_->longest_delay = 0; - progress_->message_count = 0; + progress_->cancel = false; + progress_->error = 0; + progress_->current = 0; + progress_->goal = 0; + progress_->complete = false; + progress_->longest_delay = 0; + progress_->message_count = 0; - int before = 0; - subscribe(); - nextStep(); + int before = 0; + subscribe(); + nextStep(); - const int monitorInterval = 30; // milliseconds between monitoring progress. - double rate = 0; - double totalSleep = 0; + const int monitorInterval = 30; // milliseconds between monitoring progress. + double rate = 0; + double totalSleep = 0; - while (waiting_) - { - std::this_thread::sleep_for(std::chrono::milliseconds(monitorInterval)); - totalSleep += monitorInterval; + while (waiting_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(monitorInterval)); + totalSleep += monitorInterval; - int after = 0; - { - std::lock_guard guard(mutex_); - after = messages_; - if (messages_ > 0) { - rate = static_cast(total_time_.count()) / static_cast(messages_); - if (progress_ != nullptr) - { - progress_->average_rate = rate; - } - } - } + int after = 0; + { + std::lock_guard guard(mutex_); + after = messages_; + if (messages_ > 0) { + rate = static_cast(total_time_.count()) / static_cast(messages_); + if (progress_ != nullptr) + { + progress_->average_rate = rate; + } + } + } - if (before == after) - { - if (totalSleep > (MAXIMUM_ROUND_TRIP_TIME * TIMEOUT_INTERVAL)) { - Utils::log("ftp command timeout, not getting a response, so retrying\n", Utils::kLogLevelWarn); - retry(); - totalSleep = 0; - } - } - else - { - totalSleep = 0; - } - before = after; - } - waiting_ = false; + if (before == after) + { + if (totalSleep > (MAXIMUM_ROUND_TRIP_TIME * TIMEOUT_INTERVAL)) { + Utils::log("ftp command timeout, not getting a response, so retrying\n", Utils::kLogLevelWarn); + retry(); + totalSleep = 0; + } + } + else + { + totalSleep = 0; + } + before = after; + } + waiting_ = false; } void MavLinkFtpClientImpl::nextStep() { - switch (command_) - { - case FtpCommandList: - listDirectory(); - break; - case FtpCommandGet: - readFile(); - break; - case FtpCommandPut: - writeFile(); - break; - case FtpCommandRemove: - removeFile(); - break; + switch (command_) + { + case FtpCommandList: + listDirectory(); + break; + case FtpCommandGet: + readFile(); + break; + case FtpCommandPut: + writeFile(); + break; + case FtpCommandRemove: + removeFile(); + break; case FtpCommandMkdir: mkdir(); break; case FtpCommandRmdir: rmdir(); break; - default: - break; - } + default: + break; + } } void MavLinkFtpClientImpl::removeFile() { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdRemoveFile; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); + MavLinkFileTransferProtocol ftp; + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + payload->opcode = kCmdRemoveFile; + setPayloadFilename(payload, remote_file_.c_str()); + sendMessage(ftp); + recordMessageSent(); } void MavLinkFtpClientImpl::mkdir() @@ -344,350 +344,350 @@ void MavLinkFtpClientImpl::rmdir() void MavLinkFtpClientImpl::listDirectory() { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdListDirectory; - setPayloadFilename(payload, remote_file_.c_str()); - payload->offset = file_index_; - sendMessage(ftp); - recordMessageSent(); + MavLinkFileTransferProtocol ftp; + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + payload->opcode = kCmdListDirectory; + setPayloadFilename(payload, remote_file_.c_str()); + payload->offset = file_index_; + sendMessage(ftp); + recordMessageSent(); } bool MavLinkFtpClientImpl::openSourceFile() { - file_ptr_ = fopen(local_file_.c_str(), "rb"); - if (file_ptr_ == nullptr) - { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file '%s' for reading, rc=%d", remote_file_.c_str(), progress_->error); - } - return false; - } - fseek(file_ptr_, 0, SEEK_END); - long pos = ftell(file_ptr_); - fseek(file_ptr_, 0, SEEK_SET); - file_size_ = static_cast(pos); - if (progress_ != nullptr) { - progress_->goal = file_size_; - } - return true; + file_ptr_ = fopen(local_file_.c_str(), "rb"); + if (file_ptr_ == nullptr) + { + if (progress_ != nullptr) { + progress_->error = errno; + progress_->message = Utils::stringf("Error opening file '%s' for reading, rc=%d", remote_file_.c_str(), progress_->error); + } + return false; + } + fseek(file_ptr_, 0, SEEK_END); + long pos = ftell(file_ptr_); + fseek(file_ptr_, 0, SEEK_SET); + file_size_ = static_cast(pos); + if (progress_ != nullptr) { + progress_->goal = file_size_; + } + return true; } bool MavLinkFtpClientImpl::createLocalFile() { - if (file_ptr_ == nullptr) { - auto path = FileSystem::getFullPath(local_file_); - if (FileSystem::isDirectory(path)) - { - // user was lazy, only told us where to put the file, so we borrow the name of the file - // from the source. - auto remote = FileSystem::getFileName(normalize(remote_file_)); - local_file_ = FileSystem::combine(path, remote); - } - else - { - // check if directory exists. - FileSystem::removeLeaf(path); - if (FileSystem::isDirectory(path)) - { - // perfect. - } - else if (FileSystem::exists(path)) - { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file because '%s' is not a directory", path.c_str()); - } - return false; - } - else - { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file because '%s' should be a directory but it was not found", path.c_str()); - } - return false; - } - } + if (file_ptr_ == nullptr) { + auto path = FileSystem::getFullPath(local_file_); + if (FileSystem::isDirectory(path)) + { + // user was lazy, only told us where to put the file, so we borrow the name of the file + // from the source. + auto remote = FileSystem::getFileName(normalize(remote_file_)); + local_file_ = FileSystem::combine(path, remote); + } + else + { + // check if directory exists. + FileSystem::removeLeaf(path); + if (FileSystem::isDirectory(path)) + { + // perfect. + } + else if (FileSystem::exists(path)) + { + if (progress_ != nullptr) { + progress_->error = errno; + progress_->message = Utils::stringf("Error opening file because '%s' is not a directory", path.c_str()); + } + return false; + } + else + { + if (progress_ != nullptr) { + progress_->error = errno; + progress_->message = Utils::stringf("Error opening file because '%s' should be a directory but it was not found", path.c_str()); + } + return false; + } + } - file_ptr_ = fopen(local_file_.c_str(), "wb"); - if (file_ptr_ == nullptr) - { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file '%s' for writing, rc=%d", local_file_.c_str(), errno); - } - return false; - } - file_size_ = 0; - } - return true; + file_ptr_ = fopen(local_file_.c_str(), "wb"); + if (file_ptr_ == nullptr) + { + if (progress_ != nullptr) { + progress_->error = errno; + progress_->message = Utils::stringf("Error opening file '%s' for writing, rc=%d", local_file_.c_str(), errno); + } + return false; + } + file_size_ = 0; + } + return true; } void MavLinkFtpClientImpl::readFile() { - if (!remote_file_open_) - { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdOpenFileRO; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); - } - else - { - if (createLocalFile()) - { - // use last_message_ so we preserve the sessionid. - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - payload->opcode = kCmdReadFile; - payload->offset = bytes_read_; - last_message_.target_component = getTargetComponentId(); - last_message_.target_system = getTargetSystemId(); - sendMessage(last_message_); - recordMessageSent(); - if (progress_ != nullptr) { - progress_->current = bytes_read_; - } - } - else - { - // could not create the local file, so stop. - waiting_ = false; - } - } + if (!remote_file_open_) + { + MavLinkFileTransferProtocol ftp; + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + payload->opcode = kCmdOpenFileRO; + setPayloadFilename(payload, remote_file_.c_str()); + sendMessage(ftp); + recordMessageSent(); + } + else + { + if (createLocalFile()) + { + // use last_message_ so we preserve the sessionid. + FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); + payload->opcode = kCmdReadFile; + payload->offset = bytes_read_; + last_message_.target_component = getTargetComponentId(); + last_message_.target_system = getTargetSystemId(); + sendMessage(last_message_); + recordMessageSent(); + if (progress_ != nullptr) { + progress_->current = bytes_read_; + } + } + else + { + // could not create the local file, so stop. + waiting_ = false; + } + } } void MavLinkFtpClientImpl::writeFile() { - if (!remote_file_open_) - { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdOpenFileWO; - strcpy(reinterpret_cast(&payload->data), remote_file_.c_str()); - payload->size = static_cast(remote_file_.size()); - sendMessage(ftp); - recordMessageSent(); - } - else - { - // must use last_message_ so we preserve the session id. - MavLinkFileTransferProtocol& ftp = last_message_; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - payload->opcode = kCmdWriteFile; - payload->seq_number = sequence_; - fseek(file_ptr_, bytes_written_, SEEK_SET); + if (!remote_file_open_) + { + MavLinkFileTransferProtocol ftp; + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + payload->opcode = kCmdOpenFileWO; + strcpy(reinterpret_cast(&payload->data), remote_file_.c_str()); + payload->size = static_cast(remote_file_.size()); + sendMessage(ftp); + recordMessageSent(); + } + else + { + // must use last_message_ so we preserve the session id. + MavLinkFileTransferProtocol& ftp = last_message_; + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + payload->opcode = kCmdWriteFile; + payload->seq_number = sequence_; + fseek(file_ptr_, bytes_written_, SEEK_SET); uint8_t* data = &payload->data; - size_t bytes = fread(data, 1, 251 - 12, file_ptr_); - if (progress_ != nullptr) { - progress_->current = bytes_written_; - } - if (bytes == 0) - { - success_ = true; - reset(); - int err = ferror(file_ptr_); - if (err != 0) { - if (progress_ != nullptr) { - progress_->error = err; - progress_->message = Utils::stringf("error reading local file, errno=%d", err); - } - } - fclose(file_ptr_); - file_ptr_ = nullptr; - waiting_ = false; - } - else - { - payload->offset = bytes_written_; - payload->size = static_cast(bytes); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - sendMessage(ftp); - recordMessageSent(); - } - } + size_t bytes = fread(data, 1, 251 - 12, file_ptr_); + if (progress_ != nullptr) { + progress_->current = bytes_written_; + } + if (bytes == 0) + { + success_ = true; + reset(); + int err = ferror(file_ptr_); + if (err != 0) { + if (progress_ != nullptr) { + progress_->error = err; + progress_->message = Utils::stringf("error reading local file, errno=%d", err); + } + } + fclose(file_ptr_); + file_ptr_ = nullptr; + waiting_ = false; + } + else + { + payload->offset = bytes_written_; + payload->size = static_cast(bytes); + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + sendMessage(ftp); + recordMessageSent(); + } + } } void MavLinkFtpClientImpl::cancel() { - // todo: wait for any pending responses from PX4 so we can safely start a new command. - reset(); + // todo: wait for any pending responses from PX4 so we can safely start a new command. + reset(); } void MavLinkFtpClientImpl::close() { - if (file_ptr_ != nullptr) { - reset(); - fclose(file_ptr_); - file_ptr_ = nullptr; - } - if (subscription_ != 0) { - getConnection()->unsubscribe(subscription_); - subscription_ = 0; - } + if (file_ptr_ != nullptr) { + reset(); + fclose(file_ptr_); + file_ptr_ = nullptr; + } + if (subscription_ != 0) { + getConnection()->unsubscribe(subscription_); + subscription_ = 0; + } } void MavLinkFtpClientImpl::reset() { - MavLinkFileTransferProtocol ftp; - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - payload->opcode = kCmdResetSessions; - sendMessage(ftp); - recordMessageSent(); + MavLinkFileTransferProtocol ftp; + ftp.target_component = getTargetComponentId(); + ftp.target_system = getTargetSystemId(); + FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); + payload->opcode = kCmdResetSessions; + sendMessage(ftp); + recordMessageSent(); } void MavLinkFtpClientImpl::handleListResponse() { - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); + FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->offset != file_index_) - { - // todo: error handling here? sequence is out of order... + if (payload->offset != file_index_) + { + // todo: error handling here? sequence is out of order... Utils::log(Utils::stringf("list got offset %d, but expecting file index %d\n", payload->offset, file_index_), Utils::kLogLevelError); - retry(); - return; - } + retry(); + return; + } - if (payload->offset == 0 && payload->size == 0) { - // directory must be empty then, can't do nextStep because - // it will just loop for ever re-requesting zero offset into - // empty directory. - reset(); - success_ = true; - waiting_ = false; - return; - } + if (payload->offset == 0 && payload->size == 0) { + // directory must be empty then, can't do nextStep because + // it will just loop for ever re-requesting zero offset into + // empty directory. + reset(); + success_ = true; + waiting_ = false; + return; + } - // result should be a list of null terminated file names. + // result should be a list of null terminated file names. uint8_t* data = &payload->data; - for (int offset = 0; offset < payload->size; ) - { - uint8_t dirType = data[offset]; - offset++; - retries_ = 0; - file_index_++; - int len = 0; - std::string name = reinterpret_cast(&data[offset]); - if (dirType == kDirentSkip) { - // skipping this entry - } - else if (dirType == kDirentFile) { - size_t i = name.find_last_of('\t'); - MavLinkFileInfo info; - info.is_directory = false; - len = static_cast(name.size()); - if (i > 0) { - // remove the file size field. - std::string size(name.begin() + i + 1, name.end()); - info.size = Utils::to_integer(size); - name.erase(name.begin() + i, name.end()); - } - info.name = name; - //printf("%s\n", name.c_str()); - if (files_ != nullptr) { - files_->push_back(info); - } - } - else if (dirType == kDirentDir) { - MavLinkFileInfo info; - info.is_directory = true; - info.name = name; - len = static_cast(name.size()); - if (files_ != nullptr) { - files_->push_back(info); - } - } - offset += len + 1; - } - if (progress_ != nullptr) { - progress_->current = file_index_; - } - // request the next batch. - nextStep(); + for (int offset = 0; offset < payload->size; ) + { + uint8_t dirType = data[offset]; + offset++; + retries_ = 0; + file_index_++; + int len = 0; + std::string name = reinterpret_cast(&data[offset]); + if (dirType == kDirentSkip) { + // skipping this entry + } + else if (dirType == kDirentFile) { + size_t i = name.find_last_of('\t'); + MavLinkFileInfo info; + info.is_directory = false; + len = static_cast(name.size()); + if (i > 0) { + // remove the file size field. + std::string size(name.begin() + i + 1, name.end()); + info.size = Utils::to_integer(size); + name.erase(name.begin() + i, name.end()); + } + info.name = name; + //printf("%s\n", name.c_str()); + if (files_ != nullptr) { + files_->push_back(info); + } + } + else if (dirType == kDirentDir) { + MavLinkFileInfo info; + info.is_directory = true; + info.name = name; + len = static_cast(name.size()); + if (files_ != nullptr) { + files_->push_back(info); + } + } + offset += len + 1; + } + if (progress_ != nullptr) { + progress_->current = file_index_; + } + // request the next batch. + nextStep(); } void MavLinkFtpClientImpl::handleReadResponse() { - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->req_opcode == kCmdOpenFileRO) { - remote_file_open_ = true; - bytes_read_ = 0; - retries_ = 0; - sequence_ = payload->seq_number; - uint32_t* size = reinterpret_cast(&payload->data); - file_size_ = static_cast(*size); - if (progress_ != nullptr) { - progress_->goal = file_size_; - } - nextStep(); - } - else if (payload->req_opcode == kCmdReadFile) - { - int seq = static_cast(payload->seq_number); - if (seq != sequence_ + 1) - { + FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); + if (payload->req_opcode == kCmdOpenFileRO) { + remote_file_open_ = true; + bytes_read_ = 0; + retries_ = 0; + sequence_ = payload->seq_number; + uint32_t* size = reinterpret_cast(&payload->data); + file_size_ = static_cast(*size); + if (progress_ != nullptr) { + progress_->goal = file_size_; + } + nextStep(); + } + else if (payload->req_opcode == kCmdReadFile) + { + int seq = static_cast(payload->seq_number); + if (seq != sequence_ + 1) + { Utils::log(Utils::stringf("packet %d is out of sequence, expecting number %d\n", seq, sequence_ + 1), Utils::kLogLevelError); - // perhaps this was a late response after we did a retry, so ignore it. - return; - } - else if (file_ptr_ != nullptr) - { - sequence_ = payload->seq_number; - fwrite(&payload->data, payload->size, 1, file_ptr_); - bytes_read_ += payload->size; - retries_ = 0; - nextStep(); - } - } + // perhaps this was a late response after we did a retry, so ignore it. + return; + } + else if (file_ptr_ != nullptr) + { + sequence_ = payload->seq_number; + fwrite(&payload->data, payload->size, 1, file_ptr_); + bytes_read_ += payload->size; + retries_ = 0; + nextStep(); + } + } } void MavLinkFtpClientImpl::handleWriteResponse() { - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->req_opcode == kCmdOpenFileWO) - { - remote_file_open_ = true; - bytes_written_ = 0; - retries_ = 0; - sequence_ = payload->seq_number; - nextStep(); - } - else if (payload->req_opcode == kCmdWriteFile) - { - int seq = static_cast(payload->seq_number); - if (seq != sequence_ + 1) - { + FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); + if (payload->req_opcode == kCmdOpenFileWO) + { + remote_file_open_ = true; + bytes_written_ = 0; + retries_ = 0; + sequence_ = payload->seq_number; + nextStep(); + } + else if (payload->req_opcode == kCmdWriteFile) + { + int seq = static_cast(payload->seq_number); + if (seq != sequence_ + 1) + { Utils::log(Utils::stringf("packet %d is out of sequence, expecting number %d\n", seq, sequence_ + 1), Utils::kLogLevelError); - // perhaps this was a late response after we did a retry, so ignore it. - return; - } + // perhaps this was a late response after we did a retry, so ignore it. + return; + } - uint32_t* size = reinterpret_cast(&payload->data); - // payload->size contains the bytes_written from PX4, so that's how much we advance. - bytes_written_ += static_cast(*size); - retries_ = 0; - sequence_++; - nextStep(); - } + uint32_t* size = reinterpret_cast(&payload->data); + // payload->size contains the bytes_written from PX4, so that's how much we advance. + bytes_written_ += static_cast(*size); + retries_ = 0; + sequence_++; + nextStep(); + } } void MavLinkFtpClientImpl::handleRemoveResponse() { - success_ = true; - waiting_ = false; + success_ = true; + waiting_ = false; } void MavLinkFtpClientImpl::handleRmdirResponse() @@ -704,51 +704,51 @@ void MavLinkFtpClientImpl::handleMkdirResponse() void MavLinkFtpClientImpl::handleResponse(const MavLinkMessage& msg) { - if (msg.msgid == static_cast(MavLinkMessageIds::MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL)) { + if (msg.msgid == static_cast(MavLinkMessageIds::MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL)) { - last_message_.decode(msg); - recordMessageReceived(); + last_message_.decode(msg); + recordMessageReceived(); - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->opcode == kRspNak) { + FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); + if (payload->opcode == kRspNak) { - // reached the end of the list or the file. - if (file_ptr_ != nullptr) { - fclose(file_ptr_); - file_ptr_ = nullptr; - } + // reached the end of the list or the file. + if (file_ptr_ != nullptr) { + fclose(file_ptr_); + file_ptr_ = nullptr; + } - int error = static_cast(payload->data); - if (error == kErrEOF) { - // end of file or directory listing. - success_ = true; - error = 0; - } - else - { - success_ = false; - if (progress_ != nullptr) { - if (error == kErrFailErrno) { - const uint8_t* data = &(payload->data); - error = static_cast(data[1]); - progress_->error = error; - progress_->message = Utils::stringf("ftp kErrFailErrno %d", error); - } - else { - progress_->error = error; - progress_->message = Utils::stringf("ftp error %d", error); - } - } - } - errorCode_ = error; - waiting_ = false; - reset(); - } - else if (payload->opcode == kRspAck) - { - if (progress_ != nullptr) { - progress_->message_count++; - } + int error = static_cast(payload->data); + if (error == kErrEOF) { + // end of file or directory listing. + success_ = true; + error = 0; + } + else + { + success_ = false; + if (progress_ != nullptr) { + if (error == kErrFailErrno) { + const uint8_t* data = &(payload->data); + error = static_cast(data[1]); + progress_->error = error; + progress_->message = Utils::stringf("ftp kErrFailErrno %d", error); + } + else { + progress_->error = error; + progress_->message = Utils::stringf("ftp error %d", error); + } + } + } + errorCode_ = error; + waiting_ = false; + reset(); + } + else if (payload->opcode == kRspAck) + { + if (progress_ != nullptr) { + progress_->message_count++; + } // success, data should be following... switch (payload->req_opcode) { case kCmdListDirectory: @@ -760,13 +760,13 @@ void MavLinkFtpClientImpl::handleResponse(const MavLinkMessage& msg) break; case kCmdOpenFileWO: case kCmdWriteFile: - handleWriteResponse(); + handleWriteResponse(); break; case kCmdResetSessions: // ack on this cmd is a noop break; case kCmdRemoveFile: - handleRemoveResponse(); + handleRemoveResponse(); break; case kCmdRemoveDirectory: handleRmdirResponse(); @@ -775,72 +775,72 @@ void MavLinkFtpClientImpl::handleResponse(const MavLinkMessage& msg) handleMkdirResponse(); break; default: - // todo: how to handle this? For now we ignore it and let the watchdog kick in and do a retry. + // todo: how to handle this? For now we ignore it and let the watchdog kick in and do a retry. Utils::log(Utils::stringf("Unexpected ACK with req_opcode=%d\n", static_cast(payload->req_opcode)), Utils::kLogLevelWarn); break; - } - } - } + } + } + } } void MavLinkFtpClientImpl::MavLinkFtpClientImpl::retry() { - retries_++; - if (retries_ < 10) - { + retries_++; + if (retries_ < 10) + { Utils::log(Utils::stringf("retry %d\n", retries_), Utils::kLogLevelWarn); - nextStep(); - } - else - { - // give up then. - errorCode_ = kErrRetriesExhausted; - success_ = false; - waiting_ = false; - reset(); - } + nextStep(); + } + else + { + // give up then. + errorCode_ = kErrRetriesExhausted; + success_ = false; + waiting_ = false; + reset(); + } } void MavLinkFtpClientImpl::recordMessageSent() { - // tell watchdog we are sending a request - std::lock_guard guard(mutex_); - start_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + // tell watchdog we are sending a request + std::lock_guard guard(mutex_); + start_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); } void MavLinkFtpClientImpl::recordMessageReceived() { - std::lock_guard guard(mutex_); - messages_++; - milliseconds endTime = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - milliseconds duration = endTime - start_time_; - total_time_ += duration; - if (progress_ != nullptr && duration.count() > progress_->longest_delay) - { - progress_->longest_delay = static_cast(duration.count()); - } + std::lock_guard guard(mutex_); + messages_++; + milliseconds endTime = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); + milliseconds duration = endTime - start_time_; + total_time_ += duration; + if (progress_ != nullptr && duration.count() > progress_->longest_delay) + { + progress_->longest_delay = static_cast(duration.count()); + } } std::string MavLinkFtpClientImpl::replaceAll(std::string s, char toFind, char toReplace) { - size_t pos = s.find_first_of(toFind, 0); - while (pos != std::string::npos) { - s.replace(pos, 1, 1, toReplace); - pos = s.find_first_of(toFind, 0); - } - return s; + size_t pos = s.find_first_of(toFind, 0); + while (pos != std::string::npos) { + s.replace(pos, 1, 1, toReplace); + pos = s.find_first_of(toFind, 0); + } + return s; } std::string MavLinkFtpClientImpl::normalize(std::string arg) { - if (FileSystem::kPathSeparator == '\\') { - return replaceAll(arg, '/', '\\'); // make sure user input matches what FileSystem will do when resolving paths. - } - return arg; + if (FileSystem::kPathSeparator == '\\') { + return replaceAll(arg, '/', '\\'); // make sure user input matches what FileSystem will do when resolving paths. + } + return arg; } std::string MavLinkFtpClientImpl::toPX4Path(std::string arg) { - if (FileSystem::kPathSeparator == '\\') { - return replaceAll(arg, '\\', '/'); // PX4 uses '/' - } - return arg; + if (FileSystem::kPathSeparator == '\\') { + return replaceAll(arg, '\\', '/'); // PX4 uses '/' + } + return arg; } diff --git a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp index 07566bf4..59ca5e21 100644 --- a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp @@ -273,6 +273,11 @@ bool MultiRotorConnector::isApiServerStarted() //*** Start: UpdatableState implementation ***// void MultiRotorConnector::reset() { + VehicleConnectorBase::reset(); + + //TODO: should this be done in MultiRotor.hpp + //controller_->reset(); + rc_data_ = RCData(); vehicle_pawn_->reset(); //we do flier resetPose so that flier is placed back without collisons vehicle_.reset(); @@ -280,6 +285,8 @@ void MultiRotorConnector::reset() void MultiRotorConnector::update() { + VehicleConnectorBase::update(); + //this is high frequency physics tick, flier gets ticked at rendering frame rate vehicle_.update(); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 4878a657..1ce842ae 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -1,6 +1,5 @@ #include "SimModeWorldBase.h" - const char ASimModeWorldBase::kUsageScenarioComputerVision[] = "ComputerVision"; void ASimModeWorldBase::BeginPlay() @@ -51,7 +50,6 @@ void ASimModeWorldBase::createWorld() new msr::airlib::FastPhysicsEngine() ); } - } else { physics_engine_.release(); @@ -59,13 +57,15 @@ void ASimModeWorldBase::createWorld() } world_.initialize(physics_engine_.get()); - reporter_.initialize(false); - //add default objects to world + reporter_.initialize(false); world_.insert(&reporter_); + for(size_t vi = 0; vi < vehicles_.size(); vi++) world_.insert(vehicles_.at(vi).get()); + world_.reset(); + for (auto& vehicle : vehicles_) vehicle->beginPlay(); } diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnBase.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnBase.cpp index 8c9899fb..98c78d15 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnBase.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnBase.cpp @@ -122,12 +122,12 @@ void AVehiclePawnBase::initialize() initial_state_.collisons_enabled = EnableCollisons; initial_state_.passthrough_enabled = EnablePassthroughOnCollisons; - initial_state_.collison_info = CollisionInfo(); initial_state_.was_last_move_teleport = false; initial_state_.was_last_move_teleport = canTeleportWhileMove(); + //call derived class reset which in turn should call base class reset reset(); } diff --git a/clean.cmd b/clean.cmd index 8c46a611..0a40ceb6 100644 --- a/clean.cmd +++ b/clean.cmd @@ -1,3 +1,6 @@ +rd /s/q AirLib\lib +rd /s/q AirLib\deps\MavLinkCom +rd /s/q AirLib\deps\rpclib rd /s/q external\rpclib\build msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln /t:Clean if ERRORLEVEL 1 goto :buildfailed